diff options
Diffstat (limited to 'src/drivers/blinkm/blinkm.cpp')
-rw-r--r-- | src/drivers/blinkm/blinkm.cpp | 68 |
1 files changed, 44 insertions, 24 deletions
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a5..a11f88477 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,7 @@ #include <poll.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/vehicle_gps_position.h> static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,6 +377,7 @@ BlinkM::led() static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +388,7 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_actuator_safety = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +401,9 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +458,14 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct actuator_safety_s actuator_safety; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_actuator_safety; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +479,17 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + + if (new_data_actuator_safety) { + orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); + no_data_actuator_safety = 0; + } else { + no_data_actuator_safety++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -486,15 +505,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { - if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { + + for (unsigned satloop = 0; satloop < (sizeof(vehicle_gps_position_raw.satellite_used) / sizeof(vehicle_gps_position_raw.satellite_used[0])); satloop++) { + if (vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats++; } } - if(new_data_vehicle_status || no_data_vehicle_status < 3){ - if(num_of_cells == 0) { + if (new_data_vehicle_status || no_data_vehicle_status < 3) { + if (num_of_cells == 0) { /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { @@ -530,7 +549,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(actuator_safety.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -555,26 +574,27 @@ BlinkM::led() led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - led_color_4 = LED_GREEN; - break; - } +#warning fix this +// switch((int)vehicle_status_raw.flight_mode) { +// case VEHICLE_FLIGHT_MODE_MANUAL: +// led_color_4 = LED_AMBER; +// break; +// +// case VEHICLE_FLIGHT_MODE_STAB: +// led_color_4 = LED_YELLOW; +// break; +// +// case VEHICLE_FLIGHT_MODE_HOLD: +// led_color_4 = LED_BLUE; +// break; +// +// case VEHICLE_FLIGHT_MODE_AUTO: +// led_color_4 = LED_GREEN; +// break; +// } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat´s */ + /* handling used sat�s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; |