fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 00:53: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/px4fmu/fmu.cpp
+14 -13
View File
@@ -69,6 +69,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 <systemlib/err.h>
#include <systemlib/ppm_decode.h>
@@ -104,7 +105,7 @@ private:
unsigned _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
int _t_actuator_safety;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
@@ -174,7 +175,7 @@ PX4FMU::PX4FMU() :
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
_t_actuator_safety(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
@@ -376,8 +377,8 @@ PX4FMU::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
_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 */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -396,7 +397,7 @@ PX4FMU::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].fd = _t_actuator_safety;
fds[1].events = POLLIN;
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
@@ -499,13 +500,13 @@ PX4FMU::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
actuator_safety_s as;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
/* update PWM servo armed status if armed and not locked down */
bool set_armed = aa.armed && !aa.lockdown;
bool set_armed = as.armed && !as.lockdown;
if (set_armed != _armed) {
_armed = set_armed;
up_pwm_servo_arm(set_armed);
@@ -535,7 +536,7 @@ PX4FMU::task_main()
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_armed);
::close(_t_actuator_safety);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -1021,12 +1022,12 @@ fake(int argc, char *argv[])
if (handle < 0)
errx(1, "advertise failed");
actuator_armed_s aa;
actuator_safety_s as;
aa.armed = true;
aa.lockdown = false;
as.armed = true;
as.lockdown = false;
handle = orb_advertise(ORB_ID(actuator_armed), &aa);
handle = orb_advertise(ORB_ID(actuator_safety), &as);
if (handle < 0)
errx(1, "advertise failed 2");