zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: «home position set» condition fixed, failsafe fixes, navigator: state indication bugfix, control_mode fixes

sbg
Anton Babushkin 11 years ago
parent
b7c69262a7
commit
c841929e3f
2 changed files with 133 additions and 127 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 226
      src/modules/commander/commander.cpp
  2. 34
      src/modules/navigator/navigator_main.cpp

226
src/modules/commander/commander.cpp
Unescape View File

@ -565,7 +565,6 @@ int commander_thread_main(int argc, char *argv[]) @@ -565,7 +565,6 @@ int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
commander_initialized = false;
bool home_position_set = false;
bool battery_tune_played = false;
bool arm_tune_played = false;
@ -597,7 +596,8 @@ int commander_thread_main(int argc, char *argv[]) @@ -597,7 +596,8 @@ int commander_thread_main(int argc, char *argv[])
char *failsafe_states_str[FAILSAFE_STATE_MAX];
failsafe_states_str[0] = "NORMAL";
failsafe_states_str[1] = "RTL";
failsafe_states_str[2] = "TERMINATION";
failsafe_states_str[2] = "LAND";
failsafe_states_str[3] = "TERMINATION";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@ -628,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -628,6 +628,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.failsafe_state = FAILSAFE_STATE_NORMAL;
/* neither manual nor offboard control commands have been received */
status.offboard_control_signal_found_once = false;
@ -1027,13 +1028,12 @@ int commander_thread_main(int argc, char *argv[]) @@ -1027,13 +1028,12 @@ int commander_thread_main(int argc, char *argv[])
* position to the current position.
*/
if (!home_position_set && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
/* copy position data to uORB home message, store it locally as well */
/* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.alt;
@ -1050,164 +1050,160 @@ int commander_thread_main(int argc, char *argv[]) @@ -1050,164 +1050,160 @@ int commander_thread_main(int argc, char *argv[])
}
/* mark home position as set */
home_position_set = true;
status.condition_home_position_valid = true;
tune_positive();
}
}
/* ignore RC signals if in offboard control mode */
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* start RC input check */
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
status_changed = true;
/* start RC input check */
if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
}
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
}
}
status.rc_signal_lost = false;
transition_result_t res; // store all transitions results here
status.rc_signal_lost = false;
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
transition_result_t res; // store all transitions results here
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
} else {
stick_off_counter++;
}
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
stick_off_counter = 0;
stick_off_counter++;
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
stick_off_counter = 0;
}
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
stick_on_counter = 0;
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
stick_on_counter++;
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
} else {
stick_on_counter = 0;
} else {
stick_on_counter++;
}
if (res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
} else {
stick_on_counter = 0;
}
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
if (res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
}
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe");
}
}
/* fill current_status according to mode switches */
check_mode_switches(&sp_man, &status);
/* fill current_status according to mode switches */
check_mode_switches(&sp_man, &status);
/* evaluate the main state machine according to mode switches */
res = check_main_state_machine(&status);
/* evaluate the main state machine according to mode switches */
res = check_main_state_machine(&status);
if (res == TRANSITION_CHANGED) {
tune_positive();
if (res == TRANSITION_CHANGED) {
tune_positive();
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_DENIED) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_DENIED) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
} else if (res == TRANSITION_DENIED) {
/* LAND mode denied, switch to failsafe state TERMINATION */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
} else if (res == TRANSITION_DENIED) {
/* LAND mode denied, switch to failsafe state TERMINATION */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
}
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
}
}
}
} else if (armed.armed) {
/* failsafe for manual modes */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
} else if (armed.armed) {
/* failsafe for manual modes */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
} else if (res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate), try LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
} else if (res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate), try LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
} else if (res == TRANSITION_DENIED) {
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
}
mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
}
}
}

34
src/modules/navigator/navigator_main.cpp
Unescape View File

@ -411,11 +411,13 @@ Navigator::Navigator() : @@ -411,11 +411,13 @@ Navigator::Navigator() :
memset(&_mission_result, 0, sizeof(struct mission_result_s));
memset(&_mission_item, 0, sizeof(struct mission_item_s));
memset(&nav_states_str, 0, sizeof(nav_states_str));
nav_states_str[0] = "NONE";
nav_states_str[1] = "READY";
nav_states_str[2] = "LOITER";
nav_states_str[3] = "MISSION";
nav_states_str[4] = "RTL";
nav_states_str[5] = "LAND";
/* Initialize state machine */
myState = NAV_STATE_NONE;
@ -750,6 +752,7 @@ Navigator::task_main() @@ -750,6 +752,7 @@ Navigator::task_main()
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
/* RTL on failsafe */
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -981,7 +984,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { @@ -981,7 +984,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
},
@ -1220,7 +1223,7 @@ Navigator::start_land() @@ -1220,7 +1223,7 @@ Navigator::start_land()
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.yaw = NAN;
mavlink_log_info(_mavlink_fd, "[navigator] LAND started");
mavlink_log_info(_mavlink_fd, "[navigator] land started");
}
void
@ -1616,14 +1619,7 @@ Navigator::publish_control_mode() @@ -1616,14 +1619,7 @@ Navigator::publish_control_mode()
break;
case FAILSAFE_STATE_LAND:
/* land with or without position control */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
navigator_enabled = true;
break;
case FAILSAFE_STATE_TERMINATION:
@ -1647,7 +1643,8 @@ Navigator::publish_control_mode() @@ -1647,7 +1643,8 @@ Navigator::publish_control_mode()
if (navigator_enabled) {
_control_mode.flag_control_manual_enabled = false;
if (myState == NAV_STATE_READY) {
switch (myState) {
case NAV_STATE_READY:
/* disable all controllers, armed but idle */
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
@ -1655,14 +1652,27 @@ Navigator::publish_control_mode() @@ -1655,14 +1652,27 @@ Navigator::publish_control_mode()
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
break;
} else {
case NAV_STATE_LAND:
/* land with or without position control */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
default:
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
}
}

Write Preview
Loading…
Cancel
Save