fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 05:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moving nav state from commander to navigator, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-29 12:16:49 +04:00
parent 72d9c80ce9
commit 7e29028429
11 changed files with 249 additions and 692 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+42 -236
View File
@@ -69,7 +69,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
@@ -194,7 +193,7 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
/**
* Mainloop of commander.
@@ -215,8 +214,6 @@ void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/
@@ -346,13 +343,12 @@ void print_status()
warnx("arming: %s", armed_str);
}
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* only handle high-priority commands here */
@@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
arming_res = arming_state_transition(status, safety, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
case VEHICLE_CMD_NAV_TAKEOFF: {
if (armed->armed) {
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
if (nav_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
}
if (nav_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
/* reject TAKEOFF not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case VEHICLE_CMD_PREFLIGHT_STORAGE:
/* ignore commands that handled in low prio loop */
break;
default:
/* warn about unsupported commands */
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
/* supported command handling stop */
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();
if (result == VEHICLE_CMD_RESULT_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
}
/* send any requested ACKs */
@@ -564,9 +530,6 @@ static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* Initialize all flags to false */
memset(&control_mode, 0, sizeof(control_mode));
status.main_state = MAIN_STATE_MANUAL;
status.navigation_state = NAVIGATION_STATE_DIRECT;
status.set_nav_state = NAV_STATE_INIT;
status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
@@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = true;
status.offboard_control_signal_lost = true;
/* allow manual override initially */
control_mode.flag_external_manual_override_ok = true;
/* set battery warning flag */
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
status.condition_battery_voltage_valid = false;
@@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
// XXX just disable offboard control for now
control_mode.flag_control_offboard_enabled = false;
/* advertise to ORB */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
/* publish current state machine */
@@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[])
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
/* home position */
orb_advert_t home_pub = -1;
struct home_position_s home;
@@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[])
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
control_mode.flag_external_manual_override_ok = false;
status.is_rotary_wing = true;
} else {
control_mode.flag_external_manual_override_ok = true;
status.is_rotary_wing = false;
}
@@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX this would be the right approach to do it, but do we *WANT* this?
// /* disarm if safety is now on and still armed */
// if (safety.safety_switch_available && !safety.safety_off) {
// (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
// (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
// }
}
@@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
@@ -1012,7 +963,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
} else {
// XXX: Add emergency stuff if sensors are lost
@@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[])
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
(status.condition_landed && (
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
status.navigation_state == NAVIGATION_STATE_VECTOR
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@@ -1132,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@@ -1185,12 +1134,12 @@ int commander_thread_main(int argc, char *argv[])
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
if (flighttermination_res == TRANSITION_CHANGED) {
tune_positive();
}
} else {
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
}
@@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
handle_command(&status, &safety, &control_mode, &cmd, &armed);
}
/* evaluate the navigation state machine */
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
handle_command(&status, &safety, &cmd, &armed);
}
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool navigation_state_changed = check_navigation_state_changed();
bool flighttermination_state_changed = check_flighttermination_state_changed();
hrt_abstime t1 = hrt_absolute_time();
if (navigation_state_changed || arming_state_changed) {
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
}
if (arming_state_changed || main_state_changed || navigation_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
if (arming_state_changed || main_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
status_changed = true;
}
@@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[])
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg)
}
}
transition_result_t
check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
{
transition_result_t res = TRANSITION_DENIED;
if (status->main_state == MAIN_STATE_AUTO) {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
// TODO AUTO_LAND handling
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
// XXX: only respect the condition_landed when the local position is actually valid
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
return TRANSITION_NOT_CHANGED;
}
}
if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res;
}
}
/* switch to AUTO mode */
if (status->rc_signal_found_once && !status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
if (status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
} else {
/* switch to MISSION when no RC control and first time in some AUTO mode */
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
res = TRANSITION_NOT_CHANGED;
} else {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
}
} else {
/* disarmed, always switch to AUTO_READY */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
} else {
/* manual control modes */
if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
/* switch to failsafe mode */
bool manual_control_old = control_mode->flag_control_manual_enabled;
if (!status->condition_landed && status->condition_local_position_valid) {
/* in air: try to hold position if possible */
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
} else {
/* landed: don't try to hold position but land (if taking off) */
res = TRANSITION_DENIED;
}
if (res == TRANSITION_DENIED) {
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
}
control_mode->flag_control_manual_enabled = false;
if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
/* mark navigation state as changed to force immediate flag publishing */
set_navigation_state_changed();
res = TRANSITION_CHANGED;
}
if (res == TRANSITION_CHANGED) {
if (control_mode->flag_control_position_enabled) {
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
} else {
if (status->condition_landed) {
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
}
}
}
} else {
switch (status->main_state) {
case MAIN_STATE_MANUAL:
res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
case MAIN_STATE_SEATBELT:
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
break;
case MAIN_STATE_EASY:
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
break;
default:
break;
}
}
}
return res;
}
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
@@ -1784,7 +1590,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg)
else
tune_negative();
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
break;
}
@@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg)
}
default:
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
/* don't answer on unsupported commands, it will be done in main loop */
break;
}