Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Calibration state machine fixes, generates less bogus error messages during calibration

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-21 17:25:37 +02:00
parent fb4dc27bc9
commit 9179fcefc9
3 changed files with 22 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_status.msg
+1
View File
@@ -91,6 +91,7 @@ uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state
uint8 hil_state # current hil state
bool failsafe # true if system is in failsafe state
bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT.
int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum
int32 system_id # system id, inspired by MAVLink's system ID field
src/modules/commander/commander.cpp
+7 -3
View File
@@ -1604,7 +1604,7 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -2689,6 +2689,8 @@ void *commander_low_prio_loop(void *arg)
false /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
} else {
status.calibration_enabled = true;
}
if ((int)(cmd.param1) == 1) {
@@ -2748,12 +2750,14 @@ void *commander_low_prio_loop(void *arg)
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
calib_ret = OK;
calib_ret = OK;
}
/* this always succeeds */
calib_ret = OK;
}
status.calibration_enabled = false;
if (calib_ret == OK) {
tune_positive(true);
@@ -2771,7 +2775,7 @@ void *commander_low_prio_loop(void *arg)
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
} else {
tune_negative(true);
src/modules/commander/state_machine_helper.cpp
+14 -12
View File
@@ -216,17 +216,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
valid_transition = true;
}
// Sensors need to be initialized for STANDBY state, except for HIL
if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
@@ -244,8 +233,21 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete");
feedback_provided = true;
} else {
// Silent ignore
feedback_provided = true;
}
// Sensors need to be initialized for STANDBY state, except for HIL
} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
if (!fRunPreArmChecks) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
}
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
// Finish up the state transition