Skip to content

FIxed ESP32 restart for agent issue #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
marko161205 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

FIxed ESP32 restart for agent issue #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file added .vscode/browse.vc.db
View file
Open in desktop
Binary file not shown.
15 changes: 14 additions & 1 deletion .vscode/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,16 @@
{
"cmake.sourceDirectory": "C:/Users/pavan/Downloads/sauvc25/working_code_final1 - Copy/lib/Fusion"
"cmake.sourceDirectory": "/home/omkar/auv/new/microros-code/lib/Fusion",
"ROS2.distro": "humble",
"python.autoComplete.extraPaths": [
"/home/omkar/ros2_ws/build/my_robot_controller",
"/home/omkar/ros2_ws/install/my_robot_controller/lib/python3.10/site-packages",
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
],
"python.analysis.extraPaths": [
"/home/omkar/ros2_ws/build/my_robot_controller",
"/home/omkar/ros2_ws/install/my_robot_controller/lib/python3.10/site-packages",
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
]
}
47 changes: 42 additions & 5 deletions src/communication_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,19 @@ rcl_subscription_t pwm_sub;
rcl_subscription_t calibration_sub;
rcl_subscription_t led_sub;

static int connFailedCount = 0;

void initializeCommunication() {


set_microros_serial_transports(Serial);

while (RMW_RET_OK != rmw_uros_ping_agent(100,1)) //ROS2 agent and micro ros handshake
{
delay(10);
}


rcl_allocator_t allocator = rcl_get_default_allocator();
rclc_support_t support;
rcl_node_t node;
Expand Down Expand Up @@ -127,8 +138,8 @@ void throttleCb(const void * msgin) {
(const std_msgs__msg__Int32MultiArray *)msgin;

int32_t pwm_values[NUMBER_OF_THRUSTERS];
for (size_t i = 0; i < pwm_msg->data.size && i < NUMBER_OF_THRUSTERS; i++) {
int32_t val = pwm_msg->data.data[i];
for (size_t i = 0; i < NUMBER_OF_THRUSTERS; i++) {
pwm_values[i] = pwm_msg->data.data[i];
}
setThrusterThrottle(pwm_values);
}
Expand All @@ -137,15 +148,15 @@ void calibrationCb(const void * msgin) {
const std_msgs__msg__Bool * calib =
(const std_msgs__msg__Bool *)msgin;

bool calibration_mode = calibration_msg.data;
bool calibration_mode = calib->data;
callUpdateOffset(calibration_mode);
}

void ledCb(const void * msgin) {
const std_msgs__msg__Int16 * led =
(const std_msgs__msg__Int16 *)msgin;

int16_t led_indicator = led_msg.data;
int16_t led_indicator = led->data;
setLED(led_indicator);

}
Expand All @@ -157,5 +168,31 @@ void ledCb(const void * msgin) {
// }

void checkForCommands() {
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
// if (RMW_RET_OK == rmw_uros_ping_agent(100,1))
// {
// rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
// if (connFailedCount>50)
// {
// initializeCommunication();
// }
// connFailedCount = 0;
// }
// else
// {
// connFailedCount++;
// if (connFailedCount>50)
// {
// int32_t pwm_values[NUMBER_OF_THRUSTERS];
// for (size_t i = 0; i < NUMBER_OF_THRUSTERS; i++) {
// pwm_values[i] = 1500;
// }
// setThrusterThrottle(pwm_values);
// Serial.println("Connection to micro-ROS agent lost!");
// connFailedCount = 51; //to avoid overflow
// }

// }
// delay(10);
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));

}
3 changes: 2 additions & 1 deletion src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,15 @@ MS5837 depth_sensor;
long int previous_update_rate = 0, previous_publish_rate = 0;

void setup() {

Serial.begin(115200);
while (!Serial) {
delay(1);
}
Wire.begin(21, 22);

initializeCommunication();

initializeIMU();
initializeDepthSensor();
initializeSensorMath();
Expand All @@ -36,7 +38,6 @@ void setup() {

void loop() {
if ((micros() - previous_update_rate) >= UPDATE_RATE)
;
{
previous_update_rate = micros();
updateIMUReadings(ax, ay, az, gx, gy, gz, mx, my, mz);
Expand Down