fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 07:33:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ensure RC switches get evaluated on first run

Browse Source
This commit is contained in:
Lorenz Meier
2016-03-06 14:56:25 +01:00
parent b338ba439c
commit 8a91ce65e8
1 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+7 -4
View File
@@ -205,9 +205,9 @@ static struct home_position_s _home;
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX - 1]; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX - 1];
static unsigned _last_mission_instance = 0; static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man; static manual_control_setpoint_s _last_sp_man = {};
static struct vtol_vehicle_status_s vtol_status; static struct vtol_vehicle_status_s vtol_status = {};
/** /**
* The daemon app only briefly exists to start * The daemon app only briefly exists to start
@@ -2351,10 +2351,11 @@ int commander_thread_main(int argc, char *argv[])
} }
/* evaluate the main state machine according to mode switches */ /* 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, &sp_man);
/* play tune on mode change only if armed, blink LED always */ /* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED) { if (main_res == TRANSITION_CHANGED || first_rc_eval) {
tune_positive(armed.armed); tune_positive(armed.armed);
main_state_changed = true; main_state_changed = true;
@@ -2849,13 +2850,15 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* set main state according to RC switches */ /* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED; transition_result_t res = TRANSITION_DENIED;
// XXX this should not be necessary any more, we should be able to
// just delete this and respond to mode switches
/* if offboard is set already by a mavlink command, abort */ /* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) { if (status.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
} }
/* manual setpoint has not updated, do not re-evaluate it */ /* manual setpoint has not updated, do not re-evaluate it */
if ((_last_sp_man.timestamp == sp_man->timestamp) || 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.offboard_switch == sp_man->offboard_switch) &&
(_last_sp_man.return_switch == sp_man->return_switch) && (_last_sp_man.return_switch == sp_man->return_switch) &&
(_last_sp_man.mode_switch == sp_man->mode_switch) && (_last_sp_man.mode_switch == sp_man->mode_switch) &&