fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 02:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail

Browse Source
This commit is contained in:
Lorenz Meier
2014-05-25 08:22:54 +02:00
parent 36495cdb62
commit c7bf00562d
3 changed files with 51 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+27
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -717,6 +718,9 @@ int commander_thread_main(int argc, char *argv[])
status.counter++;
status.timestamp = hrt_absolute_time();
status.condition_power_input_valid = true;
status.avionics_power_rail_voltage = -1.0f;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -853,6 +857,11 @@ int commander_thread_main(int argc, char *argv[])
struct position_setpoint_triplet_s pos_sp_triplet;
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
/* Subscribe to system power */
int system_power_sub = orb_subscribe(ORB_ID(system_power));
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -933,6 +942,24 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
}
orb_check(system_power_sub, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
if (system_power.servo_valid &&
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
status.condition_power_input_valid = false;
} else {
status.condition_power_input_valid = true;
status.avionics_power_rail_voltage = system_power.voltage5V_v;
}
}
}
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
/* update safety topic */