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

Revert "Navigator: make state names generic so that they can be used by the FSM visualisation tool later" 

This reverts commit 984a815b94.
sbg
Julian Oes 11 years ago
parent
1488d32d0f
commit
547080f188
4 changed files with 93 additions and 93 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      src/modules/commander/commander.cpp
  2. 10
      src/modules/mavlink/mavlink.c
  3. 156
      src/modules/navigator/navigator_main.cpp
  4. 14
      src/modules/uORB/topics/vehicle_control_mode.h

6
src/modules/commander/commander.cpp
Unescape View File

@ -493,14 +493,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe @@ -493,14 +493,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
unsigned int mav_goto = cmd->param1;
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = STATE_LOITER;
status->set_nav_state = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->set_nav_state = STATE_MISSION;
status->set_nav_state = NAV_STATE_MISSION;
status->set_nav_state_timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
@ -624,7 +624,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -624,7 +624,7 @@ int commander_thread_main(int argc, char *argv[])
memset(&armed, 0, sizeof(armed));
status.main_state = MAIN_STATE_MANUAL;
status.set_nav_state = STATE_NONE;
status.set_nav_state = NAV_STATE_NONE;
status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;

10
src/modules/mavlink/mavlink.c
Unescape View File

@ -220,15 +220,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u @@ -220,15 +220,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
if (control_mode.nav_state == STATE_NONE) { // failsafe, shouldn't happen
if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == STATE_READY) {
} else if (control_mode.nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == STATE_LOITER) {
} else if (control_mode.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == STATE_MISSION) {
} else if (control_mode.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (control_mode.nav_state == STATE_RTL) {
} else if (control_mode.nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
}
}

156
src/modules/navigator/navigator_main.cpp
Unescape View File

@ -188,7 +188,7 @@ private: @@ -188,7 +188,7 @@ private:
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
char *nav_states_str[MAX_STATE];
char *nav_states_str[NAV_STATE_MAX];
struct {
float min_altitude;
@ -227,7 +227,7 @@ private: @@ -227,7 +227,7 @@ private:
/**
* State machine transition table
*/
static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT];
static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
enum RTLState {
RTL_STATE_NONE = 0,
@ -363,7 +363,7 @@ Navigator *g_navigator; @@ -363,7 +363,7 @@ Navigator *g_navigator;
Navigator::Navigator() :
/* state machine transition table */
StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT),
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
_task_should_exit(false),
_navigator_task(-1),
@ -423,7 +423,7 @@ Navigator::Navigator() : @@ -423,7 +423,7 @@ Navigator::Navigator() :
nav_states_str[5] = "LAND";
/* Initialize state machine */
myState = STATE_NONE;
myState = NAV_STATE_NONE;
start_none();
}
@ -604,7 +604,7 @@ Navigator::task_main() @@ -604,7 +604,7 @@ Navigator::task_main()
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
unsigned prevState = STATE_NONE;
unsigned prevState = NAV_STATE_NONE;
bool pub_control_mode = true;
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
@ -667,7 +667,7 @@ Navigator::task_main() @@ -667,7 +667,7 @@ Navigator::task_main()
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) {
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -691,7 +691,7 @@ Navigator::task_main() @@ -691,7 +691,7 @@ Navigator::task_main()
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == STATE_RTL) {
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
@ -705,15 +705,15 @@ Navigator::task_main() @@ -705,15 +705,15 @@ Navigator::task_main()
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case STATE_NONE:
case NAV_STATE_NONE:
/* nothing to do */
break;
case STATE_LOITER:
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case STATE_MISSION:
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
@ -723,8 +723,8 @@ Navigator::task_main() @@ -723,8 +723,8 @@ Navigator::task_main()
break;
case STATE_RTL:
if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) {
case NAV_STATE_RTL:
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -737,7 +737,7 @@ Navigator::task_main() @@ -737,7 +737,7 @@ Navigator::task_main()
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == STATE_NONE) {
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
@ -755,14 +755,14 @@ Navigator::task_main() @@ -755,14 +755,14 @@ Navigator::task_main()
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
/* RTL on failsafe */
if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) {
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) {
/* LAND on failsafe */
if (myState != STATE_READY) {
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
@ -814,7 +814,7 @@ Navigator::task_main() @@ -814,7 +814,7 @@ Navigator::task_main()
global_position_update();
/* only check if waypoint has been reached in MISSION and RTL modes */
if (myState == STATE_MISSION || myState == STATE_RTL) {
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (check_mission_item_reached()) {
on_mission_item_reached();
}
@ -903,19 +903,19 @@ Navigator::status() @@ -903,19 +903,19 @@ Navigator::status()
}
switch (myState) {
case STATE_NONE:
case NAV_STATE_NONE:
warnx("State: None");
break;
case STATE_LOITER:
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case STATE_MISSION:
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case STATE_RTL:
case NAV_STATE_RTL:
warnx("State: RTL");
break;
@ -925,72 +925,72 @@ Navigator::status() @@ -925,72 +925,72 @@ Navigator::status()
}
}
StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = {
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE},
/* NAV_STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
},
{
/* STATE_READY */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_READY},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_READY},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_READY},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_READY},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_READY},
/* NAV_STATE_READY */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
},
{
/* STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_LOITER},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER},
/* NAV_STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
},
{
/* STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION},
/* NAV_STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
},
{
/* STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, // TODO need to reset rtl_state
/* NAV_STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state
},
{
/* STATE_LAND */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LAND},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LAND},
/* NAV_STATE_LAND */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND},
},
};
@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached() @@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached()
void
Navigator::on_mission_item_reached()
{
if (myState == STATE_MISSION) {
if (myState == NAV_STATE_MISSION) {
if (_do_takeoff) {
/* takeoff completed */
_do_takeoff = false;
@ -1645,7 +1645,7 @@ Navigator::publish_control_mode() @@ -1645,7 +1645,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_manual_enabled = false;
switch (myState) {
case STATE_READY:
case NAV_STATE_READY:
/* disable all controllers, armed but idle */
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
@ -1655,7 +1655,7 @@ Navigator::publish_control_mode() @@ -1655,7 +1655,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_climb_rate_enabled = false;
break;
case STATE_LAND:
case NAV_STATE_LAND:
/* land with or without position control */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;

14
src/modules/uORB/topics/vehicle_control_mode.h
Unescape View File

@ -62,13 +62,13 @@ @@ -62,13 +62,13 @@
*/
typedef enum {
STATE_NONE = 0,
STATE_READY,
STATE_LOITER,
STATE_MISSION,
STATE_RTL,
STATE_LAND,
MAX_STATE
NAV_STATE_NONE = 0,
NAV_STATE_READY,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
struct vehicle_control_mode_s

Write Preview
Loading…
Cancel
Save