mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 17:01:36 +08:00
Move autosave into the 1-second timeout check.
This commit is contained in:
Lorenz Meier
+187
-190
@@ -2549,216 +2549,213 @@ void *commander_low_prio_loop(void *arg)
|
|||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
|
|
||||||
while (!thread_should_exit) {
|
while (!thread_should_exit) {
|
||||||
/* wait for up to 200ms for data */
|
/* wait for up to 1000ms for data */
|
||||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
|
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
|
||||||
|
|
||||||
/* timed out - periodic check for thread_should_exit, etc. */
|
/* timed out - periodic check for thread_should_exit, etc. */
|
||||||
if (pret == 0) {
|
if (pret == 0) {
|
||||||
continue;
|
/* trigger a param autosave if required */
|
||||||
}
|
if (_param_autosave) {
|
||||||
|
if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) {
|
||||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
|
||||||
if (pret < 0) {
|
|
||||||
warn("poll error %d, %d", pret, errno);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* if we reach here, we have a valid command */
|
|
||||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
|
||||||
|
|
||||||
/* ignore commands the high-prio loop handles */
|
|
||||||
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
|
|
||||||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
|
|
||||||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
|
|
||||||
cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* only handle low-priority commands here */
|
|
||||||
switch (cmd.command) {
|
|
||||||
|
|
||||||
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
|
||||||
if (is_safe(&status, &safety, &armed)) {
|
|
||||||
|
|
||||||
if (((int)(cmd.param1)) == 1) {
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
usleep(100000);
|
|
||||||
/* reboot */
|
|
||||||
systemreset(false);
|
|
||||||
|
|
||||||
} else if (((int)(cmd.param1)) == 3) {
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
usleep(100000);
|
|
||||||
/* reboot to bootloader */
|
|
||||||
systemreset(true);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
|
|
||||||
|
|
||||||
int calib_ret = ERROR;
|
|
||||||
|
|
||||||
/* try to go to INIT/PREFLIGHT arming state */
|
|
||||||
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
|
|
||||||
true /* fRunPreArmChecks */, mavlink_fd)) {
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
if ((int)(cmd.param1) == 1) {
|
|
||||||
/* gyro calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
calib_ret = do_gyro_calibration(mavlink_fd);
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param2) == 1) {
|
|
||||||
/* magnetometer calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
calib_ret = do_mag_calibration(mavlink_fd);
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param3) == 1) {
|
|
||||||
/* zero-altitude pressure calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param4) == 1) {
|
|
||||||
/* RC calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
/* disable RC control input completely */
|
|
||||||
status.rc_input_blocked = true;
|
|
||||||
calib_ret = OK;
|
|
||||||
mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param4) == 2) {
|
|
||||||
/* RC trim calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
calib_ret = do_trim_calibration(mavlink_fd);
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param5) == 1) {
|
|
||||||
/* accelerometer calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
calib_ret = do_accel_calibration(mavlink_fd);
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param6) == 1) {
|
|
||||||
/* airspeed calibration */
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
calib_ret = do_airspeed_calibration(mavlink_fd);
|
|
||||||
|
|
||||||
} else if ((int)(cmd.param4) == 0) {
|
|
||||||
/* RC calibration ended - have we been in one worth confirming? */
|
|
||||||
if (status.rc_input_blocked) {
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
/* enable RC control input */
|
|
||||||
status.rc_input_blocked = false;
|
|
||||||
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* this always succeeds */
|
|
||||||
calib_ret = OK;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
if (calib_ret == OK) {
|
|
||||||
tune_positive(true);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
tune_negative(true);
|
|
||||||
}
|
|
||||||
|
|
||||||
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
|
|
||||||
|
|
||||||
if (((int)(cmd.param1)) == 0) {
|
|
||||||
int ret = param_load_default();
|
|
||||||
|
|
||||||
if (ret == OK) {
|
|
||||||
mavlink_log_info(mavlink_fd, "settings loaded");
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
mavlink_log_critical(mavlink_fd, "settings load ERROR");
|
|
||||||
|
|
||||||
/* convenience as many parts of NuttX use negative errno */
|
|
||||||
if (ret < 0) {
|
|
||||||
ret = -ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ret < 1000) {
|
|
||||||
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
|
|
||||||
}
|
|
||||||
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else if (((int)(cmd.param1)) == 1) {
|
|
||||||
int ret = param_save_default();
|
int ret = param_save_default();
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
mavlink_log_info(mavlink_fd, "settings saved");
|
mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_critical(mavlink_fd, "settings save error");
|
mavlink_and_console_log_critical(mavlink_fd, "settings save error");
|
||||||
|
|
||||||
/* convenience as many parts of NuttX use negative errno */
|
|
||||||
if (ret < 0) {
|
|
||||||
ret = -ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ret < 1000) {
|
|
||||||
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
|
|
||||||
}
|
|
||||||
|
|
||||||
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_param_autosave = false;
|
||||||
|
_param_autosave_timeout = 0;
|
||||||
|
} else {
|
||||||
|
_param_autosave_timeout = hrt_absolute_time();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else if (pret < 0) {
|
||||||
|
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||||
|
warn("poll error %d, %d", pret, errno);
|
||||||
|
continue;
|
||||||
|
} else {
|
||||||
|
|
||||||
|
/* if we reach here, we have a valid command */
|
||||||
|
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||||
|
|
||||||
|
/* ignore commands the high-prio loop handles */
|
||||||
|
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
|
||||||
|
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
|
||||||
|
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
|
||||||
|
cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* only handle low-priority commands here */
|
||||||
|
switch (cmd.command) {
|
||||||
|
|
||||||
|
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
||||||
|
if (is_safe(&status, &safety, &armed)) {
|
||||||
|
|
||||||
|
if (((int)(cmd.param1)) == 1) {
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
usleep(100000);
|
||||||
|
/* reboot */
|
||||||
|
systemreset(false);
|
||||||
|
|
||||||
|
} else if (((int)(cmd.param1)) == 3) {
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
usleep(100000);
|
||||||
|
/* reboot to bootloader */
|
||||||
|
systemreset(true);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
|
||||||
|
|
||||||
case VEHICLE_CMD_START_RX_PAIR:
|
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
|
||||||
/* handled in the IO driver */
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
int calib_ret = ERROR;
|
||||||
/* don't answer on unsupported commands, it will be done in main loop */
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* trigger a param autosave if required */
|
/* try to go to INIT/PREFLIGHT arming state */
|
||||||
if (_param_autosave) {
|
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
|
||||||
if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) {
|
true /* fRunPreArmChecks */, mavlink_fd)) {
|
||||||
int ret = param_save_default();
|
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
if (ret == OK) {
|
if ((int)(cmd.param1) == 1) {
|
||||||
mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
|
/* gyro calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
calib_ret = do_gyro_calibration(mavlink_fd);
|
||||||
|
|
||||||
} else {
|
} else if ((int)(cmd.param2) == 1) {
|
||||||
mavlink_and_console_log_critical(mavlink_fd, "settings save error");
|
/* magnetometer calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
calib_ret = do_mag_calibration(mavlink_fd);
|
||||||
|
|
||||||
|
} else if ((int)(cmd.param3) == 1) {
|
||||||
|
/* zero-altitude pressure calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||||
|
|
||||||
|
} else if ((int)(cmd.param4) == 1) {
|
||||||
|
/* RC calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
/* disable RC control input completely */
|
||||||
|
status.rc_input_blocked = true;
|
||||||
|
calib_ret = OK;
|
||||||
|
mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
|
||||||
|
|
||||||
|
} else if ((int)(cmd.param4) == 2) {
|
||||||
|
/* RC trim calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
calib_ret = do_trim_calibration(mavlink_fd);
|
||||||
|
|
||||||
|
} else if ((int)(cmd.param5) == 1) {
|
||||||
|
/* accelerometer calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
calib_ret = do_accel_calibration(mavlink_fd);
|
||||||
|
|
||||||
|
} else if ((int)(cmd.param6) == 1) {
|
||||||
|
/* airspeed calibration */
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
calib_ret = do_airspeed_calibration(mavlink_fd);
|
||||||
|
|
||||||
|
} else if ((int)(cmd.param4) == 0) {
|
||||||
|
/* RC calibration ended - have we been in one worth confirming? */
|
||||||
|
if (status.rc_input_blocked) {
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
/* enable RC control input */
|
||||||
|
status.rc_input_blocked = false;
|
||||||
|
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* this always succeeds */
|
||||||
|
calib_ret = OK;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
if (calib_ret == OK) {
|
||||||
|
tune_positive(true);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
tune_negative(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||||
|
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
_param_autosave = false;
|
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
|
||||||
_param_autosave_timeout = 0;
|
|
||||||
} else {
|
|
||||||
_param_autosave_timeout = hrt_absolute_time();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* send any requested ACKs */
|
if (((int)(cmd.param1)) == 0) {
|
||||||
if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
|
int ret = param_load_default();
|
||||||
&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
|
|
||||||
/* send acknowledge command */
|
if (ret == OK) {
|
||||||
// XXX TODO
|
mavlink_log_info(mavlink_fd, "settings loaded");
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
mavlink_log_critical(mavlink_fd, "settings load ERROR");
|
||||||
|
|
||||||
|
/* convenience as many parts of NuttX use negative errno */
|
||||||
|
if (ret < 0) {
|
||||||
|
ret = -ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ret < 1000) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
|
||||||
|
}
|
||||||
|
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (((int)(cmd.param1)) == 1) {
|
||||||
|
int ret = param_save_default();
|
||||||
|
|
||||||
|
if (ret == OK) {
|
||||||
|
mavlink_log_info(mavlink_fd, "settings saved");
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
mavlink_log_critical(mavlink_fd, "settings save error");
|
||||||
|
|
||||||
|
/* convenience as many parts of NuttX use negative errno */
|
||||||
|
if (ret < 0) {
|
||||||
|
ret = -ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ret < 1000) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
|
||||||
|
}
|
||||||
|
|
||||||
|
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case VEHICLE_CMD_START_RX_PAIR:
|
||||||
|
/* handled in the IO driver */
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
/* don't answer on unsupported commands, it will be done in main loop */
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* send any requested ACKs */
|
||||||
|
if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
|
||||||
|
&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
|
||||||
|
/* send acknowledge command */
|
||||||
|
// XXX TODO
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user