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

commander: reject PREFLIGHT_CALIBRATION and PREFLIGHT_STORAGE if armed or shutting down

sbg
Daniel Agar 5 years ago
parent
04113b4d57
commit
6705ac3e3b
1 changed files with 144 additions and 126 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 270
      src/modules/commander/Commander.cpp

270
src/modules/commander/Commander.cpp
Unescape View File

@ -3353,111 +3353,120 @@ void *commander_low_prio_loop(void *arg)
switch (cmd.command) { switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
int calib_ret = PX4_ERROR; if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) {
/* try to go to INIT/PREFLIGHT arming state */ // reject if armed or shutting down
if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed, answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
30_s) // time since boot not relevant for switching to ARMING_STATE_INIT
) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
break;
} else { } else {
status_flags.condition_calibration_enabled = true;
}
if ((int)(cmd.param1) == 1) { int calib_ret = PX4_ERROR;
/* gyro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || /* try to go to INIT/PREFLIGHT arming state */
(int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed,
(int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
/* temperature calibration: handled in events module */ PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
break; 30_s) // time since boot not relevant for switching to ARMING_STATE_INIT
) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
break;
} else if ((int)(cmd.param2) == 1) { } else {
/* magnetometer calibration */ status_flags.condition_calibration_enabled = true;
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_mag_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* disable RC control input completely */
status_flags.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_trim_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_accel_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_level_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status_flags.rc_input_blocked) {
/* enable RC control input */
status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input");
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); if ((int)(cmd.param1) == 1) {
/* this always succeeds */ /* gyro calibration */
calib_ret = OK; answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else { } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
} (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
/* temperature calibration: handled in events module */
break;
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_mag_calibration(&mavlink_log_pub);
status_flags.condition_calibration_enabled = false; } else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
if (calib_ret == OK) { } else if ((int)(cmd.param4) == 1) {
tune_positive(true); /* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* disable RC control input completely */
status_flags.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_trim_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_accel_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_level_calibration(&mavlink_log_pub);
// time since boot not relevant here } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
if (PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, false, false, true, 30_s)) { // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status_flags.rc_input_blocked) {
/* enable RC control input */
status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input");
}
status_flags.condition_system_sensors_initialized = true; answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* this always succeeds */
calib_ret = OK;
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
} }
arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_STANDBY, &armed, status_flags.condition_calibration_enabled = false;
false /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY
30_s); // time since boot not relevant for switching to ARMING_STATE_STANDBY
} else { if (calib_ret == OK) {
tune_negative(true); tune_positive(true);
// time since boot not relevant here
if (PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, false, false, true, 30_s)) {
status_flags.condition_system_sensors_initialized = true;
}
arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY
30_s); // time since boot not relevant for switching to ARMING_STATE_STANDBY
} else {
tune_negative(true);
}
} }
break; break;
@ -3465,59 +3474,68 @@ void *commander_low_prio_loop(void *arg)
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (((int)(cmd.param1)) == 0) { if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
int ret = param_load_default(); || status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) {
if (ret == OK) { // reject if armed or shutting down
mavlink_log_info(&mavlink_log_pub, "Settings loaded"); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "Error loading settings");
/* convenience as many parts of NuttX use negative errno */ if (((int)(cmd.param1)) == 0) {
if (ret < 0) { int ret = param_load_default();
ret = -ret;
} if (ret == OK) {
mavlink_log_info(&mavlink_log_pub, "Settings loaded");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else {
mavlink_log_critical(&mavlink_log_pub, "Error loading settings");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0) {
ret = -ret;
}
if (ret < 1000) { if (ret < 1000) {
mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret)); mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub); } else if (((int)(cmd.param1)) == 1) {
}
} else if (((int)(cmd.param1)) == 1) { int ret = param_save_default();
int ret = param_save_default(); if (ret == OK) {
/* do not spam MAVLink, but provide the answer / green led mechanism */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
if (ret == OK) { } else {
/* do not spam MAVLink, but provide the answer / green led mechanism */ mavlink_log_critical(&mavlink_log_pub, "Error saving settings");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else { /* convenience as many parts of NuttX use negative errno */
mavlink_log_critical(&mavlink_log_pub, "Error saving settings"); if (ret < 0) {
ret = -ret;
}
/* convenience as many parts of NuttX use negative errno */ if (ret < 1000) {
if (ret < 0) { mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
ret = -ret; }
}
if (ret < 1000) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub); } else if (((int)(cmd.param1)) == 2) {
}
} else if (((int)(cmd.param1)) == 2) {
/* reset parameters and save empty file */ /* reset parameters and save empty file */
param_reset_all(); param_reset_all();
/* do not spam MAVLink, but provide the answer / green led mechanism */ /* do not spam MAVLink, but provide the answer / green led mechanism */
mavlink_log_critical(&mavlink_log_pub, "Onboard parameters reset"); mavlink_log_critical(&mavlink_log_pub, "Onboard parameters reset");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
}
} }
break; break;

Write Preview
Loading…
Cancel
Save