From cae070c73e661f3242ec816cfae28bbeb37897da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Sep 2012 11:33:52 +0200 Subject: [PATCH] Changed to publishing armed state in commander --- apps/commander/state_machine_helper.c | 5 +++++ .../multirotor_att_control_main.c | 20 ++----------------- .../multirotor_attitude_control.c | 3 +-- .../multirotor_attitude_control.h | 4 +--- 4 files changed, 9 insertions(+), 23 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 7db920f915..4e2166a3a7 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -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 9fbe4c3ca4..4f5082bedd 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 2fb9ae5442..4afa4ed4a0 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 c3caa75981..1fc00d8c11 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -54,8 +54,6 @@ #include 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_ */