From a098ca4ec68b8737243d8e7aab5bdb2db4d842a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 13:06:05 -0700 Subject: Move autosave into the 1-second timeout check. --- src/modules/commander/commander.cpp | 307 ++++++++++++++++++------------------ 1 file changed, 152 insertions(+), 155 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dadb5ea6e..35f538b23 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2549,216 +2549,213 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - /* wait for up to 200ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); + /* wait for up to 1000ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); /* timed out - periodic check for thread_should_exit, etc. */ 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) { + int ret = param_save_default(); + + if (ret == OK) { + mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); + } else { + mavlink_and_console_log_critical(mavlink_fd, "settings save error"); + } + + _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 */ - if (pret < 0) { 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); + /* 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; - } + /* 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) { + /* only handle low-priority commands here */ + switch (cmd.command) { - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { + 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); - 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 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); } - } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } + break; - break; + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + int calib_ret = ERROR; - 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; + } - /* 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); - 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.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.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.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) { + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ 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"); + 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; + } - /* this always succeeds */ - calib_ret = OK; + if (calib_ret == OK) { + tune_positive(true); - } + } else { + tune_negative(true); + } - if (calib_ret == OK) { - tune_positive(true); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); - } else { - tune_negative(true); + break; } - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - break; - } + if (((int)(cmd.param1)) == 0) { + int ret = param_load_default(); - case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (ret == OK) { + mavlink_log_info(mavlink_fd, "settings loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - if (((int)(cmd.param1)) == 0) { - int ret = param_load_default(); + } else { + mavlink_log_critical(mavlink_fd, "settings load ERROR"); - if (ret == OK) { - mavlink_log_info(mavlink_fd, "settings loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) { + ret = -ret; + } - } else { - mavlink_log_critical(mavlink_fd, "settings load ERROR"); + if (ret < 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + } - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } - if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); - } + } else if (((int)(cmd.param1)) == 1) { + int ret = param_save_default(); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } + if (ret == OK) { + mavlink_log_info(mavlink_fd, "settings saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else if (((int)(cmd.param1)) == 1) { - int ret = param_save_default(); + } else { + mavlink_log_critical(mavlink_fd, "settings save error"); - if (ret == OK) { - mavlink_log_info(mavlink_fd, "settings saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) { + ret = -ret; + } - } else { - mavlink_log_critical(mavlink_fd, "settings save error"); + if (ret < 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + } - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } - - 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; - } - - 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; - } - /* trigger a param autosave if required */ - if (_param_autosave) { - if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) { - int ret = param_save_default(); - - if (ret == OK) { - mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); - - } else { - mavlink_and_console_log_critical(mavlink_fd, "settings save error"); - } - - _param_autosave = false; - _param_autosave_timeout = 0; - } else { - _param_autosave_timeout = hrt_absolute_time(); + 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 + /* 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 + } } } -- cgit v1.2.3