fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 16:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Introduced new actuator_safety topic

Browse Source
This commit is contained in:
Julian Oes
2013-06-14 13:53:26 +02:00
parent 236053a600
commit 90f5e30f2a
22 changed files with 424 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/blinkm/blinkm.cpp
+20 -1
View File
@@ -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) {
@@ -530,7 +549,7 @@ BlinkM::led()
} else {
/* no battery warnings here */
if(vehicle_status_raw.flag_fmu_armed == false) {
if(actuator_safety.armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;