fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 03: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/px4io/px4io.cpp
+21 -20
View File
@@ -75,6 +75,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
@@ -152,7 +153,7 @@ private:
/* subscribed topics */
int _t_actuators; ///< actuator controls topic
int _t_armed; ///< system armed control topic
int _t_actuator_safety; ///< system armed control topic
int _t_vstatus; ///< system / vehicle status
int _t_param; ///< parameter update topic
@@ -327,7 +328,7 @@ PX4IO::PX4IO() :
_status(0),
_alarms(0),
_t_actuators(-1),
_t_armed(-1),
_t_actuator_safety(-1),
_t_vstatus(-1),
_t_param(-1),
_to_input_rc(0),
@@ -433,20 +434,20 @@ PX4IO::init()
* remains untouched (so manual override is still available).
*/
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
/* fill with initial values, clear updated flag */
vehicle_status_s status;
struct actuator_safety_s armed;
uint64_t try_start_time = hrt_absolute_time();
bool updated = false;
/* keep checking for an update, ensure we got a recent state,
/* keep checking for an update, ensure we got a arming information,
not something that was published a long time ago. */
do {
orb_check(vstatus_sub, &updated);
orb_check(safety_sub, &updated);
if (updated) {
/* got data, copy and exit loop */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
break;
}
@@ -472,10 +473,10 @@ PX4IO::init()
cmd.param6 = 0;
cmd.param7 = 0;
cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
cmd.target_system = status.system_id;
cmd.target_component = status.component_id;
cmd.source_system = status.system_id;
cmd.source_component = status.component_id;
// cmd.target_system = status.system_id;
// cmd.target_component = status.component_id;
// cmd.source_system = status.system_id;
// cmd.source_component = status.component_id;
/* ask to confirm command */
cmd.confirmation = 1;
@@ -484,10 +485,10 @@ PX4IO::init()
/* spin here until IO's state has propagated into the system */
do {
orb_check(vstatus_sub, &updated);
orb_check(safety_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
}
/* wait 10 ms */
@@ -500,7 +501,7 @@ PX4IO::init()
}
/* keep waiting for state change for 10 s */
} while (!status.flag_fmu_armed);
} while (!armed.armed);
/* regular boot, no in-air restart, init IO */
} else {
@@ -564,8 +565,8 @@ PX4IO::task_main()
ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
@@ -577,7 +578,7 @@ PX4IO::task_main()
pollfd fds[4];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].fd = _t_actuator_safety;
fds[1].events = POLLIN;
fds[2].fd = _t_vstatus;
fds[2].events = POLLIN;
@@ -713,16 +714,16 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
int
PX4IO::io_set_arming_state()
{
actuator_armed_s armed; ///< system armed state
actuator_safety_s safety; ///< system armed state
vehicle_status_s vstatus; ///< overall system state
orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed && !armed.lockdown) {
if (safety.armed && !safety.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;