diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-17 18:39:46 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-17 18:39:46 +0200 |
commit | 36d474bfa31a44c49647cf8fc9d825cb1e919182 (patch) | |
tree | ed9773a95f01d0df711aadf0f5e6c4034fd38760 | |
parent | e9b6cfd671c72b75f2bf79bf1031ea8697f430b2 (diff) | |
download | px4-firmware-36d474bfa31a44c49647cf8fc9d825cb1e919182.tar.gz px4-firmware-36d474bfa31a44c49647cf8fc9d825cb1e919182.tar.bz2 px4-firmware-36d474bfa31a44c49647cf8fc9d825cb1e919182.zip |
WIP on getting low-priority non-command tasks out of the commander main loop
-rw-r--r-- | src/modules/commander/commander.cpp | 288 |
1 files changed, 149 insertions, 139 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034e..891dd893b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -280,6 +280,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* only handle high-priority commands here */ + /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { @@ -363,95 +365,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // XXX implement later + result = VEHICLE_CMD_RESULT_DENIED; break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { - - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ - - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - break; - - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if ((int)(cmd->param1) == 1) { - /* gyro calibration */ - new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - - } else if ((int)(cmd->param2) == 1) { - /* magnetometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - - } else if ((int)(cmd->param3) == 1) { - /* zero-altitude pressure calibration */ - //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - } else if ((int)(cmd->param4) == 1) { - /* RC calibration */ - new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - - } else if ((int)(cmd->param5) == 1) { - /* accelerometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - - } else if ((int)(cmd->param6) == 1) { - /* airspeed calibration */ - new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if (((int)(cmd->param1)) == 0) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - - } else if (((int)(cmd->param1)) == 1) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - default: break; } @@ -460,6 +377,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* we do not care in the high prio loop about commands we don't know */ } else { tune_negative(); @@ -472,19 +391,24 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); } } /* send any requested ACKs */ - if (cmd->confirmation > 0) { + if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } } +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -524,13 +448,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); /* armed topic */ - struct actuator_armed_s armed; orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); @@ -631,7 +553,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; memset(&safety, 0, sizeof(safety)); safety.safety_switch_available = false; safety.safety_off = false; @@ -1633,80 +1554,169 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); - low_prio_task = LOW_PRIO_TASK_NONE; + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = cmd_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { - switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - } + /* 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; + } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - case LOW_PRIO_TASK_PARAM_SAVE: + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + continue; - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - } + /* only handle low-priority commands here */ + switch (cmd.command) { - low_prio_task = LOW_PRIO_TASK_NONE; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + /* reboot to bootloader */ + systemreset(true); + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - do_gyro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + /* try to go to INIT/PREFLIGHT arming state */ - case LOW_PRIO_TASK_MAG_CALIBRATION: + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + result = VEHICLE_CMD_RESULT_DENIED; + break; + } - do_mag_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + do_gyro_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + do_mag_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - // do_baro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + result = VEHICLE_CMD_RESULT_DENIED; + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + result = VEHICLE_CMD_RESULT_DENIED; - case LOW_PRIO_TASK_RC_CALIBRATION: + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + do_accel_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - // do_rc_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + do_airspeed_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - do_accel_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + break; + } - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - do_airspeed_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + } + + break; + } - case LOW_PRIO_TASK_NONE: default: - /* slow down to 10Hz */ - usleep(100000); break; } + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + + } else { + tune_negative(); + + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + } + } + + /* 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 + } + } return 0; |