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

Better, platform independent failsafe handling

Browse Source
This commit is contained in:
Lorenz Meier 2012-11-09 16:30:00 +01:00
parent a866fb2f2c
commit 0baca3ee31
2 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
apps/commander/state_machine_helper.c
View File

@ -220,11 +220,8 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
void publish_armed_status(const struct vehicle_status_s *current_status) {
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* lock down actuators if required */
// XXX FIXME Currently any loss of RC will completely disable all actuators
// needs proper failsafe
armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost)
|| current_status->flag_hil_enabled) ? true : false;
/* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}

20
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -184,7 +184,13 @@ mc_thread_main(int argc, char *argv[])
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
if (state.offboard_control_signal_lost) {
/* give up in rate mode on signal loss */
rates_sp.thrust = 0.0f;
} else {
/* no signal loss, normal throttle */
rates_sp.thrust = offboard_sp.p4;
}
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
@ -192,7 +198,17 @@ mc_thread_main(int argc, char *argv[])
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
/* check if control signal is lost */
if (state.offboard_control_signal_lost) {
/* set failsafe thrust */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.thrust = (failsafe_throttle < 0.6f) : failsafe_throttle ? 0.5f;
} else {
/* no signal loss, normal throttle */
att_sp.thrust = offboard_sp.p4;
}
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */