diff options
39 files changed, 505 insertions, 231 deletions
diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index b98ab4774..38e65435b 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,7 +2,7 @@ # # UNTESTED UNTESTED! # -# Generic 10” Hexa coaxial geometry +# Generic 10" Hexa coaxial geometry # # Lorenz Meier <lm@inf.ethz.ch> # diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 655cb6226..99f902a6d 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo coaxial geometry +# Generic 10" Octo coaxial geometry # # Lorenz Meier <lm@inf.ethz.ch> # diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a3004d1e1..6cbd23643 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,9 +2,38 @@ # # Phantom FPV Flying Wing # -# Simon Wilks <sjwilks@gmail.com> +# Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com> # sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 13 + param set FW_AIRSPD_TRIM 18 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.75 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.2 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.03 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.5 + param set FW_RR_I 0.02 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.08 + param set FW_R_LIM 70 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha new file mode 100644 index 000000000..7dbda54d3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -0,0 +1,42 @@ +#!nsh +# +# TBS Caipirinha Flying Wing +# +# Thomas Gubler <thomasgubler@gmail.com> +# + +sh /etc/init.d/rc.fw_defaults + +if [ $DO_AUTOCONFIG == yes ] +then + + # TODO: these are the X5 default parameters, update them to the caipi + + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.3 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.03 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.3 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.03 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 345b0e3e4..06c54a41d 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Quad X geometry +# Generic 10" Quad X geometry # # Lorenz Meier <lm@inf.ethz.ch> # diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 55b31067d..1fb25e5d8 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Quad + geometry +# Generic 10" Quad + geometry # # Anton Babushkin <anton.babushkin@me.com> # diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 7714a508c..34fc6523f 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Hexa X geometry +# Generic 10" Hexa X geometry # # Anton Babushkin <anton.babushkin@me.com> # diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 60db8c069..235e376a6 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Hexa + geometry +# Generic 10" Hexa + geometry # # Anton Babushkin <anton.babushkin@me.com> # diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 411aee114..769217dc7 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo X geometry +# Generic 10" Octo X geometry # # Anton Babushkin <anton.babushkin@me.com> # diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 81132fd3e..28b074a54 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo + geometry +# Generic 10" Octo + geometry # # Anton Babushkin <anton.babushkin@me.com> # diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 7aaf7133e..4ae6a2794 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -97,6 +97,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3100 +then + sh /etc/init.d/3100_tbs_caipirinha +fi + # # Quad X # diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56..4cd73e23f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,4 +11,6 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_PITCH 1 fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 55e2a886e..65a66a085 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,7 +108,6 @@ then set HIL no set VEHICLE_TYPE none set MIXER none - set USE_IO yes set OUTPUT_MODE none set PWM_OUTPUTS none set PWM_RATE none @@ -129,6 +128,16 @@ then else set DO_AUTOCONFIG no fi + + # + # Set USE_IO flag + # + if param compare SYS_USE_IO 1 + then + set USE_IO yes + else + set USE_IO no + fi # # Set parameters and env variables for selected AUTOSTART @@ -466,7 +475,7 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -525,7 +534,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.mc_apps fi diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b6e80ce1d..d27ab9727 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,7 +132,6 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); } return ret; @@ -308,7 +307,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no ETS airspeed sensor connected"); } /** diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d43e3aef9..5d1f58b85 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include <arch/board/board.h> #include <mavlink/mavlink_log.h> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> @@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu float prev_revolution = md25.getRevolutions1(); // debug publication - control::UOrbPublication<debug_key_value_s> debug_msg(NULL, + uORB::Publication<debug_key_value_s> debug_msg(NULL, ORB_ID(debug_key_value)); // sine wave for motor 1 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 1661f67f9..962c6b881 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/Subscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription<actuator_controls_s> _actuators; + uORB::Subscription<actuator_controls_s> _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 39ee1e882..2c3efdc35 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -108,7 +108,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); protected: @@ -128,7 +128,7 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, +MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) { @@ -167,23 +167,25 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - uint8_t status = val[0] & 0xC0; + uint8_t status = (val[0] & 0xC0) >> 6; - if (status == 2) { - log("err: stale data"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } else if (status == 3) { - log("err: fault"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; + switch (status) { + case 0: + break; + + case 1: + /* fallthrough */ + case 2: + /* fallthrough */ + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; @@ -199,19 +201,21 @@ MEASAirspeed::collect() */ const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; - if (diff_press_pa < 0.0f) - diff_press_pa = 0.0f; + float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + + if (diff_press_pa < 0.0f) { + diff_press_pa = 0.0f; + } struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + _max_differential_pressure_pa = diff_press_pa; } report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); @@ -268,8 +272,9 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -310,15 +315,17 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver, try the MS4525DO first */ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { @@ -326,22 +333,26 @@ start(int i2c_bus) g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -352,7 +363,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no MS4525 airspeed sensor connected"); } /** @@ -386,21 +397,24 @@ test() int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -411,14 +425,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); @@ -426,8 +442,9 @@ test() } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -440,14 +457,17 @@ reset() { int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -458,8 +478,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -498,32 +519,37 @@ meas_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { meas_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { meas_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { meas_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { meas_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { meas_airspeed::info(); + } meas_airspeed_usage(); exit(0); diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 50089282a..f214b5964 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -74,7 +74,7 @@ /* Configuration Constants */ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION -#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A +#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d65a9be36..dd5e4d3e0 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -53,7 +53,7 @@ #include <arch/board/board.h> #include <mavlink/mavlink_log.h> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index e9f35cf95..58994d6fa 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/Subscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription<actuator_controls_s> _actuators; + uORB::Subscription<actuator_controls_s> _actuators; // private data float _motor1Position; diff --git a/src/modules/controllib/block/List.hpp b/src/include/containers/List.hpp index 96b0b94d1..13cbda938 100644 --- a/src/modules/controllib/block/List.hpp +++ b/src/include/containers/List.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file Node.h + * @file List.hpp * - * A node of a linked list. + * A linked list. */ #pragma once @@ -43,7 +43,7 @@ template<class T> class __EXPORT ListNode { public: - ListNode() : _sibling(NULL) { + ListNode() : _sibling(nullptr) { } void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8..caf93bc78 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include <mathlib/mathlib.h> #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -#include <controllib/uorb/UOrbSubscription.hpp> -#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/Subscription.hpp> +#include <uORB/Publication.hpp> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> @@ -138,13 +138,13 @@ protected: math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions - control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */ - control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */ - control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ + uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */ + uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */ + uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */ // publications - control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ - control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */ - control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */ + uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */ + uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */ // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index b964d40a3..6768bfa7e 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -41,10 +41,11 @@ #include <string.h> #include <stdio.h> +#include <uORB/Subscription.hpp> +#include <uORB/Publication.hpp> + #include "Block.hpp" #include "BlockParam.hpp" -#include "../uorb/UOrbSubscription.hpp" -#include "../uorb/UOrbPublication.hpp" namespace control { @@ -100,7 +101,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - UOrbSubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionBase *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -118,7 +119,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - UOrbPublicationBase *pub = getPublications().getHead(); + uORB::PublicationBase *pub = getPublications().getHead(); int count = 0; while (pub != NULL) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 258701f27..736698e21 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -42,7 +42,13 @@ #include <stdint.h> #include <inttypes.h> -#include "List.hpp" +#include <containers/List.hpp> + +// forward declaration +namespace uORB { + class SubscriptionBase; + class PublicationBase; +} namespace control { @@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80; // forward declaration class BlockParamBase; -class UOrbSubscriptionBase; -class UOrbPublicationBase; class SuperBlock; /** @@ -79,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; } - List<UOrbPublicationBase *> & getPublications() { return _publications; } + List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; } + List<uORB::PublicationBase *> & getPublications() { return _publications; } List<BlockParamBase *> & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List<UOrbSubscriptionBase *> _subscriptions; - List<UOrbPublicationBase *> _publications; + List<uORB::SubscriptionBase *> _subscriptions; + List<uORB::PublicationBase *> _publications; List<BlockParamBase *> _params; }; diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index fd12e365d..8f98da74f 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref printf("error finding param: %s\n", fullname); }; +template <class T> +BlockParam<T>::BlockParam(Block *block, const char *name, + bool parent_prefix) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); +} + +template <class T> +T BlockParam<T>::get() { return _val; } + +template <class T> +void BlockParam<T>::set(T val) { _val = val; } + +template <class T> +void BlockParam<T>::update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); +} + +template <class T> +BlockParam<T>::~BlockParam() {}; + +template class __EXPORT BlockParam<float>; +template class __EXPORT BlockParam<int>; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 36bc8c24b..a64d0139e 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -42,7 +42,7 @@ #include <systemlib/param/param.h> #include "Block.hpp" -#include "List.hpp" +#include <containers/List.hpp> namespace control { @@ -70,38 +70,21 @@ protected: * Parameters that are tied to blocks for updating and nameing. */ -class __EXPORT BlockParamFloat : public BlockParamBase +template <class T> +class BlockParam : public BlockParamBase { public: - BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - float get() { return _val; } - void set(float val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } + BlockParam(Block *block, const char *name, + bool parent_prefix=true); + T get(); + void set(T val); + void update(); + virtual ~BlockParam(); protected: - float _val; + T _val; }; -class __EXPORT BlockParamInt : public BlockParamBase -{ -public: - BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - int get() { return _val; } - void set(int val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } -protected: - int _val; -}; +typedef BlockParam<float> BlockParamFloat; +typedef BlockParam<int> BlockParamInt; } // namespace control diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index d815a8feb..f0139a4b7 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,5 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - uorb/UOrbPublication.cpp \ - uorb/UOrbSubscription.cpp \ uorb/blocks.cpp \ blocks.cpp diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 7c80c4b2b..a8a70507e 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -62,8 +62,8 @@ extern "C" { } #include "../blocks.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include <uORB/Subscription.hpp> +#include <uORB/Publication.hpp> namespace control { @@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions - UOrbSubscription<vehicle_attitude_s> _att; - UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; - UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; - UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<position_setpoint_triplet_s> _missionCmd; - UOrbSubscription<manual_control_setpoint_s> _manual; - UOrbSubscription<vehicle_status_s> _status; - UOrbSubscription<parameter_update_s> _param_update; + uORB::Subscription<vehicle_attitude_s> _att; + uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd; + uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd; + uORB::Subscription<vehicle_global_position_s> _pos; + uORB::Subscription<position_setpoint_triplet_s> _missionCmd; + uORB::Subscription<manual_control_setpoint_s> _manual; + uORB::Subscription<vehicle_status_s> _status; + uORB::Subscription<parameter_update_s> _param_update; // publications - UOrbPublication<actuator_controls_s> _actuators; + uORB::Publication<actuator_controls_s> _actuators; public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); virtual ~BlockUorbEnabledAutopilot(); diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f7c0b6148..cfae07275 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); @@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5ade835ff..f139c25f4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -108,14 +108,14 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ @@ -123,20 +123,19 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ struct { float tconst; @@ -245,7 +244,7 @@ private: /** * Check for airspeed updates. */ - bool vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. @@ -308,19 +307,18 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false), - _airspeed_valid(false) + _setpoint_valid(false) { /* safely initialize structs */ - _att = {0}; - _accel = {0}; - _att_sp = {0}; - _manual = {0}; - _airspeed = {0}; - _vcontrol_mode = {0}; - _actuators = {0}; - _actuators_airframe = {0}; - _global_pos = {0}; + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -482,7 +480,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -bool +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ @@ -492,10 +490,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); // warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); - return true; } - - return false; } void @@ -569,7 +564,7 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -626,7 +621,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - _airspeed_valid = vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -666,9 +661,9 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is smaller than min, the sensor is not giving good readings */ - if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { + if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || + !isfinite(_airspeed.indicated_airspeed_m_s) || + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { @@ -791,10 +786,6 @@ FixedwingAttitudeControl::task_main() warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); } - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); - /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4ee04d462..177f21667 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1031,6 +1031,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; raw.differential_pressure_counter++; + _airspeed.timestamp = hrt_absolute_time(); _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index cb35a2541..ec2bc3a9a 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); * @group System */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); + +/** + * Set usage of IO board + * + * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + * + * @min 0 + * @max 1 + * @group System + */ +PARAM_DEFINE_INT32(SYS_USE_IO, 1); diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/uORB/Publication.cpp index f69b39d90..5a5981617 100644 --- a/src/modules/controllib/uorb/UOrbPublication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -32,8 +32,49 @@ ****************************************************************************/ /** - * @file UOrbPublication.cpp + * @file Publication.cpp * */ -#include "UOrbPublication.hpp" +#include "Publication.hpp" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_global_position.h" +#include "topics/debug_key_value.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_global_velocity_setpoint.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" +#include "topics/actuator_outputs.h" +#include "topics/encoders.h" + +namespace uORB { + +template<class T> +Publication<T>::Publication( + List<PublicationBase *> * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + PublicationBase(list, meta) { +} + +template<class T> +Publication<T>::~Publication() {} + +template<class T> +void * Publication<T>::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template class __EXPORT Publication<vehicle_attitude_s>; +template class __EXPORT Publication<vehicle_local_position_s>; +template class __EXPORT Publication<vehicle_global_position_s>; +template class __EXPORT Publication<debug_key_value_s>; +template class __EXPORT Publication<actuator_controls_s>; +template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>; +template class __EXPORT Publication<vehicle_attitude_setpoint_s>; +template class __EXPORT Publication<vehicle_rates_setpoint_s>; +template class __EXPORT Publication<actuator_outputs_s>; +template class __EXPORT Publication<encoders_s>; + +} diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/uORB/Publication.hpp index 6f1f3fc1c..8650b3df8 100644 --- a/src/modules/controllib/uorb/UOrbPublication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -32,32 +32,29 @@ ****************************************************************************/ /** - * @file UOrbPublication.h + * @file Publication.h * */ #pragma once #include <uORB/uORB.h> -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include <containers/List.hpp> -namespace control +namespace uORB { -class Block; - /** * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *> +class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *> { public: - UOrbPublicationBase( - List<UOrbPublicationBase *> * list, + PublicationBase( + List<PublicationBase *> * list, const struct orb_metadata *meta) : _meta(meta), _handle(-1) { @@ -71,7 +68,7 @@ public: } } virtual void *getDataVoidPtr() = 0; - virtual ~UOrbPublicationBase() { + virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } const struct orb_metadata *getMeta() { return _meta; } @@ -83,12 +80,12 @@ protected: }; /** - * UOrb Publication wrapper class + * Publication wrapper class */ template<class T> -class UOrbPublication : +class Publication : public T, // this must be first! - public UOrbPublicationBase + public PublicationBase { public: /** @@ -98,13 +95,9 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbPublication( - List<UOrbPublicationBase *> * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - UOrbPublicationBase(list, meta) { - } - virtual ~UOrbPublication() {} + Publication(List<PublicationBase *> * list, + const struct orb_metadata *meta); + virtual ~Publication(); /* * XXX * This function gets the T struct, assuming @@ -112,7 +105,7 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } + void *getDataVoidPtr(); }; -} // namespace control +} // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp new file mode 100644 index 000000000..c1d1a938f --- /dev/null +++ b/src/modules/uORB/Subscription.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 Subscription.cpp + * + */ + +#include "Subscription.hpp" +#include "topics/parameter_update.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_gps_position.h" +#include "topics/sensor_combined.h" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_global_position.h" +#include "topics/encoders.h" +#include "topics/position_setpoint_triplet.h" +#include "topics/vehicle_status.h" +#include "topics/manual_control_setpoint.h" +#include "topics/vehicle_local_position_setpoint.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" + +namespace uORB +{ + +bool __EXPORT SubscriptionBase::updated() +{ + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; +} + +template<class T> +Subscription<T>::Subscription( + List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + SubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); +} + +template<class T> +Subscription<T>::~Subscription() {} + +template<class T> +void * Subscription<T>::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template<class T> +T Subscription<T>::getData() { + return T(*this); +} + +template class __EXPORT Subscription<parameter_update_s>; +template class __EXPORT Subscription<actuator_controls_s>; +template class __EXPORT Subscription<vehicle_gps_position_s>; +template class __EXPORT Subscription<sensor_combined_s>; +template class __EXPORT Subscription<vehicle_attitude_s>; +template class __EXPORT Subscription<vehicle_global_position_s>; +template class __EXPORT Subscription<encoders_s>; +template class __EXPORT Subscription<position_setpoint_triplet_s>; +template class __EXPORT Subscription<vehicle_status_s>; +template class __EXPORT Subscription<manual_control_setpoint_s>; +template class __EXPORT Subscription<vehicle_local_position_setpoint_s>; +template class __EXPORT Subscription<vehicle_local_position_s>; +template class __EXPORT Subscription<vehicle_attitude_setpoint_s>; +template class __EXPORT Subscription<vehicle_rates_setpoint_s>; + +} // namespace uORB diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/uORB/Subscription.hpp index d337d89a8..34e9a83e0 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -32,28 +32,25 @@ ****************************************************************************/ /** - * @file UOrbSubscription.h + * @file Subscription.h * */ #pragma once #include <uORB/uORB.h> -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include <containers/List.hpp> -namespace control +namespace uORB { -class Block; - /** * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT UOrbSubscriptionBase : - public ListNode<control::UOrbSubscriptionBase *> +class __EXPORT SubscriptionBase : + public ListNode<SubscriptionBase *> { public: // methods @@ -64,8 +61,8 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbSubscriptionBase( - List<UOrbSubscriptionBase *> * list, + SubscriptionBase( + List<SubscriptionBase *> * list, const struct orb_metadata *meta) : _meta(meta), _handle() { @@ -78,7 +75,7 @@ public: } } virtual void *getDataVoidPtr() = 0; - virtual ~UOrbSubscriptionBase() { + virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } // accessors @@ -93,12 +90,12 @@ protected: }; /** - * UOrb Subscription wrapper class + * Subscription wrapper class */ template<class T> -class __EXPORT UOrbSubscription : +class __EXPORT Subscription : public T, // this must be first! - public UOrbSubscriptionBase + public SubscriptionBase { public: /** @@ -109,19 +106,13 @@ public: * for the topic. * @param interval The minimum interval in milliseconds between updates */ - UOrbSubscription( - List<UOrbSubscriptionBase *> * 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); - } - + Subscription( + List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval); /** * Deconstructor */ - virtual ~UOrbSubscription() {} + virtual ~Subscription(); /* * XXX @@ -130,8 +121,8 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } - T getData() { return T(*this); } + void *getDataVoidPtr(); + T getData(); }; -} // namespace control +} // namespace uORB diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 5ec31ab01..0c29101fe 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -41,4 +41,6 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 4096 SRCS = uORB.cpp \ - objects_common.cpp + objects_common.cpp \ + Publication.cpp \ + Subscription.cpp diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4c84c1f25..fb24de8d1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); #include "topics/esc_status.h" ORB_DEFINE(esc_status, struct esc_status_s); + +#include "topics/encoders.h" +ORB_DEFINE(encoders, struct encoders_s); diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/uORB/topics/encoders.h index 022cadd24..588c0ddb1 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.cpp +++ b/src/modules/uORB/topics/encoders.h @@ -32,20 +32,35 @@ ****************************************************************************/ /** - * @file UOrbSubscription.cpp + * @file encoders.h + * + * Encoders topic. * */ -#include "UOrbSubscription.hpp" +#ifndef TOPIC_ENCODERS_H +#define TOPIC_ENCODERS_H + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ -namespace control -{ +#define NUM_ENCODERS 4 + +struct encoders_s { + uint64_t timestamp; + int64_t counts[NUM_ENCODERS]; // counts of encoder + float velocity[NUM_ENCODERS]; // counts of encoder/ second +}; + +/** + * @} + */ -bool __EXPORT UOrbSubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} +ORB_DECLARE(encoders); -} // namespace control +#endif |