From 984a815b94709916174f6a0beeb8ae8217a9aed1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:08:23 +0100 Subject: [PATCH 1/7] Navigator: make state names generic so that they can be used by the FSM visualisation tool later --- src/modules/commander/commander.cpp | 6 +- src/modules/mavlink/mavlink.c | 10 +- src/modules/navigator/navigator_main.cpp | 144 +++++++++--------- .../uORB/topics/vehicle_control_mode.h | 14 +- 4 files changed, 87 insertions(+), 87 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52ad..ad142d09ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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 = NAV_STATE_LOITER; + status->set_nav_state = 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 = NAV_STATE_MISSION; + status->set_nav_state = 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[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; + status.set_nav_state = STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f80917162..a13d0c537f 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -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 == NAV_STATE_NONE) { // failsafe, shouldn't happen + if (control_mode.nav_state == STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { + } else if (control_mode.nav_state == STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (control_mode.nav_state == STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (control_mode.nav_state == STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (control_mode.nav_state == STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e3..8cc2a1489e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -188,7 +188,7 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - char *nav_states_str[NAV_STATE_MAX]; + char *nav_states_str[MAX_STATE]; struct { float min_altitude; @@ -227,7 +227,7 @@ private: /** * State machine transition table */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; + static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; enum RTLState { RTL_STATE_NONE = 0, @@ -363,7 +363,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), + StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -423,7 +423,7 @@ Navigator::Navigator() : nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; + myState = STATE_NONE; start_none(); } @@ -604,7 +604,7 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = 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() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -691,7 +691,7 @@ Navigator::task_main() stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == 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() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: + case STATE_NONE: /* nothing to do */ break; - case NAV_STATE_LOITER: + case STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - case NAV_STATE_MISSION: + case STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -723,8 +723,8 @@ Navigator::task_main() break; - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + case STATE_RTL: + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -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 == NAV_STATE_NONE) { + if (myState == STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -755,14 +755,14 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { + if (myState != STATE_READY) { dispatch(EVENT_LAND_REQUESTED); } @@ -814,7 +814,7 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { + if (myState == STATE_MISSION || myState == STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); } @@ -903,19 +903,19 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: + case STATE_NONE: warnx("State: None"); break; - case NAV_STATE_LOITER: + case STATE_LOITER: warnx("State: Loiter"); break; - case NAV_STATE_MISSION: + case STATE_MISSION: warnx("State: Mission"); break; - case NAV_STATE_RTL: + case STATE_RTL: warnx("State: RTL"); break; @@ -925,72 +925,72 @@ Navigator::status() } } -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { { /* 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}, + /* 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}, }, { /* 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}, + /* 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}, }, { /* 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}, + /* 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}, }, { /* 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}, + /* 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}, }, { /* 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 + /* 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 }, { /* 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_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_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 */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, }, }; @@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached() void Navigator::on_mission_item_reached() { - if (myState == NAV_STATE_MISSION) { + if (myState == STATE_MISSION) { if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1644,7 +1644,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_manual_enabled = false; switch (myState) { - case NAV_STATE_READY: + case STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1654,7 +1654,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_climb_rate_enabled = false; break; - case NAV_STATE_LAND: + case STATE_LAND: /* land with or without position control */ _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac8987..7869a69585 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,13 +62,13 @@ */ typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + STATE_NONE = 0, + STATE_READY, + STATE_LOITER, + STATE_MISSION, + STATE_RTL, + STATE_LAND, + MAX_STATE } nav_state_t; struct vehicle_control_mode_s From cd9ec04904fa684f0ecddb3f2a4511346458fe8b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:15:28 +0100 Subject: [PATCH 2/7] Added FSM visualisation script --- Tools/draw_fsm_diagram.py | 202 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100755 Tools/draw_fsm_diagram.py diff --git a/Tools/draw_fsm_diagram.py b/Tools/draw_fsm_diagram.py new file mode 100755 index 0000000000..a36d39b41e --- /dev/null +++ b/Tools/draw_fsm_diagram.py @@ -0,0 +1,202 @@ +#!/usr/bin/python3 + +"""draw_fsm_diagram.py: Create dot code from a state transition table + +convert dot code to png using graphviz: + +dot fsm.dot -Tpng -o fsm.png +""" + +from optparse import OptionParser +import re +import subprocess + +__author__ = "Julian Oes" + +def get_dot_header(): + + return """digraph finite_state_machine { + graph [ dpi = 300 ]; + ratio = 1.5 + node [shape = circle];""" + +def get_dot_footer(): + + return """}\n""" + +def main(): + + # parse input arguments + parser = OptionParser() + parser.add_option("-i", "--input-file", default=None, help="choose file to parse") + parser.add_option("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_option("-t", "--table-file", default=None, help="choose file for output of table") + (options, args) = parser.parse_args() + + # open source file + if options.input_file == None: + exit('please specify file') + f = open(options.input_file,'r') + source = f.read() + + # search for state transition table and extract the table itself + # first look for StateTable::Tran + # then accept anything including newline until { + # but don't accept the definition (without ;) + # then extract anything inside the brackets until }; + match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source) + + if not match: + exit('no state transition table found') + + table_source = match.group(1) + + # bookkeeping for error checking + num_errors_found = 0 + + states = [] + events = [] + + # first get all states and events + for table_line in table_source.split('\n'): + + match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + if match: + states.append(str(match.group(1))) + # go to next line + continue + + if len(states) == 1: + match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line) + if match: + events.append(str(match.group(1))) + + print('Found %d states and %d events' % (len(states), len(events))) + + + # keep track of origin state + state = None + + # fill dot code in here + dot_code = '' + + # create table len(states)xlen(events) + transition_table = [[[] for x in range(len(states))] for y in range(len(events))] + + # now fill the transition table and write the dot code + for table_line in table_source.split('\n'): + + # get states + # from: /* STATE_NONE */ + # extract only "NONE" + match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + if match: + state = match.group(1) + state_index = states.index(state) + # go to next line + continue + + # can't advance without proper state + if state == None: + continue + + # get event and next state + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY} + # extract "READY_REQUESTED" and "READY" if there is ACTION + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*STATE_(\w+)', table_line) + + # get event and next state + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + # extract "NONE_REQUESTED" and "STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*STATE_(\w+)', table_line) + + # ignore lines with brackets only + if match_action or match_no_action: + + # only write arrows for actions + if match_action: + event = match_action.group(1) + new_state = match_action.group(2) + dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n' + + elif match_no_action: + event = match_no_action.group(1) + new_state = match_no_action.group(2) + + # check for state changes without action + if state != new_state: + print('Error: no action but state change:') + print('State: ' + state + ' changed to: ' + new_state) + print(table_line) + num_errors_found += 1 + + # check for wrong events + if event not in events: + print('Error: unknown event: ' + event) + print(table_line) + num_errors_found += 1 + + # check for wrong new states + if new_state not in states: + print('Error: unknown new state: ' + new_state) + print(table_line) + num_errors_found += 1 + + # save new state in transition table + event_index = events.index(event) + + # bold for action + if match_action: + transition_table[event_index][state_index] = '**' + new_state + '**' + else: + transition_table[event_index][state_index] = new_state + + + + # assemble dot code + dot_code = get_dot_header() + dot_code + get_dot_footer() + + # write or print dot file + if options.dot_file: + f = open(options.dot_file,'w') + f.write(dot_code) + print('Wrote dot file') + else: + print('##########Dot-start##########') + print(dot_code) + print('##########Dot-end############') + + + # assemble doku wiki table + table_code = '| ^ ' + # start with header of all states + for state in states: + table_code += state + ' ^ ' + + table_code += '\n' + + # add events and new states + for event, row in zip(events, transition_table): + table_code += '^ ' + event + ' | ' + for new_state in row: + table_code += new_state + ' | ' + table_code += '\n' + + # write or print wiki table + if options.table_file: + f = open(options.table_file,'w') + f.write(table_code) + print('Wrote table file') + else: + print('##########Table-start########') + print(table_code) + print('##########Table-end##########') + + # report obvous errors + if num_errors_found: + print('Obvious errors found: %d' % num_errors_found) + else: + print('No obvious errors found') + +if __name__ == '__main__': + main() From 6002819f8fccb491dcfbe23de892a827cd9f4618 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:21:48 +0100 Subject: [PATCH 3/7] Navigator: FSM bugfix and missing break --- src/modules/navigator/navigator_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8cc2a1489e..2a782ea032 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -989,8 +989,8 @@ StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { /* 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_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LAND}, }, }; @@ -1607,6 +1607,7 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break; From c5a3dd916899ddd8409380c27c05a8b2d903c8f4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:46:44 +0100 Subject: [PATCH 4/7] FSM visualisation script: use argparse instead of optionparse and some minor cleanup --- Tools/draw_fsm_diagram.py | 29 ++++++++++++++--------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/Tools/draw_fsm_diagram.py b/Tools/draw_fsm_diagram.py index a36d39b41e..fe37b6ba3f 100755 --- a/Tools/draw_fsm_diagram.py +++ b/Tools/draw_fsm_diagram.py @@ -1,15 +1,14 @@ -#!/usr/bin/python3 +#!/usr/bin/env python3 -"""draw_fsm_diagram.py: Create dot code from a state transition table +"""draw_fsm_diagram.py: Create dot code and dokuwiki table from a state transition table convert dot code to png using graphviz: dot fsm.dot -Tpng -o fsm.png """ -from optparse import OptionParser +import argparse import re -import subprocess __author__ = "Julian Oes" @@ -27,16 +26,16 @@ def get_dot_footer(): def main(): # parse input arguments - parser = OptionParser() - parser.add_option("-i", "--input-file", default=None, help="choose file to parse") - parser.add_option("-d", "--dot-file", default=None, help="choose file for output dot file") - parser.add_option("-t", "--table-file", default=None, help="choose file for output of table") - (options, args) = parser.parse_args() + parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.') + parser.add_argument("-i", "--input-file", default=None, help="choose file to parse") + parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table") + args = parser.parse_args() # open source file - if options.input_file == None: + if args.input_file == None: exit('please specify file') - f = open(options.input_file,'r') + f = open(args.input_file,'r') source = f.read() # search for state transition table and extract the table itself @@ -157,8 +156,8 @@ def main(): dot_code = get_dot_header() + dot_code + get_dot_footer() # write or print dot file - if options.dot_file: - f = open(options.dot_file,'w') + if args.dot_file: + f = open(args.dot_file,'w') f.write(dot_code) print('Wrote dot file') else: @@ -183,8 +182,8 @@ def main(): table_code += '\n' # write or print wiki table - if options.table_file: - f = open(options.table_file,'w') + if args.table_file: + f = open(args.table_file,'w') f.write(table_code) print('Wrote table file') else: From 1488d32d0f25469da9119f36429c8783256048b1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:49:14 +0100 Subject: [PATCH 5/7] FSM visualisation script: renamed the file --- Tools/{draw_fsm_diagram.py => fsm_visualisation.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename Tools/{draw_fsm_diagram.py => fsm_visualisation.py} (98%) diff --git a/Tools/draw_fsm_diagram.py b/Tools/fsm_visualisation.py similarity index 98% rename from Tools/draw_fsm_diagram.py rename to Tools/fsm_visualisation.py index fe37b6ba3f..5532b99bc8 100755 --- a/Tools/draw_fsm_diagram.py +++ b/Tools/fsm_visualisation.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -"""draw_fsm_diagram.py: Create dot code and dokuwiki table from a state transition table +"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table convert dot code to png using graphviz: From 547080f1882918a65db4e7cba396ae2c72d84544 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 12:29:30 +0100 Subject: [PATCH 6/7] Revert "Navigator: make state names generic so that they can be used by the FSM visualisation tool later" This reverts commit 984a815b94709916174f6a0beeb8ae8217a9aed1. --- src/modules/commander/commander.cpp | 6 +- src/modules/mavlink/mavlink.c | 10 +- src/modules/navigator/navigator_main.cpp | 156 +++++++++--------- .../uORB/topics/vehicle_control_mode.h | 14 +- 4 files changed, 93 insertions(+), 93 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ad142d09ec..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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[]) 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; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a13d0c537f..4f80917162 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -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; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2a782ea032..e6b7ef93d8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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: /** * 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; 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() : nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = STATE_NONE; + myState = NAV_STATE_NONE; start_none(); } @@ -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() /* 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() 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() _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() 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() } 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() } 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() 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() } 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() } } -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() 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() _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() _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; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7869a69585..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -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 From 99875bbd28ee813950efb9402b3acce55c34469b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 12:31:23 +0100 Subject: [PATCH 7/7] FSM visualisation script: cope with prefixed state names --- Tools/fsm_visualisation.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py index 5532b99bc8..c678ef0f4b 100755 --- a/Tools/fsm_visualisation.py +++ b/Tools/fsm_visualisation.py @@ -59,7 +59,7 @@ def main(): # first get all states and events for table_line in table_source.split('\n'): - match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) if match: states.append(str(match.group(1))) # go to next line @@ -86,9 +86,9 @@ def main(): for table_line in table_source.split('\n'): # get states - # from: /* STATE_NONE */ + # from: /* NAV_STATE_NONE */ # extract only "NONE" - match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) if match: state = match.group(1) state_index = states.index(state) @@ -100,14 +100,14 @@ def main(): continue # get event and next state - # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY} + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY} # extract "READY_REQUESTED" and "READY" if there is ACTION - match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*STATE_(\w+)', table_line) + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line) # get event and next state - # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - # extract "NONE_REQUESTED" and "STATE_NONE" if there is NO_ACTION - match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*STATE_(\w+)', table_line) + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + # extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line) # ignore lines with brackets only if match_action or match_no_action: