Merge pull request #13 from husarion/ranges_names

Nan value when ranges are overranged
husarion · Mar 21, 2023 · 0ed8ae8 · 0ed8ae8
2 parents 5fb161d + 99ad6b6
commit 0ed8ae8
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 14 deletions.
diff --git a/include/main.hpp b/include/main.hpp
@@ -23,8 +23,9 @@ static UARTSerial microros_serial(RPI_SERIAL_TX, RPI_SERIAL_RX);
 
 static volatile bool distance_sensors_enabled = false;
 
-static volatile bool button1_publish_flag = false;
-static volatile bool button2_publish_flag = false;
+static volatile bool buttons_publish_flag[BUTTONS_COUNT] = {false, false};
+static volatile float button_release_time = 0.1;
+
 static volatile float battery_voltage = 0.0;
 
 static volatile bool is_speed_watchdog_enabled = true;
@@ -53,6 +54,7 @@ std::map<double, uint8_t> servo_voltage_configuration{
 #define MOTOR_FL MOTOR4
 #define MOTOR_RR MOTOR2
 #define MOTOR_RL MOTOR3
+time_t last_button_press_time[BUTTONS_COUNT];
 
 constexpr uint8_t POLARITY = 0b00111100;
 constexpr float ROBOT_LENGTH = 0.197;

diff --git a/src/main.cpp b/src/main.cpp
@@ -36,11 +36,11 @@ std_msgs__msg__Bool button_msgs[BUTTONS_COUNT];
 static uint32_t spin_count = 1;
 
 static void button1Callback() {
-    button1_publish_flag = true;
+    buttons_publish_flag[0] = true;
 }
 
 static void button2Callback() {
-    button2_publish_flag = true;
+    buttons_publish_flag[1] = true;
 }
 
 void range_sensors_msg_handler() {
@@ -111,16 +111,17 @@ void battery_msg_handler() {
 }
 
 void button_msgs_handler() {
-    if (button1_publish_flag) {
-        button_msgs[0].data = 1;
-        button1_publish_flag = false;
-        publish_button_msg(&button_msgs[0], 0);
-    }
-
-    if (button2_publish_flag) {
-        button_msgs[1].data = 1;
-        button2_publish_flag = false;
-        publish_button_msg(&button_msgs[1], 1);
+    for(auto i = 0u; i < BUTTONS_COUNT; ++i){
+        if (buttons_publish_flag[i]) {
+            button_msgs[i].data = true;
+            buttons_publish_flag[i] = false;
+            publish_button_msg(&button_msgs[i], i);
+            last_button_press_time[i] = time(NULL);
+        }
+        else if(time(NULL) - last_button_press_time[i]  > button_release_time){
+            button_msgs[i].data = false;
+            publish_button_msg(&button_msgs[i], i);
+        }
     }
 }
 

diff --git a/src/microros.cpp b/src/microros.cpp
@@ -328,4 +328,7 @@ void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id) {
     msg->field_of_view = 0.26;
     msg->min_range = 0.01;
     msg->max_range = 0.90;
+    if(msg->range > msg->max_range || msg->range < msg->min_range){
+        msg->range = NAN;
+    }
 }