mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 13:13:34 +08:00
Merged beta into mavlink rework branch
This commit is contained in:
Lorenz Meier
Executable
+201
@@ -0,0 +1,201 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
"""fsm_visualisation.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
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import re
|
||||
|
||||
__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 = 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 args.input_file == None:
|
||||
exit('please specify file')
|
||||
f = open(args.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+\w+_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: /* NAV_STATE_NONE */
|
||||
# extract only "NONE"
|
||||
match = re.search(r'/\*\s+\w+_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), NAV_STATE_READY}
|
||||
# extract "READY_REQUESTED" and "READY" if there is ACTION
|
||||
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, 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:
|
||||
|
||||
# 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 args.dot_file:
|
||||
f = open(args.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 args.table_file:
|
||||
f = open(args.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()
|
||||
@@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output):
|
||||
result = ""
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
|
||||
for param in group.GetParams():
|
||||
code = param.GetFieldValue("code")
|
||||
name = param.GetFieldValue("short_desc")
|
||||
if code != name:
|
||||
name = "%s (%s)" % (name, code)
|
||||
result += "=== %s ===\n\n" % name
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
if long_desc is not None:
|
||||
result += "%s\n\n" % long_desc
|
||||
name = name.replace("\n", "")
|
||||
result += "| %s | %s " % (code, name)
|
||||
min_val = param.GetFieldValue("min")
|
||||
if min_val is not None:
|
||||
result += "* Minimal value: %s\n" % min_val
|
||||
result += "| %s " % min_val
|
||||
else:
|
||||
result += "|"
|
||||
max_val = param.GetFieldValue("max")
|
||||
if max_val is not None:
|
||||
result += "* Maximal value: %s\n" % max_val
|
||||
result += "| %s " % max_val
|
||||
else:
|
||||
result += "|"
|
||||
def_val = param.GetFieldValue("default")
|
||||
if def_val is not None:
|
||||
result += "* Default value: %s\n" % def_val
|
||||
result += "\n"
|
||||
result += "| %s " % def_val
|
||||
else:
|
||||
result += "|"
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
if long_desc is not None:
|
||||
long_desc = long_desc.replace("\n", "")
|
||||
result += "| %s " % long_desc
|
||||
else:
|
||||
result += "|"
|
||||
result += "|\n"
|
||||
result += "\n"
|
||||
return result
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
import output
|
||||
|
||||
class DokuWikiOutput(output.Output):
|
||||
def Generate(self, groups):
|
||||
result = ""
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
for param in group.GetParams():
|
||||
code = param.GetFieldValue("code")
|
||||
name = param.GetFieldValue("short_desc")
|
||||
if code != name:
|
||||
name = "%s (%s)" % (name, code)
|
||||
result += "=== %s ===\n\n" % name
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
if long_desc is not None:
|
||||
result += "%s\n\n" % long_desc
|
||||
min_val = param.GetFieldValue("min")
|
||||
if min_val is not None:
|
||||
result += "* Minimal value: %s\n" % min_val
|
||||
max_val = param.GetFieldValue("max")
|
||||
if max_val is not None:
|
||||
result += "* Maximal value: %s\n" % max_val
|
||||
def_val = param.GetFieldValue("default")
|
||||
if def_val is not None:
|
||||
result += "* Default value: %s\n" % def_val
|
||||
result += "\n"
|
||||
return result
|
||||
@@ -10,11 +10,13 @@ LIBS=-lm
|
||||
#_DEPS = test.h
|
||||
#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
|
||||
|
||||
_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
|
||||
_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
|
||||
mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
|
||||
OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
|
||||
|
||||
#$(DEPS)
|
||||
$(ODIR)/%.o: %.cpp
|
||||
mkdir -p obj
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
|
||||
@@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
#include <sys/time.h>
|
||||
#include <inttypes.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <stdio.h>
|
||||
|
||||
hrt_abstime hrt_absolute_time() {
|
||||
struct timeval te;
|
||||
gettimeofday(&te, NULL); // get current time
|
||||
hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
|
||||
return us;
|
||||
}
|
||||
|
||||
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) {
|
||||
// not thread safe
|
||||
return hrt_absolute_time() - *then;
|
||||
}
|
||||
@@ -9,4 +9,6 @@ int main(int argc, char *argv[]) {
|
||||
"../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
|
||||
|
||||
test_mixer(3, args);
|
||||
|
||||
test_conv(1, args);
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
/************************************************************************
|
||||
* include/queue.h
|
||||
*
|
||||
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name NuttX nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
************************************************************************/
|
||||
|
||||
#ifndef __INCLUDE_QUEUE_H
|
||||
#define __INCLUDE_QUEUE_H
|
||||
|
||||
#ifndef FAR
|
||||
#define FAR
|
||||
#endif
|
||||
|
||||
/************************************************************************
|
||||
* Included Files
|
||||
************************************************************************/
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
/************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************/
|
||||
|
||||
#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
|
||||
#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
|
||||
|
||||
#define sq_next(p) ((p)->flink)
|
||||
#define dq_next(p) ((p)->flink)
|
||||
#define dq_prev(p) ((p)->blink)
|
||||
|
||||
#define sq_empty(q) ((q)->head == NULL)
|
||||
#define dq_empty(q) ((q)->head == NULL)
|
||||
|
||||
#define sq_peek(q) ((q)->head)
|
||||
#define dq_peek(q) ((q)->head)
|
||||
|
||||
/************************************************************************
|
||||
* Global Type Declarations
|
||||
************************************************************************/
|
||||
|
||||
struct sq_entry_s
|
||||
{
|
||||
FAR struct sq_entry_s *flink;
|
||||
};
|
||||
typedef struct sq_entry_s sq_entry_t;
|
||||
|
||||
struct dq_entry_s
|
||||
{
|
||||
FAR struct dq_entry_s *flink;
|
||||
FAR struct dq_entry_s *blink;
|
||||
};
|
||||
typedef struct dq_entry_s dq_entry_t;
|
||||
|
||||
struct sq_queue_s
|
||||
{
|
||||
FAR sq_entry_t *head;
|
||||
FAR sq_entry_t *tail;
|
||||
};
|
||||
typedef struct sq_queue_s sq_queue_t;
|
||||
|
||||
struct dq_queue_s
|
||||
{
|
||||
FAR dq_entry_t *head;
|
||||
FAR dq_entry_t *tail;
|
||||
};
|
||||
typedef struct dq_queue_s dq_queue_t;
|
||||
|
||||
/************************************************************************
|
||||
* Global Function Prototypes
|
||||
************************************************************************/
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define EXTERN extern "C"
|
||||
extern "C" {
|
||||
#else
|
||||
#define EXTERN extern
|
||||
#endif
|
||||
|
||||
EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
|
||||
EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
|
||||
EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
|
||||
sq_queue_t *queue);
|
||||
EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
|
||||
dq_queue_t *queue);
|
||||
EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
|
||||
dq_queue_t *queue);
|
||||
|
||||
EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
|
||||
EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
|
||||
EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
|
||||
EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
|
||||
EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
|
||||
|
||||
#undef EXTERN
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __INCLUDE_QUEUE_H_ */
|
||||
|
||||
Reference in New Issue
Block a user