From ffb0d37c8a56c88e83e257b770053fe24946fc48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 9 May 2016 23:01:06 +0200 Subject: [PATCH] Commander: Fix reposition handling, run faster to allow catching of consecutive commands --- src/modules/commander/commander.cpp | 78 ++++++++++++++++------------- 1 file changed, 43 insertions(+), 35 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c0c42077db..f451b44d4e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -138,7 +138,7 @@ extern struct system_load_s system_load; static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ /* Decouple update interval and hysteris counters, all depends on intervals */ -#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_INTERVAL 10000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) #define MAVLINK_OPEN_INTERVAL 50000 @@ -210,7 +210,8 @@ static struct commander_state_s internal_state = {}; static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX; static unsigned _last_mission_instance = 0; -static manual_control_setpoint_s _last_sp_man = {}; +struct manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint +static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch static struct vtol_vehicle_status_s vtol_status = {}; @@ -262,7 +263,7 @@ void get_circuit_breaker_params(); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void set_control_mode(); @@ -677,7 +678,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // the data at the exact same time. // Check if a mode switch had been requested - if ((((uint8_t)cmd->param1) & 1) > 0) { + if ((((uint32_t)cmd->param2) & 1) > 0) { transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); if ((main_ret != TRANSITION_DENIED)) { @@ -837,12 +838,20 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s else { // Refuse to arm if preflight checks have failed - if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) { + if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) { mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed."); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; break; } + // Refuse to arm if in manual with non-zero throttle + if ((status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || + status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO) && sp_man.z > 0.1f) { + mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Manual throttle non-zero."); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + break; + } + } transition_result_t arming_res = arm_disarm(cmd_arms,&mavlink_log_pub, "arm/disarm component command"); @@ -1384,7 +1393,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); /* Subscribe to offboard control data */ @@ -2446,7 +2454,7 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine according to mode switches */ bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0); - transition_result_t main_res = set_main_state_rc(&status, &sp_man); + transition_result_t main_res = set_main_state_rc(&status); /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED || first_rc_eval) { @@ -2958,7 +2966,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; @@ -2971,34 +2979,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* manual setpoint has not updated, do not re-evaluate it */ - if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man->timestamp)) || - ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && - (_last_sp_man.return_switch == sp_man->return_switch) && - (_last_sp_man.mode_switch == sp_man->mode_switch) && - (_last_sp_man.acro_switch == sp_man->acro_switch) && - (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) && - (_last_sp_man.posctl_switch == sp_man->posctl_switch) && - (_last_sp_man.loiter_switch == sp_man->loiter_switch) && - (_last_sp_man.mode_slot == sp_man->mode_slot))) { + if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || + ((_last_sp_man.offboard_switch == sp_man.offboard_switch) && + (_last_sp_man.return_switch == sp_man.return_switch) && + (_last_sp_man.mode_switch == sp_man.mode_switch) && + (_last_sp_man.acro_switch == sp_man.acro_switch) && + (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) && + (_last_sp_man.posctl_switch == sp_man.posctl_switch) && + (_last_sp_man.loiter_switch == sp_man.loiter_switch) && + (_last_sp_man.mode_slot == sp_man.mode_slot))) { // update these fields for the geofence system if (!rtl_on) { - _last_sp_man.timestamp = sp_man->timestamp; - _last_sp_man.x = sp_man->x; - _last_sp_man.y = sp_man->y; - _last_sp_man.z = sp_man->z; - _last_sp_man.r = sp_man->r; + _last_sp_man.timestamp = sp_man.timestamp; + _last_sp_man.x = sp_man.x; + _last_sp_man.y = sp_man.y; + _last_sp_man.z = sp_man.z; + _last_sp_man.r = sp_man.r; } /* no timestamp change or no switch change -> nothing changed */ return TRANSITION_NOT_CHANGED; } - _last_sp_man = *sp_man; + _last_sp_man = sp_man; /* offboard switch overrides main switch */ - if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); if (res == TRANSITION_DENIED) { @@ -3012,7 +3020,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* RTL switch overrides main switch */ - if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { warnx("RTL switch changed and ON!"); res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state); @@ -3032,14 +3040,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* we know something has changed - check if we are in mode slot operation */ - if (sp_man->mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) { + if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) { - if (sp_man->mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) { - warnx("overflow"); + if (sp_man.mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) { + warnx("m slot overflow"); return TRANSITION_DENIED; } - int new_mode = _flight_mode_slots[sp_man->mode_slot]; + int new_mode = _flight_mode_slots[sp_man.mode_slot]; if (new_mode < 0) { /* slot is unused */ @@ -3171,13 +3179,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* offboard and RTL switches off or denied, check main mode switch */ - switch (sp_man->mode_switch) { + switch (sp_man.mode_switch) { case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; break; case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL - if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* manual mode is stabilized already for multirotors, so switch to acro * for any non-manual mode @@ -3192,7 +3200,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } } - else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ + else if(sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ /* Similar to acro transitions for multirotors. FW aircraft don't need a * rattitude mode.*/ if (status.is_rotary_wing) { @@ -3208,7 +3216,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) { @@ -3225,7 +3233,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this mode } - if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { print_reject_mode(status_local, "ALTITUDE CONTROL"); } @@ -3235,7 +3243,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) {