diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a64d99cd49..1149f735f3 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 3b05bfa25a..2623673705 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -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 */