From 8267bdf4a5aa857db0f970da750d782f1d8ec369 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 9 Mar 2014 15:50:41 +0100 Subject: [PATCH 01/20] fw_att_control: airspeed is now used correctly --- src/modules/fw_att_control/fw_att_control_main.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) 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 17b1028f9a..c59a396c27 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -230,7 +230,7 @@ private: /** * Check for airspeed updates. */ - bool vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. @@ -452,7 +452,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -bool +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ @@ -462,10 +462,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 @@ -539,7 +536,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(); @@ -596,7 +593,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(); @@ -636,8 +633,7 @@ 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) || + if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; From 2db140bcabaafcccc1ed2ce070d7a24b4133eb57 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 10:59:11 +0100 Subject: [PATCH 02/20] fw_att_control: delete the unused flag --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 c59a396c27..ec91f4d8ca 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -136,7 +136,6 @@ private: 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; @@ -293,8 +292,7 @@ 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}; @@ -753,10 +751,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 From 2cd29b7c8ce7fc26af6f1a3957e243a18bc09d5b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:07:48 +0100 Subject: [PATCH 03/20] fw_att_control: proper struct initialization --- .../fw_att_control/fw_att_control_main.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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 ec91f4d8ca..fa0f5962f6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -295,15 +295,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _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"); From 2f06f15240a9d03a282212538283e53e9bcc4b9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:09:49 +0100 Subject: [PATCH 04/20] fw_att_control: whitespace only --- .../fw_att_control/fw_att_control_main.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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 fa0f5962f6..deafd7f86f 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,15 +123,15 @@ 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 */ From a977ff2b6295bd4bac614a854ecc35e1b9b2d75c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 00:20:27 +0100 Subject: [PATCH 05/20] fw att control: add timestamp dependency in airspeed check --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 deafd7f86f..497d1d15a4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -632,7 +632,8 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { + !isfinite(_airspeed.indicated_airspeed_m_s) || + (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { airspeed = _parameters.airspeed_trim; } else { From d9d0e60bd574b135d98bdaa48c4b32056f815217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 10:02:39 +0100 Subject: [PATCH 06/20] fw att ctrl: airspeed check: remove unnecessary cast --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 497d1d15a4..bb674d6c98 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -633,7 +633,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s) || - (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { From b6ea38b91a3d161ce01ab3683b0097f45c4e4de3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 13:32:24 +0100 Subject: [PATCH 07/20] sensors app: add timestamp in airspeed message --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..6e2df89ed0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1030,6 +1030,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_pa = _diff_pres.differential_pressure_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); From 4466dbf0b37f0a80790c346affee1e4ac86bef22 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 16 Mar 2014 18:47:21 +0100 Subject: [PATCH 08/20] add SYS_USE_IO param which allows using standard startup scripts for FMU only mode --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- src/modules/systemlib/system_params.c | 11 +++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7c3524fef0..65a66a085c 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 diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index cb35a25413..ec2bc3a9a0 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); From c5a1f4617c6d3e1782dde5d4552122059200db84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 18:30:05 +0100 Subject: [PATCH 09/20] Fixed MS4525 error flag handling --- src/drivers/meas_airspeed/meas_airspeed.cpp | 115 +++++++++++++------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf77..7baa8738f6 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -104,7 +104,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: @@ -123,7 +123,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) { @@ -161,23 +161,30 @@ 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: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 2: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; @@ -193,19 +200,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.voltage = 0; @@ -261,8 +270,9 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -303,15 +313,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()) { @@ -319,22 +331,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); @@ -379,21 +395,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++) { @@ -404,14 +423,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); @@ -419,8 +440,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"); } @@ -433,14 +455,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); } @@ -451,8 +476,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(); @@ -491,32 +517,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); From fc877f9c20aaeb59275c7e1953bad818ba27662c Mon Sep 17 00:00:00 2001 From: dominiho Date: Wed, 19 Mar 2014 16:13:17 +0100 Subject: [PATCH 10/20] changed px4flow I2C address to default value --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 50089282ab..f214b5964f 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 */ From 2c32cdf16bb02a9ae4d47b60cb32553fefb33738 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 15:54:35 -0400 Subject: [PATCH 11/20] Fixed block param template. --- src/modules/controllib/block/BlockParam.cpp | 25 +++++++++++++ src/modules/controllib/block/BlockParam.hpp | 39 ++++++--------------- 2 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index fd12e365dc..8f98da74fa 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 +BlockParam::BlockParam(Block *block, const char *name, + bool parent_prefix) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); +} + +template +T BlockParam::get() { return _val; } + +template +void BlockParam::set(T val) { _val = val; } + +template +void BlockParam::update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); +} + +template +BlockParam::~BlockParam() {}; + +template class __EXPORT BlockParam; +template class __EXPORT BlockParam; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 36bc8c24ba..735b2b2922 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -70,38 +70,21 @@ protected: * Parameters that are tied to blocks for updating and nameing. */ -class __EXPORT BlockParamFloat : public BlockParamBase +template +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 BlockParamFloat; +typedef BlockParam BlockParamInt; } // namespace control From fd6590cfa7e14436fa8af6a0569b6382cd39069a Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 16:14:56 -0400 Subject: [PATCH 12/20] Moved UOrbPubliction/Subscription to uORB::Publication/Subscription --- src/drivers/md25/md25.cpp | 4 +-- src/drivers/md25/md25.hpp | 4 +-- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/drivers/roboclaw/RoboClaw.hpp | 4 +-- .../att_pos_estimator_ekf/KalmanNav.hpp | 16 ++++----- src/modules/controllib/block/Block.cpp | 8 ++--- src/modules/controllib/block/Block.hpp | 16 +++++---- src/modules/controllib/module.mk | 2 -- src/modules/controllib/uorb/blocks.hpp | 22 ++++++------ .../Publication.cpp} | 4 +-- .../Publication.hpp} | 33 ++++++++--------- .../Subscription.cpp} | 10 +++--- .../Subscription.hpp} | 35 +++++++++---------- src/modules/uORB/module.mk | 4 ++- 14 files changed, 81 insertions(+), 83 deletions(-) rename src/modules/{controllib/uorb/UOrbPublication.cpp => uORB/Publication.cpp} (96%) rename src/modules/{controllib/uorb/UOrbPublication.hpp => uORB/Publication.hpp} (85%) rename src/modules/{controllib/uorb/UOrbSubscription.cpp => uORB/Subscription.cpp} (92%) rename src/modules/{controllib/uorb/UOrbSubscription.hpp => uORB/Subscription.hpp} (85%) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d43e3aef9e..6d5e805ea1 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include @@ -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_msg(NULL, + uORB::Publication 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 1661f67f9a..962c6b881d 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription _actuators; + uORB::Subscription _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d65a9be361..dd5e4d3e0c 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -53,7 +53,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index e9f35cf956..58994d6fa6 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription _actuators; + uORB::Subscription _actuators; // private data float _motor1Position; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8e..caf93bc787 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 @@ -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 _sensors; /**< sensors sub. */ - control::UOrbSubscription _gps; /**< gps sub. */ - control::UOrbSubscription _param_update; /**< parameter update sub. */ + uORB::Subscription _sensors; /**< sensors sub. */ + uORB::Subscription _gps; /**< gps sub. */ + uORB::Subscription _param_update; /**< parameter update sub. */ // publications - control::UOrbPublication _pos; /**< position pub. */ - control::UOrbPublication _localPos; /**< local position pub. */ - control::UOrbPublication _att; /**< attitude pub. */ + uORB::Publication _pos; /**< position pub. */ + uORB::Publication _localPos; /**< local position pub. */ + uORB::Publication _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 b964d40a31..0b4cce9792 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 "../uorb/UOrbSubscription.hpp" -#include "../uorb/UOrbPublication.hpp" +#include "uORB/Subscription.hpp" +#include "uORB/Publication.hpp" namespace control { @@ -100,7 +100,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - UOrbSubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionBase *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -118,7 +118,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 258701f279..6a8e1b5b9c 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -44,6 +44,12 @@ #include "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 & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } + List & getSubscriptions() { return _subscriptions; } + List & getPublications() { return _publications; } List & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List _subscriptions; - List _publications; + List _subscriptions; + List _publications; List _params; }; diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index d815a8feb5..f0139a4b70 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 7c80c4b2ba..a8a70507e9 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 +#include namespace control { @@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions - UOrbSubscription _att; - UOrbSubscription _attCmd; - UOrbSubscription _ratesCmd; - UOrbSubscription _pos; - UOrbSubscription _missionCmd; - UOrbSubscription _manual; - UOrbSubscription _status; - UOrbSubscription _param_update; + uORB::Subscription _att; + uORB::Subscription _attCmd; + uORB::Subscription _ratesCmd; + uORB::Subscription _pos; + uORB::Subscription _missionCmd; + uORB::Subscription _manual; + uORB::Subscription _status; + uORB::Subscription _param_update; // publications - UOrbPublication _actuators; + uORB::Publication _actuators; public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); virtual ~BlockUorbEnabledAutopilot(); diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/uORB/Publication.cpp similarity index 96% rename from src/modules/controllib/uorb/UOrbPublication.cpp rename to src/modules/uORB/Publication.cpp index f69b39d90d..ed67b485de 100644 --- a/src/modules/controllib/uorb/UOrbPublication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file UOrbPublication.cpp + * @file Publication.cpp * */ -#include "UOrbPublication.hpp" +#include "Publication.hpp" diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/uORB/Publication.hpp similarity index 85% rename from src/modules/controllib/uorb/UOrbPublication.hpp rename to src/modules/uORB/Publication.hpp index 6f1f3fc1c0..7fa6bcc175 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 -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include -namespace control +namespace uORB { -class Block; - /** * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT UOrbPublicationBase : public ListNode +class __EXPORT PublicationBase : public ListNode { public: - UOrbPublicationBase( - List * list, + PublicationBase( + List * 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 UOrbPublication : +class Publication : public T, // this must be first! - public UOrbPublicationBase + public PublicationBase { public: /** @@ -98,13 +95,13 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbPublication( - List * list, + Publication( + List * list, const struct orb_metadata *meta) : T(), // initialize data structure to zero - UOrbPublicationBase(list, meta) { + PublicationBase(list, meta) { } - virtual ~UOrbPublication() {} + virtual ~Publication() {} /* * XXX * This function gets the T struct, assuming @@ -115,4 +112,4 @@ public: void *getDataVoidPtr() { return (void *)(T *)(this); } }; -} // namespace control +} // namespace uORB diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/uORB/Subscription.cpp similarity index 92% rename from src/modules/controllib/uorb/UOrbSubscription.cpp rename to src/modules/uORB/Subscription.cpp index 022cadd245..6e8830708b 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -32,20 +32,20 @@ ****************************************************************************/ /** - * @file UOrbSubscription.cpp + * @file Subscription.cpp * */ -#include "UOrbSubscription.hpp" +#include "Subscription.hpp" -namespace control +namespace uORB { -bool __EXPORT UOrbSubscriptionBase::updated() +bool __EXPORT SubscriptionBase::updated() { bool isUpdated = false; orb_check(_handle, &isUpdated); return isUpdated; } -} // namespace control +} // namespace uORB diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/uORB/Subscription.hpp similarity index 85% rename from src/modules/controllib/uorb/UOrbSubscription.hpp rename to src/modules/uORB/Subscription.hpp index d337d89a88..55fb95d51c 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 -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include -namespace control +namespace uORB { -class Block; - /** * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT UOrbSubscriptionBase : - public ListNode +class __EXPORT SubscriptionBase : + public ListNode { public: // methods @@ -64,8 +61,8 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbSubscriptionBase( - List * list, + SubscriptionBase( + List * 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 __EXPORT UOrbSubscription : +class __EXPORT Subscription : public T, // this must be first! - public UOrbSubscriptionBase + public SubscriptionBase { public: /** @@ -109,11 +106,11 @@ public: * for the topic. * @param interval The minimum interval in milliseconds between updates */ - UOrbSubscription( - List * list, + Subscription( + List * list, const struct orb_metadata *meta, unsigned interval) : T(), // initialize data structure to zero - UOrbSubscriptionBase(list, meta) { + SubscriptionBase(list, meta) { setHandle(orb_subscribe(getMeta())); orb_set_interval(getHandle(), interval); } @@ -121,7 +118,7 @@ public: /** * Deconstructor */ - virtual ~UOrbSubscription() {} + virtual ~Subscription() {} /* * XXX @@ -134,4 +131,4 @@ public: T getData() { return T(*this); } }; -} // namespace control +} // namespace uORB diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 5ec31ab010..0c29101fec 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 From da9dab27998fa9e8b7a66d53a00aa3cae93573ec Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 16:21:06 -0400 Subject: [PATCH 13/20] Moved List.hpp from controllib to src/include/containers. --- .../controllib/block => include/containers}/List.hpp | 6 +++--- src/modules/controllib/block/Block.hpp | 2 +- src/modules/controllib/block/BlockParam.hpp | 2 +- src/modules/uORB/Publication.hpp | 2 +- src/modules/uORB/Subscription.hpp | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) rename src/{modules/controllib/block => include/containers}/List.hpp (96%) diff --git a/src/modules/controllib/block/List.hpp b/src/include/containers/List.hpp similarity index 96% rename from src/modules/controllib/block/List.hpp rename to src/include/containers/List.hpp index 96b0b94d10..13cbda9382 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 __EXPORT ListNode { public: - ListNode() : _sibling(NULL) { + ListNode() : _sibling(nullptr) { } void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 6a8e1b5b9c..736698e213 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -42,7 +42,7 @@ #include #include -#include "List.hpp" +#include // forward declaration namespace uORB { diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 735b2b2922..a64d0139e3 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -42,7 +42,7 @@ #include #include "Block.hpp" -#include "List.hpp" +#include namespace control { diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 7fa6bcc175..21840ed701 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 55fb95d51c..d839f80239 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace uORB From afb2c37bfc20150718114c4ef56e40a7c4d4722f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:01:03 -0400 Subject: [PATCH 14/20] Fixed uORB Pub/Sub templates for GCC 4.7 --- src/modules/uORB/Publication.cpp | 27 ++++++++++++++++++++++++++ src/modules/uORB/Publication.hpp | 12 ++++-------- src/modules/uORB/Subscription.cpp | 32 +++++++++++++++++++++++++++++++ src/modules/uORB/Subscription.hpp | 14 ++++---------- 4 files changed, 67 insertions(+), 18 deletions(-) diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index ed67b485de..78a5cd2045 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -37,3 +37,30 @@ */ #include "Publication.hpp" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_global_position.h" + +namespace uORB { + +template +Publication::Publication( + List * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + PublicationBase(list, meta) { +} + +template +Publication::~Publication() {} + +template +void * Publication::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; + +} diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 21840ed701..8650b3df89 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -95,13 +95,9 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - Publication( - List * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - PublicationBase(list, meta) { - } - virtual ~Publication() {} + Publication(List * list, + const struct orb_metadata *meta); + virtual ~Publication(); /* * XXX * This function gets the T struct, assuming @@ -109,7 +105,7 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } + void *getDataVoidPtr(); }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 6e8830708b..16da3b66f4 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -37,6 +37,10 @@ */ #include "Subscription.hpp" +#include "topics/parameter_update.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_gps_position.h" +#include "topics/sensor_combined.h" namespace uORB { @@ -48,4 +52,32 @@ bool __EXPORT SubscriptionBase::updated() return isUpdated; } +template +Subscription::Subscription( + List * 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 +Subscription::~Subscription() {} + +template +void * Subscription::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template +T Subscription::getData() { + return T(*this); +} + +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; + } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index d839f80239..34e9a83e03 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -108,17 +108,11 @@ public: */ Subscription( List * 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); - } - + const struct orb_metadata *meta, unsigned interval); /** * Deconstructor */ - virtual ~Subscription() {} + virtual ~Subscription(); /* * XXX @@ -127,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 uORB From 1b7472ef4c009caa8e76d6a4d90979677f14b0f4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:06:37 -0400 Subject: [PATCH 15/20] Fix for md25 and uORB update. --- src/drivers/md25/md25.cpp | 2 +- src/modules/uORB/Publication.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 6d5e805ea1..5d1f58b854 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/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 78a5cd2045..dbd56e6425 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -40,6 +40,7 @@ #include "topics/vehicle_attitude.h" #include "topics/vehicle_local_position.h" #include "topics/vehicle_global_position.h" +#include "topics/debug_key_value.h" namespace uORB { @@ -62,5 +63,6 @@ void * Publication::getDataVoidPtr() { template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } From 8f0b223c874b33bf09a8ceebcaf40cd60ee3800d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:16:30 -0400 Subject: [PATCH 16/20] Fixed include style for List in Block. --- src/modules/controllib/block/Block.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 0b4cce9792..6768bfa7ed 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -41,10 +41,11 @@ #include #include +#include +#include + #include "Block.hpp" #include "BlockParam.hpp" -#include "uORB/Subscription.hpp" -#include "uORB/Publication.hpp" namespace control { From eaef67f21df3d38a6523a79bb966fe62d44092ae Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 18:35:26 -0400 Subject: [PATCH 17/20] Added encoder uORB message/ fixedwing_backside working if enabled. --- src/modules/fixedwing_backside/fixedwing.cpp | 6 +- src/modules/uORB/Publication.cpp | 12 ++++ src/modules/uORB/Subscription.cpp | 20 ++++++ src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/encoders.h | 66 ++++++++++++++++++++ 5 files changed, 104 insertions(+), 3 deletions(-) create mode 100644 src/modules/uORB/topics/encoders.h diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f7c0b61488..cfae072752 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/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index dbd56e6425..5a59816172 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -41,6 +41,12 @@ #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 { @@ -64,5 +70,11 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 16da3b66f4..c1d1a938f1 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -41,6 +41,16 @@ #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 { @@ -79,5 +89,15 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4c84c1f25e..fb24de8d16 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/uORB/topics/encoders.h b/src/modules/uORB/topics/encoders.h new file mode 100644 index 0000000000..588c0ddb13 --- /dev/null +++ b/src/modules/uORB/topics/encoders.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 encoders.h + * + * Encoders topic. + * + */ + +#ifndef TOPIC_ENCODERS_H +#define TOPIC_ENCODERS_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +#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 +}; + +/** + * @} + */ + +ORB_DECLARE(encoders); + +#endif From 98ef3b9ab882da077c67f81eb2f7852c44badb36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:07:56 +0100 Subject: [PATCH 18/20] Make driver start less verbose --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - src/drivers/meas_airspeed/meas_airspeed.cpp | 13 ++++--------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b6e80ce1d0..e77f31e872 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; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7baa8738f6..4f64d85859 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -169,18 +169,13 @@ MEASAirspeed::collect() uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { - case 0: break; + case 0: + break; case 1: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 2: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); From 8af83f70747d104511cc100e4f94ff4d5c6ac6d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:11:28 +0100 Subject: [PATCH 19/20] Make error message less dramatic, as its part of a scanning progress --- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index e77f31e872..d27ab9727a 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -307,7 +307,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no ETS airspeed sensor connected"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 4f64d85859..beca28e7a4 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -356,7 +356,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no MS4525 airspeed sensor connected"); } /** From 057bcf3172f3c8d1bab561e7e4cad14977cd74d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 10:45:12 +0100 Subject: [PATCH 20/20] Changed RC scaling for fixed wing defaults --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56f..4cd73e23fe 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