From 290ca1f9bf973a9ef957cb413930f7aac06054e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 10 Jul 2013 21:45:59 +0200 Subject: Reworked airspeed driver to keep most of it abstract --- makefiles/config_px4fmu-v1_default.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'makefiles/config_px4fmu-v1_default.mk') diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 213eb651b..a769eb8f2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -31,7 +31,9 @@ MODULES += drivers/hott_telemetry MODULES += drivers/blinkm MODULES += drivers/mkblctrl MODULES += drivers/md25 +MODULES += drivers/airspeed MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed MODULES += modules/sensors # -- cgit v1.2.3 From 95dde5f1bed21d1a36a065c94c961a8f216a10b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Jul 2013 18:24:37 +0200 Subject: Implemented config command, fixed a number range set / get issues for some sensor drivers, fixed gyro calibration --- makefiles/config_px4fmu-v1_default.mk | 1 + src/drivers/mpu6000/mpu6000.cpp | 44 ++-- src/modules/commander/commander.c | 10 +- src/systemcmds/config/config.c | 413 ++++++++++++++++++++++++++++++++++ src/systemcmds/config/module.mk | 44 ++++ 5 files changed, 496 insertions(+), 16 deletions(-) create mode 100644 src/systemcmds/config/config.c create mode 100644 src/systemcmds/config/module.mk (limited to 'makefiles/config_px4fmu-v1_default.mk') diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 74be1cd23..1d9c0e527 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -50,6 +50,7 @@ MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests +MODULES += systemcmds/config # # General system control diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 8d9054a38..1fd6cb17e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,9 @@ * @file mpu6000.cpp * * Driver for the Invensense MPU6000 connected via SPI. + * + * @author Andrew Tridgell + * @author Pat Hickey */ #include @@ -191,6 +194,7 @@ private: orb_advert_t _gyro_topic; unsigned _reads; + unsigned _sample_rate; perf_counter_t _sample_perf; /** @@ -314,6 +318,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro_range_rad_s(0.0f), _gyro_topic(-1), _reads(0), + _sample_rate(500), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) { // disable debug() calls @@ -366,10 +371,6 @@ MPU6000::init() return ret; } - /* advertise sensor topics */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); - // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -384,7 +385,7 @@ MPU6000::init() // SAMPLE RATE //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(200); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) @@ -463,10 +464,18 @@ MPU6000::init() /* do CDev init for the gyro device node, keep it optional */ int gyro_ret = _gyro->init(); + /* ensure we got real values to share */ + measure(); + if (gyro_ret != OK) { _gyro_topic = -1; + } else { + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); } + /* advertise sensor topics */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); + return ret; } @@ -509,6 +518,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) if(div>200) div=200; if(div<1) div=1; write_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; } /* @@ -660,8 +670,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; - case ACCELIOCSSAMPLERATE: case ACCELIOCGSAMPLERATE: + return _sample_rate; + + case ACCELIOCSSAMPLERATE: _set_sample_rate(arg); return OK; @@ -689,12 +701,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSRANGE: - case ACCELIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_rad_s = 8.0f * 9.81f; + // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; + case ACCELIOCGRANGE: + return _accel_range_m_s2; case ACCELIOCSELFTEST: return self_test(); @@ -718,10 +731,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCRESET: return ioctl(filp, cmd, arg); - case GYROIOCSSAMPLERATE: case GYROIOCGSAMPLERATE: - _set_sample_rate(arg); - return OK; + return _sample_rate; + + case GYROIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: @@ -739,12 +754,13 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: - case GYROIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _gyro_range_scale = xx - // _gyro_range_m_s2 = xx + // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: + return _gyro_range_rad_s; case GYROIOCSELFTEST: return self_test(); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 8e5e36712..e9d1f3954 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -587,6 +587,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(fd); + int errcount = 0; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -602,8 +604,12 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + errcount++; + } + + if (errcount > 1000) { + /* any persisting poll error is a reason to abort */ + mavlink_log_info(mavlink_fd, "permanent gyro error, aborted."); return; } } diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c new file mode 100644 index 000000000..c4b03bbff --- /dev/null +++ b/src/systemcmds/config/config.c @@ -0,0 +1,413 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file config.c + * @author Lorenz Meier + * + * config tool. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +__EXPORT int config_main(int argc, char *argv[]); + +static void do_gyro(int argc, char *argv[]); +static void do_accel(int argc, char *argv[]); +static void do_mag(int argc, char *argv[]); + +int +config_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "gyro")) { + if (argc >= 3) { + do_gyro(argc - 2, argv + 2); + } else { + errx(1, "not enough parameters."); + } + } + + if (!strcmp(argv[1], "accel")) { + if (argc >= 3) { + do_accel(argc - 2, argv + 2); + } else { + errx(1, "not enough parameters."); + } + } + + if (!strcmp(argv[1], "mag")) { + if (argc >= 3) { + do_mag(argc - 2, argv + 2); + } else { + errx(1, "not enough parameters."); + } + } + } + + errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); +} + +static void +do_gyro(int argc, char *argv[]) +{ + int fd; + + fd = open(GYRO_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", GYRO_DEVICE_PATH); + errx(1, "FATAL: no gyro found"); + + } else { + + if (argc >= 2) { + + char* end; + int i = strtol(argv[1],&end,10); + + if (!strcmp(argv[0], "sampling")) { + + /* set the accel internal sampling rate up to at leat i Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, i); + + } else if (!strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, i); + } else if (!strcmp(argv[0], "range")) { + + /* set the range to i dps */ + ioctl(fd, GYROIOCSRANGE, i); + } + + } else if (!(argc > 0 && !strcmp(argv[0], "info"))) { + warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); + } + + int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = ioctl(fd, GYROIOCGRANGE, 0); + + warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + + close(fd); + } + + exit(0); +} + +static void +do_mag(int argc, char *argv[]) +{ + exit(0); +} + +static void +do_accel(int argc, char *argv[]) +{ + int fd; + + fd = open(ACCEL_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", ACCEL_DEVICE_PATH); + errx(1, "FATAL: no accelerometer found"); + + } else { + + if (argc >= 2) { + + char* end; + int i = strtol(argv[1],&end,10); + + if (!strcmp(argv[0], "sampling")) { + + /* set the accel internal sampling rate up to at leat i Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, i); + + } else if (!strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, i); + } else if (!strcmp(argv[0], "range")) { + + /* set the range to i dps */ + ioctl(fd, ACCELIOCSRANGE, i); + } + } else if (!(argc > 0 && !strcmp(argv[0], "info"))) { + warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t"); + } + + int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = ioctl(fd, ACCELIOCGRANGE, 0); + + warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range); + + close(fd); + } + + exit(0); +} + +// static void +// do_load(const char* param_file_name) +// { +// int fd = open(param_file_name, O_RDONLY); + +// if (fd < 0) +// err(1, "open '%s'", param_file_name); + +// int result = param_load(fd); +// close(fd); + +// if (result < 0) { +// errx(1, "error importing from '%s'", param_file_name); +// } + +// exit(0); +// } + +// static void +// do_import(const char* param_file_name) +// { +// int fd = open(param_file_name, O_RDONLY); + +// if (fd < 0) +// err(1, "open '%s'", param_file_name); + +// int result = param_import(fd); +// close(fd); + +// if (result < 0) +// errx(1, "error importing from '%s'", param_file_name); + +// exit(0); +// } + +// static void +// do_show(const char* search_string) +// { +// printf(" + = saved, * = unsaved\n"); +// param_foreach(do_show_print, search_string, false); + +// exit(0); +// } + +// static void +// do_show_print(void *arg, param_t param) +// { +// int32_t i; +// float f; +// const char *search_string = (const char*)arg; + +// /* print nothing if search string is invalid and not matching */ +// if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { +// /* param not found */ +// return; +// } + +// printf("%c %s: ", +// param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), +// param_name(param)); + +// /* +// * This case can be expanded to handle printing common structure types. +// */ + +// switch (param_type(param)) { +// case PARAM_TYPE_INT32: +// if (!param_get(param, &i)) { +// printf("%d\n", i); +// return; +// } + +// break; + +// case PARAM_TYPE_FLOAT: +// if (!param_get(param, &f)) { +// printf("%4.4f\n", (double)f); +// return; +// } + +// break; + +// case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: +// printf("\n", 0 + param_type(param), param_size(param)); +// return; + +// default: +// printf("\n", 0 + param_type(param)); +// return; +// } + +// printf("\n", param); +// } + +// static void +// do_set(const char* name, const char* val) +// { +// int32_t i; +// float f; +// param_t param = param_find(name); + +// /* set nothing if parameter cannot be found */ +// if (param == PARAM_INVALID) { +// param not found +// errx(1, "Error: Parameter %s not found.", name); +// } + +// printf("%c %s: ", +// param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), +// param_name(param)); + +// /* +// * Set parameter if type is known and conversion from string to value turns out fine +// */ + +// switch (param_type(param)) { +// case PARAM_TYPE_INT32: +// if (!param_get(param, &i)) { +// printf("curr: %d", i); + +// /* convert string */ +// char* end; +// i = strtol(val,&end,10); +// param_set(param, &i); +// printf(" -> new: %d\n", i); + +// } + +// break; + +// case PARAM_TYPE_FLOAT: +// if (!param_get(param, &f)) { +// printf("curr: %4.4f", (double)f); + +// /* convert string */ +// char* end; +// f = strtod(val,&end); +// param_set(param, &f); +// printf(" -> new: %f\n", f); + +// } + +// break; + +// default: +// errx(1, "\n", 0 + param_type(param)); +// } + +// exit(0); +// } + +// static void +// do_compare(const char* name, const char* val) +// { +// int32_t i; +// float f; +// param_t param = param_find(name); + +// /* set nothing if parameter cannot be found */ +// if (param == PARAM_INVALID) { +// /* param not found */ +// errx(1, "Error: Parameter %s not found.", name); +// } + +// /* +// * Set parameter if type is known and conversion from string to value turns out fine +// */ + +// int ret = 1; + +// switch (param_type(param)) { +// case PARAM_TYPE_INT32: +// if (!param_get(param, &i)) { + +// /* convert string */ +// char* end; +// int j = strtol(val,&end,10); +// if (i == j) { +// printf(" %d: ", i); +// ret = 0; +// } + +// } + +// break; + +// case PARAM_TYPE_FLOAT: +// if (!param_get(param, &f)) { + +// /* convert string */ +// char* end; +// float g = strtod(val, &end); +// if (fabsf(f - g) < 1e-7f) { +// printf(" %4.4f: ", (double)f); +// ret = 0; +// } +// } + +// break; + +// default: +// errx(1, "\n", 0 + param_type(param)); +// } + +// if (ret == 0) { +// printf("%c %s: equal\n", +// param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), +// param_name(param)); +// } + +// exit(ret); +// } diff --git a/src/systemcmds/config/module.mk b/src/systemcmds/config/module.mk new file mode 100644 index 000000000..0a75810b0 --- /dev/null +++ b/src/systemcmds/config/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# Build the config tool. +# + +MODULE_COMMAND = config +SRCS = config.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os + -- cgit v1.2.3 From 1980d9dd63e29390f7c3ba9b31be576c07706f73 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 01:35:43 -0400 Subject: Working on segway controller, restructure of fixedwing. --- makefiles/config_px4fmu-v1_default.mk | 1 + src/drivers/md25/md25.cpp | 2 +- src/drivers/md25/md25.hpp | 2 +- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 4 +- src/modules/controllib/block/Block.cpp | 4 +- src/modules/controllib/block/UOrbPublication.cpp | 39 --- src/modules/controllib/block/UOrbPublication.hpp | 118 ------- src/modules/controllib/block/UOrbSubscription.cpp | 51 --- src/modules/controllib/block/UOrbSubscription.hpp | 137 -------- src/modules/controllib/blocks.hpp | 1 + src/modules/controllib/fixedwing.cpp | 379 --------------------- src/modules/controllib/fixedwing.hpp | 344 ------------------- src/modules/controllib/module.mk | 8 +- src/modules/controllib/uorb/UOrbPublication.cpp | 39 +++ src/modules/controllib/uorb/UOrbPublication.hpp | 118 +++++++ src/modules/controllib/uorb/UOrbSubscription.cpp | 51 +++ src/modules/controllib/uorb/UOrbSubscription.hpp | 137 ++++++++ src/modules/controllib/uorb/blocks.cpp | 64 ++++ src/modules/controllib/uorb/blocks.hpp | 90 +++++ src/modules/fixedwing_backside/fixedwing.cpp | 361 ++++++++++++++++++++ src/modules/fixedwing_backside/fixedwing.hpp | 303 ++++++++++++++++ .../fixedwing_backside/fixedwing_backside_main.cpp | 3 +- src/modules/fixedwing_backside/module.mk | 1 + src/modules/segway/BlockSegwayController.cpp | 56 +++ src/modules/segway/BlockSegwayController.hpp | 18 + src/modules/segway/module.mk | 42 +++ src/modules/segway/params.c | 72 ++++ src/modules/segway/segway_main.cpp | 173 ++++++++++ 28 files changed, 1539 insertions(+), 1079 deletions(-) delete mode 100644 src/modules/controllib/block/UOrbPublication.cpp delete mode 100644 src/modules/controllib/block/UOrbPublication.hpp delete mode 100644 src/modules/controllib/block/UOrbSubscription.cpp delete mode 100644 src/modules/controllib/block/UOrbSubscription.hpp delete mode 100644 src/modules/controllib/fixedwing.cpp delete mode 100644 src/modules/controllib/fixedwing.hpp create mode 100644 src/modules/controllib/uorb/UOrbPublication.cpp create mode 100644 src/modules/controllib/uorb/UOrbPublication.hpp create mode 100644 src/modules/controllib/uorb/UOrbSubscription.cpp create mode 100644 src/modules/controllib/uorb/UOrbSubscription.hpp create mode 100644 src/modules/controllib/uorb/blocks.cpp create mode 100644 src/modules/controllib/uorb/blocks.hpp create mode 100644 src/modules/fixedwing_backside/fixedwing.cpp create mode 100644 src/modules/fixedwing_backside/fixedwing.hpp create mode 100644 src/modules/segway/BlockSegwayController.cpp create mode 100644 src/modules/segway/BlockSegwayController.hpp create mode 100644 src/modules/segway/module.mk create mode 100644 src/modules/segway/params.c create mode 100644 src/modules/segway/segway_main.cpp (limited to 'makefiles/config_px4fmu-v1_default.mk') diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 74be1cd23..cbca4f6a6 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -71,6 +71,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # +MODULES += modules/segway MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 1079005c0..d6dd64a09 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 2fc317f5e..780978514 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 49d0d157d..f01ee0355 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include #include #include -#include -#include +#include +#include #include #include diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 5994d2315..b964d40a3 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include "../uorb/UOrbSubscription.hpp" +#include "../uorb/UOrbPublication.hpp" namespace control { diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/block/UOrbPublication.cpp deleted file mode 100644 index f69b39d90..000000000 --- a/src/modules/controllib/block/UOrbPublication.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * 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 PX4 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. - * - ****************************************************************************/ - -/** - * @file UOrbPublication.cpp - * - */ - -#include "UOrbPublication.hpp" diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/block/UOrbPublication.hpp deleted file mode 100644 index 0a8ae2ff7..000000000 --- a/src/modules/controllib/block/UOrbPublication.hpp +++ /dev/null @@ -1,118 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * 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 PX4 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. - * - ****************************************************************************/ - -/** - * @file UOrbPublication.h - * - */ - -#pragma once - -#include -#include "Block.hpp" -#include "List.hpp" - - -namespace control -{ - -class Block; - -/** - * Base publication warapper class, used in list traversal - * of various publications. - */ -class __EXPORT UOrbPublicationBase : public ListNode -{ -public: - - UOrbPublicationBase( - List * list, - const struct orb_metadata *meta) : - _meta(meta), - _handle(-1) { - if (list != NULL) list->add(this); - } - void update() { - if (_handle > 0) { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); - } else { - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); - } - } - virtual void *getDataVoidPtr() = 0; - virtual ~UOrbPublicationBase() { - orb_unsubscribe(getHandle()); - } - const struct orb_metadata *getMeta() { return _meta; } - int getHandle() { return _handle; } -protected: - void setHandle(orb_advert_t handle) { _handle = handle; } - const struct orb_metadata *_meta; - orb_advert_t _handle; -}; - -/** - * UOrb Publication wrapper class - */ -template -class UOrbPublication : - public T, // this must be first! - public UOrbPublicationBase -{ -public: - /** - * Constructor - * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - */ - UOrbPublication( - List * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - UOrbPublicationBase(list, meta) { - } - virtual ~UOrbPublication() {} - /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr() { return (void *)(T *)(this); } -}; - -} // namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/block/UOrbSubscription.cpp deleted file mode 100644 index 022cadd24..000000000 --- a/src/modules/controllib/block/UOrbSubscription.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * 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 PX4 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. - * - ****************************************************************************/ - -/** - * @file UOrbSubscription.cpp - * - */ - -#include "UOrbSubscription.hpp" - -namespace control -{ - -bool __EXPORT UOrbSubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} - -} // namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/block/UOrbSubscription.hpp deleted file mode 100644 index 22cc2e114..000000000 --- a/src/modules/controllib/block/UOrbSubscription.hpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * 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 PX4 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. - * - ****************************************************************************/ - -/** - * @file UOrbSubscription.h - * - */ - -#pragma once - -#include -#include "Block.hpp" -#include "List.hpp" - - -namespace control -{ - -class Block; - -/** - * Base subscription warapper class, used in list traversal - * of various subscriptions. - */ -class __EXPORT UOrbSubscriptionBase : - public ListNode -{ -public: -// methods - - /** - * Constructor - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - */ - UOrbSubscriptionBase( - List * list, - const struct orb_metadata *meta) : - _meta(meta), - _handle() { - if (list != NULL) list->add(this); - } - bool updated(); - void update() { - if (updated()) { - orb_copy(_meta, _handle, getDataVoidPtr()); - } - } - virtual void *getDataVoidPtr() = 0; - virtual ~UOrbSubscriptionBase() { - orb_unsubscribe(_handle); - } -// accessors - const struct orb_metadata *getMeta() { return _meta; } - int getHandle() { return _handle; } -protected: -// accessors - void setHandle(int handle) { _handle = handle; } -// attributes - const struct orb_metadata *_meta; - int _handle; -}; - -/** - * UOrb Subscription wrapper class - */ -template -class __EXPORT UOrbSubscription : - public T, // this must be first! - public UOrbSubscriptionBase -{ -public: - /** - * Constructor - * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param interval The minimum interval in milliseconds between updates - */ - UOrbSubscription( - List * list, - const struct orb_metadata *meta, unsigned interval) : - T(), // initialize data structure to zero - UOrbSubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } - - /** - * Deconstructor - */ - virtual ~UOrbSubscription() {} - - /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr() { return (void *)(T *)(this); } - T getData() { return T(*this); } -}; - -} // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 7a785d12e..fefe99702 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include "block/Block.hpp" diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp deleted file mode 100644 index 77b2ac806..000000000 --- a/src/modules/controllib/fixedwing.cpp +++ /dev/null @@ -1,379 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * 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 PX4 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. - * - ****************************************************************************/ - -/** - * @file fixedwing.cpp - * - * Controller library code - */ - -#include "fixedwing.hpp" - -namespace control -{ - -namespace fixedwing -{ - -BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _rLowPass(this, "R_LP"), - _rWashout(this, "R_HP"), - _r2Rdr(this, "R2RDR"), - _rudder(0) -{ -} - -BlockYawDamper::~BlockYawDamper() {}; - -void BlockYawDamper::update(float rCmd, float r, float outputScale) -{ - _rudder = outputScale*_r2Rdr.update(rCmd - - _rWashout.update(_rLowPass.update(r))); -} - -BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _yawDamper(this, ""), - _pLowPass(this, "P_LP"), - _qLowPass(this, "Q_LP"), - _p2Ail(this, "P2AIL"), - _q2Elv(this, "Q2ELV"), - _aileron(0), - _elevator(0) -{ -} - -BlockStabilization::~BlockStabilization() {}; - -void BlockStabilization::update(float pCmd, float qCmd, float rCmd, - float p, float q, float r, float outputScale) -{ - _aileron = outputScale*_p2Ail.update( - pCmd - _pLowPass.update(p)); - _elevator = outputScale*_q2Elv.update( - qCmd - _qLowPass.update(q)); - _yawDamper.update(rCmd, r, outputScale); -} - -BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _xtYawLimit(this, "XT2YAW"), - _xt2Yaw(this, "XT2YAW"), - _psiCmd(0) -{ -} - -BlockWaypointGuidance::~BlockWaypointGuidance() {}; - -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) -{ - - // heading to waypoint - float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - // cross track - struct crosstrack_error_s xtrackError; - get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - _psiCmd = _wrap_2pi(psiTrack - - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); -} - -BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) -{ -} - -BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; - -BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : - BlockUorbEnabledAutopilot(parent, name), - _stabilization(this, ""), // no name needed, already unique - - // heading hold - _psi2Phi(this, "PSI2PHI"), - _phi2P(this, "PHI2P"), - _phiLimit(this, "PHI_LIM"), - - // velocity hold - _v2Theta(this, "V2THE"), - _theta2Q(this, "THE2Q"), - _theLimit(this, "THE"), - _vLimit(this, "V"), - - // altitude/climb rate hold - _h2Thr(this, "H2THR"), - _cr2Thr(this, "CR2THR"), - - // guidance block - _guide(this, ""), - - _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ - _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ - _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ - _trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ - _trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */ - - _vCmd(this, "V_CMD"), - _crMax(this, "CR_MAX"), - _attPoll(), - _lastPosCmd(), - _timeStamp(0) -{ - _attPoll.fd = _att.getHandle(); - _attPoll.events = POLLIN; -} - -void BlockMultiModeBacksideAutopilot::update() -{ - // wait for a sensor update, check for exit condition every 100 ms - if (poll(&_attPoll, 1, 100) < 0) return; // poll error - - uint64_t newTimeStamp = hrt_absolute_time(); - float dt = (newTimeStamp - _timeStamp) / 1.0e6f; - _timeStamp = newTimeStamp; - - // check for sane values of dt - // to prevent large control responses - if (dt > 1.0f || dt < 0) return; - - // set dt for all child blocks - setDt(dt); - - // store old position command before update if new command sent - if (_posCmd.updated()) { - _lastPosCmd = _posCmd.getData(); - } - - // check for new updates - if (_param_update.updated()) updateParams(); - - // get new information from subscriptions - updateSubscriptions(); - - // default all output to zero unless handled by mode - for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) - _actuators.control[i] = 0.0f; - - // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { - // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); - } - - // XXX handle STABILIZED (loiter on spot) as well - // once the system switches from manual or auto to stabilized - // the setpoint should update to loitering around this position - - // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { - - // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between - // the min/max velocity - float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + - _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); - - // limit velocity command between min/max velocity - float vCmd = _vLimit.update(_vCmd.get()); - - // altitude hold - float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); - - // heading hold - float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); - float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); - float pCmd = _phi2P.update(phiCmd - _att.roll); - - // velocity hold - // negative sign because nose over to increase speed - float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - - // yaw rate cmd - float rCmd = 0; - - // stabilization - float velocityRatio = _trimV.get()/v; - float outputScale = velocityRatio*velocityRatio; - // this term scales the output based on the dynamic pressure change from trim - _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed, - outputScale); - - // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - _actuators.control[CH_THR] = dThrottle + _trimThr.get(); - - // XXX limit throttle to manual setting (safety) for now. - // If it turns out to be confusing, it can be removed later once - // a first binary release can be targeted. - // This is not a hack, but a design choice. - - /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } - - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; - - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between - // the min/max velocity - float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + - _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); - - // pitch channel -> rate of climb - // TODO, might want to put a gain on this, otherwise commanding - // from +1 -> -1 m/s for rate of climb - //float dThrottle = _cr2Thr.update( - //_crMax.get()*_manual.pitch - _pos.vz); - - // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); - - // throttle channel -> velocity - // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); - float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - - // yaw rate cmd - float rCmd = 0; - - // stabilization - _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - - // currently using manual throttle - // XXX if you enable this watch out, vz might be very noisy - //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; - - // XXX limit throttle to manual setting (safety) for now. - // If it turns out to be confusing, it can be removed later once - // a first binary release can be targeted. - // This is not a hack, but a design choice. - - /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } - } - - // body rates controller, disabled for now - else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { - - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } - } - - // update all publications - updatePublications(); -} - -BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() -{ - // send one last publication when destroyed, setting - // all output to zero - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - _actuators.control[i] = 0.0f; - - updatePublications(); -} - -} // namespace fixedwing - -} // namespace control - diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp deleted file mode 100644 index e4028c40d..000000000 --- a/src/modules/controllib/fixedwing.hpp +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * 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 PX4 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. - * - ****************************************************************************/ - -/** - * @file fixedwing.h - * - * Controller library code - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "blocks.hpp" -#include "block/UOrbSubscription.hpp" -#include "block/UOrbPublication.hpp" - -extern "C" { -#include -} - -namespace control -{ - -namespace fixedwing -{ - -/** - * BlockYawDamper - * - * This block has more explations to help new developers - * add their own blocks. It includes a limited explanation - * of some C++ basics. - * - * Block: The generic class describing a typical block as you - * would expect in Simulink or ScicosLab. A block can have - * parameters. It cannot have other blocks. - * - * SuperBlock: A superblock is a block that can have other - * blocks. It has methods that manage the blocks beneath it. - * - * BlockYawDamper inherits from SuperBlock publically, this - * means that any public function in SuperBlock are public within - * BlockYawDamper and may be called from outside the - * class methods. Any protected function within block - * are private to the class and may not be called from - * outside this class. Protected should be preferred - * where possible to public as it is important to - * limit access to the bare minimum to prevent - * accidental errors. - */ -class __EXPORT BlockYawDamper : public SuperBlock -{ -private: - /** - * Declaring other blocks used by this block - * - * In this section we declare all child blocks that - * this block is composed of. They are private - * members so only this block has direct access to - * them. - */ - BlockLowPass _rLowPass; - BlockHighPass _rWashout; - BlockP _r2Rdr; - - /** - * Declaring output values and accessors - * - * If we have any output values for the block we - * declare them here. Output can be directly returned - * through the update function, but outputs should be - * declared here if the information will likely be requested - * again, or if there are multiple outputs. - * - * You should only be able to set outputs from this block, - * so the outputs are declared in the private section. - * A get accessor is provided - * in the public section for other blocks to get the - * value of the output. - */ - float _rudder; -public: - /** - * BlockYawDamper Constructor - * - * The job of the constructor is to initialize all - * parameter in this block and initialize all child - * blocks. Note also, that the output values must be - * initialized here. The order of the members in the - * member initialization list should follow the - * order in which they are declared within the class. - * See the private member declarations above. - * - * Block Construction - * - * All blocks are constructed with their parent block - * and their name. This allows parameters within the - * block to construct a fully qualified name from - * concatenating the two. If the name provided to the - * block is "", then the block will use the parent - * name as it's name. This is useful in cases where - * you have a block that has parameters "MIN", "MAX", - * such as BlockLimit and you do not want an extra name - * to qualify them since the parent block has no "MIN", - * "MAX" parameters. - * - * Block Parameter Construction - * - * Block parameters are named constants, they are - * constructed using: - * BlockParam::BlockParam(Block * parent, const char * name) - * This funciton takes both a parent block and a name. - * The constructore then uses the parent name and the name of - * the paramter to ask the px4 param library if it has any - * parameters with this name. If it does, a handle to the - * parameter is retrieved. - * - * Block/ BlockParam Naming - * - * When desigining new blocks, the naming of the parameters and - * blocks determines the fully qualified name of the parameters - * within the ground station, so it is important to choose - * short, easily understandable names. Again, when a name of - * "" is passed, the parent block name is used as the value to - * prepend to paramters names. - */ - BlockYawDamper(SuperBlock *parent, const char *name); - /** - * Block deconstructor - * - * It is always a good idea to declare a virtual - * deconstructor so that upon calling delete from - * a class derived from this, all of the - * deconstructors all called, the derived class first, and - * then the base class - */ - virtual ~BlockYawDamper(); - - /** - * Block update function - * - * The job of the update function is to compute the output - * values for the block. In a simple block with one output, - * the output may be returned directly. If the output is - * required frequenly by other processses, it might be a - * good idea to declare a member to store the temporary - * variable. - */ - void update(float rCmd, float r, float outputScale = 1.0); - - /** - * Rudder output value accessor - * - * This is a public accessor function, which means that the - * private value _rudder is returned to anyone calling - * BlockYawDamper::getRudder(). Note thate a setRudder() is - * not provided, this is because the updateParams() call - * for a block provides the mechanism for updating the - * paramter. - */ - float getRudder() { return _rudder; } -}; - -/** - * Stability augmentation system. - * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299 - */ -class __EXPORT BlockStabilization : public SuperBlock -{ -private: - BlockYawDamper _yawDamper; - BlockLowPass _pLowPass; - BlockLowPass _qLowPass; - BlockP _p2Ail; - BlockP _q2Elv; - float _aileron; - float _elevator; -public: - BlockStabilization(SuperBlock *parent, const char *name); - virtual ~BlockStabilization(); - void update(float pCmd, float qCmd, float rCmd, - float p, float q, float r, - float outputScale = 1.0); - float getAileron() { return _aileron; } - float getElevator() { return _elevator; } - float getRudder() { return _yawDamper.getRudder(); } -}; - -/** - * Frontside/ Backside Control Systems - * - * Frontside : - * velocity error -> throttle - * altitude error -> elevator - * - * Backside : - * velocity error -> elevator - * altitude error -> throttle - * - * Backside control systems are more resilient at - * slow speeds on the back-side of the power - * required curve/ landing etc. Less performance - * than frontside at high speeds. - */ - -/** - * Waypoint Guidance block - */ -class __EXPORT BlockWaypointGuidance : public SuperBlock -{ -private: - BlockLimitSym _xtYawLimit; - BlockP _xt2Yaw; - float _psiCmd; -public: - BlockWaypointGuidance(SuperBlock *parent, const char *name); - virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); - float getPsiCmd() { return _psiCmd; } -}; - -/** - * UorbEnabledAutopilot - */ -class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock -{ -protected: - // subscriptions - UOrbSubscription _att; - UOrbSubscription _attCmd; - UOrbSubscription _ratesCmd; - UOrbSubscription _pos; - UOrbSubscription _posCmd; - UOrbSubscription _manual; - UOrbSubscription _status; - UOrbSubscription _param_update; - // publications - UOrbPublication _actuators; -public: - BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); - virtual ~BlockUorbEnabledAutopilot(); -}; - -/** - * Multi-mode Autopilot - */ -class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot -{ -private: - // stabilization - BlockStabilization _stabilization; - - // heading hold - BlockP _psi2Phi; - BlockP _phi2P; - BlockLimitSym _phiLimit; - - // velocity hold - BlockPID _v2Theta; - BlockPID _theta2Q; - BlockLimit _theLimit; - BlockLimit _vLimit; - - // altitude/ climb rate hold - BlockPID _h2Thr; - BlockPID _cr2Thr; - - // guidance - BlockWaypointGuidance _guide; - - // block params - BlockParam _trimAil; - BlockParam _trimElv; - BlockParam _trimRdr; - BlockParam _trimThr; - BlockParam _trimV; - BlockParam _vCmd; - BlockParam _crMax; - - struct pollfd _attPoll; - vehicle_global_position_set_triplet_s _lastPosCmd; - enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; - uint64_t _timeStamp; -public: - BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name); - void update(); - virtual ~BlockMultiModeBacksideAutopilot(); -}; - - -} // namespace fixedwing - -} // namespace control - diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index 13d1069c7..d815a8feb 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,7 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - block/UOrbPublication.cpp \ - block/UOrbSubscription.cpp \ - blocks.cpp \ - fixedwing.cpp + uorb/UOrbPublication.cpp \ + uorb/UOrbSubscription.cpp \ + uorb/blocks.cpp \ + blocks.cpp diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp new file mode 100644 index 000000000..f69b39d90 --- /dev/null +++ b/src/modules/controllib/uorb/UOrbPublication.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file UOrbPublication.cpp + * + */ + +#include "UOrbPublication.hpp" diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp new file mode 100644 index 000000000..6f1f3fc1c --- /dev/null +++ b/src/modules/controllib/uorb/UOrbPublication.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file UOrbPublication.h + * + */ + +#pragma once + +#include +#include "../block/Block.hpp" +#include "../block/List.hpp" + + +namespace control +{ + +class Block; + +/** + * Base publication warapper class, used in list traversal + * of various publications. + */ +class __EXPORT UOrbPublicationBase : public ListNode +{ +public: + + UOrbPublicationBase( + List * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle(-1) { + if (list != NULL) list->add(this); + } + void update() { + if (_handle > 0) { + orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + } else { + setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~UOrbPublicationBase() { + orb_unsubscribe(getHandle()); + } + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: + void setHandle(orb_advert_t handle) { _handle = handle; } + const struct orb_metadata *_meta; + orb_advert_t _handle; +}; + +/** + * UOrb Publication wrapper class + */ +template +class UOrbPublication : + public T, // this must be first! + public UOrbPublicationBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + UOrbPublication( + List * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + UOrbPublicationBase(list, meta) { + } + virtual ~UOrbPublication() {} + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } +}; + +} // namespace control diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp new file mode 100644 index 000000000..022cadd24 --- /dev/null +++ b/src/modules/controllib/uorb/UOrbSubscription.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file UOrbSubscription.cpp + * + */ + +#include "UOrbSubscription.hpp" + +namespace control +{ + +bool __EXPORT UOrbSubscriptionBase::updated() +{ + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; +} + +} // namespace control diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp new file mode 100644 index 000000000..d337d89a8 --- /dev/null +++ b/src/modules/controllib/uorb/UOrbSubscription.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file UOrbSubscription.h + * + */ + +#pragma once + +#include +#include "../block/Block.hpp" +#include "../block/List.hpp" + + +namespace control +{ + +class Block; + +/** + * Base subscription warapper class, used in list traversal + * of various subscriptions. + */ +class __EXPORT UOrbSubscriptionBase : + public ListNode +{ +public: +// methods + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + UOrbSubscriptionBase( + List * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle() { + if (list != NULL) list->add(this); + } + bool updated(); + void update() { + if (updated()) { + orb_copy(_meta, _handle, getDataVoidPtr()); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~UOrbSubscriptionBase() { + orb_unsubscribe(_handle); + } +// accessors + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: +// accessors + void setHandle(int handle) { _handle = handle; } +// attributes + const struct orb_metadata *_meta; + int _handle; +}; + +/** + * UOrb Subscription wrapper class + */ +template +class __EXPORT UOrbSubscription : + public T, // this must be first! + public UOrbSubscriptionBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param interval The minimum interval in milliseconds between updates + */ + UOrbSubscription( + List * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + UOrbSubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); + } + + /** + * Deconstructor + */ + virtual ~UOrbSubscription() {} + + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } + T getData() { return T(*this); } +}; + +} // namespace control diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp new file mode 100644 index 000000000..6e5ade519 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.cpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file uorb_blocks.cpp + * + * uorb block library code + */ + +#include "blocks.hpp" + +namespace control +{ + +BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), + _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), + _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), + _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), + _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _actuators(&getPublications(), ORB_ID(actuator_controls_0)) +{ +} + +BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; + +} // namespace control + diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp new file mode 100644 index 000000000..54f31735c --- /dev/null +++ b/src/modules/controllib/uorb/blocks.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file uorb_blocks.h + * + * uorb block library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../blocks.hpp" +#include "UOrbSubscription.hpp" +#include "UOrbPublication.hpp" + +namespace control +{ + +/** + * UorbEnabledAutopilot + */ +class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock +{ +protected: + // subscriptions + UOrbSubscription _att; + UOrbSubscription _attCmd; + UOrbSubscription _ratesCmd; + UOrbSubscription _pos; + UOrbSubscription _posCmd; + UOrbSubscription _manual; + UOrbSubscription _status; + UOrbSubscription _param_update; + // publications + UOrbPublication _actuators; +public: + BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); + virtual ~BlockUorbEnabledAutopilot(); +}; + +} // namespace control + diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp new file mode 100644 index 000000000..d5dc746e0 --- /dev/null +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file fixedwing.cpp + * + * Controller library code + */ + +#include "fixedwing.hpp" + +namespace control +{ + +namespace fixedwing +{ + +BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _rLowPass(this, "R_LP"), + _rWashout(this, "R_HP"), + _r2Rdr(this, "R2RDR"), + _rudder(0) +{ +} + +BlockYawDamper::~BlockYawDamper() {}; + +void BlockYawDamper::update(float rCmd, float r, float outputScale) +{ + _rudder = outputScale*_r2Rdr.update(rCmd - + _rWashout.update(_rLowPass.update(r))); +} + +BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _yawDamper(this, ""), + _pLowPass(this, "P_LP"), + _qLowPass(this, "Q_LP"), + _p2Ail(this, "P2AIL"), + _q2Elv(this, "Q2ELV"), + _aileron(0), + _elevator(0) +{ +} + +BlockStabilization::~BlockStabilization() {}; + +void BlockStabilization::update(float pCmd, float qCmd, float rCmd, + float p, float q, float r, float outputScale) +{ + _aileron = outputScale*_p2Ail.update( + pCmd - _pLowPass.update(p)); + _elevator = outputScale*_q2Elv.update( + qCmd - _qLowPass.update(q)); + _yawDamper.update(rCmd, r, outputScale); +} + +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + +BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : + BlockUorbEnabledAutopilot(parent, name), + _stabilization(this, ""), // no name needed, already unique + + // heading hold + _psi2Phi(this, "PSI2PHI"), + _phi2P(this, "PHI2P"), + _phiLimit(this, "PHI_LIM"), + + // velocity hold + _v2Theta(this, "V2THE"), + _theta2Q(this, "THE2Q"), + _theLimit(this, "THE"), + _vLimit(this, "V"), + + // altitude/climb rate hold + _h2Thr(this, "H2THR"), + _cr2Thr(this, "CR2THR"), + + // guidance block + _guide(this, ""), + + _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ + _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ + _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ + _trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ + _trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */ + + _vCmd(this, "V_CMD"), + _crMax(this, "CR_MAX"), + _attPoll(), + _lastPosCmd(), + _timeStamp(0) +{ + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; +} + +void BlockMultiModeBacksideAutopilot::update() +{ + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // store old position command before update if new command sent + if (_posCmd.updated()) { + _lastPosCmd = _posCmd.getData(); + } + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); + } + + // XXX handle STABILIZED (loiter on spot) as well + // once the system switches from manual or auto to stabilized + // the setpoint should update to loitering around this position + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + + // update guidance + _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); + + // calculate velocity, XXX should be airspeed, but using ground speed for now + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // limit velocity command between min/max velocity + float vCmd = _vLimit.update(_vCmd.get()); + + // altitude hold + float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); + + // heading hold + float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); + float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); + float pCmd = _phi2P.update(phiCmd - _att.roll); + + // velocity hold + // negative sign because nose over to increase speed + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + + // yaw rate cmd + float rCmd = 0; + + // stabilization + float velocityRatio = _trimV.get()/v; + float outputScale = velocityRatio*velocityRatio; + // this term scales the output based on the dynamic pressure change from trim + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed, + outputScale); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.control[CH_THR] = dThrottle + _trimThr.get(); + + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (!_status.flag_hil_enabled) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; + } + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + + // calculate velocity, XXX should be airspeed, but using ground speed for now + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // pitch channel -> rate of climb + // TODO, might want to put a gain on this, otherwise commanding + // from +1 -> -1 m/s for rate of climb + //float dThrottle = _cr2Thr.update( + //_crMax.get()*_manual.pitch - _pos.vz); + + // roll channel -> bank angle + float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.roll); + + // throttle channel -> velocity + // negative sign because nose over to increase speed + float vCmd = _vLimit.update(_manual.throttle * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + + // yaw rate cmd + float rCmd = 0; + + // stabilization + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + + // currently using manual throttle + // XXX if you enable this watch out, vz might be very noisy + //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.control[CH_THR] = _manual.throttle; + + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (!_status.flag_hil_enabled) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; + } + } + + // body rates controller, disabled for now + else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { + + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; + } + } + + // update all publications + updatePublications(); +} + +BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() +{ + // send one last publication when destroyed, setting + // all output to zero + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + updatePublications(); +} + +} // namespace fixedwing + +} // namespace control + diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp new file mode 100644 index 000000000..eb5d38381 --- /dev/null +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -0,0 +1,303 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file fixedwing.h + * + * Controller library code + */ + +#pragma once + +#include +#include + +extern "C" { +#include +} + +namespace control +{ + +namespace fixedwing +{ + +/** + * BlockYawDamper + * + * This block has more explations to help new developers + * add their own blocks. It includes a limited explanation + * of some C++ basics. + * + * Block: The generic class describing a typical block as you + * would expect in Simulink or ScicosLab. A block can have + * parameters. It cannot have other blocks. + * + * SuperBlock: A superblock is a block that can have other + * blocks. It has methods that manage the blocks beneath it. + * + * BlockYawDamper inherits from SuperBlock publically, this + * means that any public function in SuperBlock are public within + * BlockYawDamper and may be called from outside the + * class methods. Any protected function within block + * are private to the class and may not be called from + * outside this class. Protected should be preferred + * where possible to public as it is important to + * limit access to the bare minimum to prevent + * accidental errors. + */ +class __EXPORT BlockYawDamper : public SuperBlock +{ +private: + /** + * Declaring other blocks used by this block + * + * In this section we declare all child blocks that + * this block is composed of. They are private + * members so only this block has direct access to + * them. + */ + BlockLowPass _rLowPass; + BlockHighPass _rWashout; + BlockP _r2Rdr; + + /** + * Declaring output values and accessors + * + * If we have any output values for the block we + * declare them here. Output can be directly returned + * through the update function, but outputs should be + * declared here if the information will likely be requested + * again, or if there are multiple outputs. + * + * You should only be able to set outputs from this block, + * so the outputs are declared in the private section. + * A get accessor is provided + * in the public section for other blocks to get the + * value of the output. + */ + float _rudder; +public: + /** + * BlockYawDamper Constructor + * + * The job of the constructor is to initialize all + * parameter in this block and initialize all child + * blocks. Note also, that the output values must be + * initialized here. The order of the members in the + * member initialization list should follow the + * order in which they are declared within the class. + * See the private member declarations above. + * + * Block Construction + * + * All blocks are constructed with their parent block + * and their name. This allows parameters within the + * block to construct a fully qualified name from + * concatenating the two. If the name provided to the + * block is "", then the block will use the parent + * name as it's name. This is useful in cases where + * you have a block that has parameters "MIN", "MAX", + * such as BlockLimit and you do not want an extra name + * to qualify them since the parent block has no "MIN", + * "MAX" parameters. + * + * Block Parameter Construction + * + * Block parameters are named constants, they are + * constructed using: + * BlockParam::BlockParam(Block * parent, const char * name) + * This funciton takes both a parent block and a name. + * The constructore then uses the parent name and the name of + * the paramter to ask the px4 param library if it has any + * parameters with this name. If it does, a handle to the + * parameter is retrieved. + * + * Block/ BlockParam Naming + * + * When desigining new blocks, the naming of the parameters and + * blocks determines the fully qualified name of the parameters + * within the ground station, so it is important to choose + * short, easily understandable names. Again, when a name of + * "" is passed, the parent block name is used as the value to + * prepend to paramters names. + */ + BlockYawDamper(SuperBlock *parent, const char *name); + /** + * Block deconstructor + * + * It is always a good idea to declare a virtual + * deconstructor so that upon calling delete from + * a class derived from this, all of the + * deconstructors all called, the derived class first, and + * then the base class + */ + virtual ~BlockYawDamper(); + + /** + * Block update function + * + * The job of the update function is to compute the output + * values for the block. In a simple block with one output, + * the output may be returned directly. If the output is + * required frequenly by other processses, it might be a + * good idea to declare a member to store the temporary + * variable. + */ + void update(float rCmd, float r, float outputScale = 1.0); + + /** + * Rudder output value accessor + * + * This is a public accessor function, which means that the + * private value _rudder is returned to anyone calling + * BlockYawDamper::getRudder(). Note thate a setRudder() is + * not provided, this is because the updateParams() call + * for a block provides the mechanism for updating the + * paramter. + */ + float getRudder() { return _rudder; } +}; + +/** + * Stability augmentation system. + * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299 + */ +class __EXPORT BlockStabilization : public SuperBlock +{ +private: + BlockYawDamper _yawDamper; + BlockLowPass _pLowPass; + BlockLowPass _qLowPass; + BlockP _p2Ail; + BlockP _q2Elv; + float _aileron; + float _elevator; +public: + BlockStabilization(SuperBlock *parent, const char *name); + virtual ~BlockStabilization(); + void update(float pCmd, float qCmd, float rCmd, + float p, float q, float r, + float outputScale = 1.0); + float getAileron() { return _aileron; } + float getElevator() { return _elevator; } + float getRudder() { return _yawDamper.getRudder(); } +}; + +/** + * Frontside/ Backside Control Systems + * + * Frontside : + * velocity error -> throttle + * altitude error -> elevator + * + * Backside : + * velocity error -> elevator + * altitude error -> throttle + * + * Backside control systems are more resilient at + * slow speeds on the back-side of the power + * required curve/ landing etc. Less performance + * than frontside at high speeds. + */ + +/** + * Waypoint Guidance block + */ +class __EXPORT BlockWaypointGuidance : public SuperBlock +{ +private: + BlockLimitSym _xtYawLimit; + BlockP _xt2Yaw; + float _psiCmd; +public: + BlockWaypointGuidance(SuperBlock *parent, const char *name); + virtual ~BlockWaypointGuidance(); + void update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + +/** + * Multi-mode Autopilot + */ +class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot +{ +private: + // stabilization + BlockStabilization _stabilization; + + // heading hold + BlockP _psi2Phi; + BlockP _phi2P; + BlockLimitSym _phiLimit; + + // velocity hold + BlockPID _v2Theta; + BlockPID _theta2Q; + BlockLimit _theLimit; + BlockLimit _vLimit; + + // altitude/ climb rate hold + BlockPID _h2Thr; + BlockPID _cr2Thr; + + // guidance + BlockWaypointGuidance _guide; + + // block params + BlockParam _trimAil; + BlockParam _trimElv; + BlockParam _trimRdr; + BlockParam _trimThr; + BlockParam _trimV; + BlockParam _vCmd; + BlockParam _crMax; + + struct pollfd _attPoll; + vehicle_global_position_set_triplet_s _lastPosCmd; + enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; + uint64_t _timeStamp; +public: + BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name); + void update(); + virtual ~BlockMultiModeBacksideAutopilot(); +}; + + +} // namespace fixedwing + +} // namespace control + diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526e..f4ea05088 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -45,12 +45,13 @@ #include #include #include -#include #include #include #include #include +#include "fixedwing.hpp" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk index ec958d7cb..133728781 100644 --- a/src/modules/fixedwing_backside/module.mk +++ b/src/modules/fixedwing_backside/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fixedwing_backside SRCS = fixedwing_backside_main.cpp \ + fixedwing.cpp \ params.c diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp new file mode 100644 index 000000000..b7a0bbbcc --- /dev/null +++ b/src/modules/segway/BlockSegwayController.cpp @@ -0,0 +1,56 @@ +#include "BlockSegwayController.hpp" + +void BlockSegwayController::update() { + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + } + + // XXX handle STABILIZED (loiter on spot) as well + // once the system switches from manual or auto to stabilized + // the setpoint should update to loitering around this position + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + + float spdCmd = phi2spd.update(_att.phi); + + // output + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + _actuators.control[0] = _manual.roll; + _actuators.control[1] = _manual.roll; + } + + // update all publications + updatePublications(); + +} + diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp new file mode 100644 index 000000000..b16d38338 --- /dev/null +++ b/src/modules/segway/BlockSegwayController.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include + +using namespace control; + +class BlockSegwayController : public control::BlockUorbEnabledAutopilot { +public: + BlockSegwayController() : + BlockUorbEnabledAutopilot(NULL,"SEG"), + phi2spd(this, "PHI2SPD") + { + } + void update(); +private: + BlockP phi2spd; +}; + diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk new file mode 100644 index 000000000..d5da85601 --- /dev/null +++ b/src/modules/segway/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# segway controller +# + +MODULE_COMMAND = segway + +SRCS = segway_main.cpp \ + BlockSegwayController.cpp \ + params.c diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c new file mode 100644 index 000000000..db30af416 --- /dev/null +++ b/src/modules/segway/params.c @@ -0,0 +1,72 @@ +#include + +// currently tuned for easystar from arkhangar in HIL +//https://github.com/arktools/arkhangar + +// 16 is max name length + +// gyro low pass filter +PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq + +// yaw washout +PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass + +// stabilization mode +PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder + +// psi -> phi -> p +PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll +PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg + +// velocity -> theta +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass +PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard +PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle +PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle + + +// theta -> q +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID +PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); + +// h -> thr +PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID +PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); + +// crosstrack +PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain + +// speed command +PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity + +// rate of climb +// this is what rate of climb is commanded (in m/s) +// when the pitch stick is fully defelcted in simple mode +PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); + +// climb rate -> thr +PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID +PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f); + +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp new file mode 100644 index 000000000..8be1cc7aa --- /dev/null +++ b/src/modules/segway/segway_main.cpp @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file segway_main.cpp + * @author James Goppert + * + * Segway controller using control library + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BlockSegwayController.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int segway_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int control_demo_thread_main(int argc, char *argv[]); + +/** + * Test function + */ +void test(); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: segway {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int segway_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + + deamon_task = task_spawn_cmd("segway", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + control_demo_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + test(); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int control_demo_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + BlockSegwayController autopilot; + + thread_running = true; + + while (!thread_should_exit) { + autopilot.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} + +void test() +{ + warnx("beginning control lib test"); + control::basicBlocksTest(); +} -- cgit v1.2.3 From f4fc3bbd571ce99b707d326a206159a6eab49547 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 14:10:37 +0200 Subject: Added RAMTRON device, updated diagrams --- Documentation/flight_mode_state_machine.odg | Bin 18105 -> 18452 bytes makefiles/config_px4fmu-v1_default.mk | 1 + src/systemcmds/ramtron/module.mk | 6 + src/systemcmds/ramtron/ramtron.c | 279 ++++++++++++++++++++++++++++ 4 files changed, 286 insertions(+) create mode 100644 src/systemcmds/ramtron/module.mk create mode 100644 src/systemcmds/ramtron/ramtron.c (limited to 'makefiles/config_px4fmu-v1_default.mk') diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index b630ecb40..f9fa7a032 100644 Binary files a/Documentation/flight_mode_state_machine.odg and b/Documentation/flight_mode_state_machine.odg differ diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 74be1cd23..3f88b8c8c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -39,6 +39,7 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/eeprom +MODULES += systemcmds/ramtron MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk new file mode 100644 index 000000000..e4eb1d143 --- /dev/null +++ b/src/systemcmds/ramtron/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = ramtron +SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c new file mode 100644 index 000000000..03c713987 --- /dev/null +++ b/src/systemcmds/ramtron/ramtron.c @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file ramtron.c + * + * ramtron service and utility app. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int ramtron_main(int argc, char *argv[]); + +#ifndef CONFIG_MTD_RAMTRON + +/* create a fake command with decent message to not confuse users */ +int ramtron_main(int argc, char *argv[]) +{ + errx(1, "RAMTRON not enabled, skipping."); +} +#else + +static void ramtron_attach(void); +static void ramtron_start(void); +static void ramtron_erase(void); +static void ramtron_ioctl(unsigned operation); +static void ramtron_save(const char *name); +static void ramtron_load(const char *name); +static void ramtron_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *ramtron_mtd; + +int ramtron_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + ramtron_start(); + + if (!strcmp(argv[1], "save_param")) + ramtron_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + ramtron_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + ramtron_erase(); + + if (!strcmp(argv[1], "test")) + ramtron_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + ramtron_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + ramtron_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); + + +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + // xxx set in ramtron driver, leave this out +// SPI_SETFREQUENCY(spi, 4000000); + SPI_SETFREQUENCY(spi, 375000000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + ramtron_mtd = ramtron_initialize(spi); + if (ramtron_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: ramtron needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (ramtron_mtd == NULL) + errx(1, "failed to initialize ramtron driver"); + + attached = true; +} + +static void +ramtron_start(void) +{ + int ret; + + if (started) + errx(1, "ramtron already mounted"); + + if (!attached) + ramtron_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(ramtron_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); + + /* mount the ramtron */ + ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /ramtron - erase ramtron to reformat"); + + started = true; + warnx("mounted ramtron at /ramtron"); + exit(0); +} + +//extern int at24c_nuke(void); + +static void +ramtron_erase(void) +{ + if (!attached) + ramtron_attach(); + +// if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +ramtron_ioctl(unsigned operation) +{ + int fd; + + fd = open("/ramtron/.", 0); + + if (fd < 0) + err(1, "open /ramtron"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +ramtron_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +ramtron_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +//extern void at24c_test(void); + +static void +ramtron_test(void) +{ +// at24c_test(); + exit(0); +} + +#endif -- cgit v1.2.3 From 7c3ee6714c0e184c297ffced880488a41e7d255d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:14:25 +0200 Subject: Enabled mathlib --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) (limited to 'makefiles/config_px4fmu-v1_default.mk') diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e6ec840f9..38cf437e0 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,6 +94,7 @@ MODULES += modules/sdlog2 MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB -- cgit v1.2.3