diff options
4 files changed, 9 insertions, 23 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 7db920f91..4e2166a3a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -43,6 +43,7 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> #include <systemlib/systemlib.h> #include <arch/board/up_hrt.h> #include <mavlink/mavlink_log.h> @@ -198,6 +199,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); + struct actuator_armed_s armed; + armed.armed = current_status->flag_system_armed; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); ret = OK; } if (invalid_state) { diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 9fbe4c3ca..4f5082bed 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -117,7 +117,6 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); armed.armed = false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); /* register the perf counter */ @@ -151,25 +150,13 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; } else { - if (state.flag_system_armed) { - att_sp.thrust = manual.throttle; - armed.armed = true; - - } else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) { - /* immediately cut off motors */ - armed.armed = false; - - } else { - /* limit motor throttle to zero for an unknown mode */ - armed.armed = false; - } + att_sp.thrust = manual.throttle; } - multirotor_control_attitude(&att_sp, &att, &state, &actuators, motor_test_mode); + multirotor_control_attitude(&att_sp, &att, &actuators); /* publish the result */ - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -179,8 +166,6 @@ mc_thread_main(int argc, char *argv[]) printf("[multirotor att control] stopping, disarming motors.\n"); /* kill all outputs */ - armed.armed = false; - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -190,7 +175,6 @@ mc_thread_main(int argc, char *argv[]) close(state_sub); close(manual_sub); close(actuator_pub); - close(armed_pub); close(att_sp_pub); perf_print_counter(mc_loop_perf); diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2fb9ae544..4afa4ed4a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -188,8 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, - struct actuator_controls_s *actuators, bool verbose) + const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index c3caa7598..1fc00d8c1 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -54,8 +54,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose); - -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose); + struct actuator_controls_s *actuators); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ |