From 0d44510f673ca78cfda26b7c537a710b146c6d77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:12:40 +0200 Subject: [PATCH 01/11] Examples: Fix copyright --- src/examples/px4_daemon_app/px4_daemon_app.c | 2 +- src/examples/px4_mavlink_debug/px4_mavlink_debug.c | 5 +++-- src/examples/px4_simple_app/px4_simple_app.c | 5 +++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 6e99939d14b2..adaa502abb5b 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c index aa1eb5ed8f5f..c1dd28463260 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file px4_mavlink_debug.c * Debug application example for PX4 autopilot + * + * @author Example User */ #include diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index e22c59fa2259..bc5d8697ed14 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file px4_simple_app.c * Minimal application example for PX4 autopilot + * + * @author Example User */ #include From 5c5ba1a85fd377bb6a0f1c04fcd70d08fa1718f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:39:49 +0200 Subject: [PATCH 02/11] Fix path for examples --- makefiles/config_px4fmu-v2_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 0998ebf9f868..9e8dab685055 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -140,19 +140,19 @@ MODULES += examples/rover_steering_control # #MODULES += examples/math_demo # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky +# https://px4.io/dev/px4_simple_app #MODULES += examples/px4_simple_app # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon +# https://px4.io/dev/daemon #MODULES += examples/px4_daemon_app # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values +# https://px4.io/dev/debug_values #MODULES += examples/px4_mavlink_debug # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +# https://px4.io/dev/example_fixedwing_control #MODULES += examples/fixedwing_control # Hardware test From dcf7b81f8321555a9e0c209cfaf59726dea154f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:48:48 +0200 Subject: [PATCH 03/11] Airspeed: fix code style --- src/drivers/airspeed/airspeed.cpp | 75 +++++++++++++++++++------------ 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index b1679f7c5726..772e9242594a 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file ets_airspeed.cpp - * @author Simon Wilks + * @file airspeed.cpp + * @author Simon Wilks + * @author Lorenz Meier * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ @@ -76,7 +77,7 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -105,12 +106,14 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -124,13 +127,16 @@ Airspeed::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -146,8 +152,9 @@ Airspeed::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub < 0) + if (_airspeed_pub < 0) { warnx("uORB started?"); + } } ret = OK; @@ -166,7 +173,7 @@ Airspeed::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -179,20 +186,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -202,13 +209,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -217,15 +225,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -233,21 +243,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -261,16 +275,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +301,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -369,6 +384,7 @@ Airspeed::update_status() if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,6 +418,7 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } From bac89be4b98869dce6899db2b18da4da52da471b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:48:58 +0200 Subject: [PATCH 04/11] Airspeed ETS: fix code style --- src/drivers/ets_airspeed/ets_airspeed.cpp | 77 +++++++++++++++-------- 1 file changed, 50 insertions(+), 27 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 4c75143acaaf..44d23e01de35 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -94,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH); protected: @@ -113,8 +113,8 @@ class ETSAirspeed : public Airspeed */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) +ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -155,15 +155,16 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - if (diff_pres_pa_raw == 0) { + + if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the // caller could end up using this value as part of an // average perf_count(_comms_errors); - log("zero value from sensor"); + log("zero value from sensor"); return -1; - } + } // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; @@ -175,7 +176,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); // XXX we may want to smooth out the readings to remove noise. report.differential_pressure_filtered_pa = diff_pres_pa_raw; @@ -210,6 +211,7 @@ ETSAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ @@ -239,6 +241,7 @@ ETSAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { debug("measure error"); } @@ -287,26 +290,31 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new ETSAirspeed(i2c_bus); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - 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(AIRSPEED0_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); @@ -351,21 +359,24 @@ test() int fd = open(ETS_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_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: %f pa", (double)report.differential_pressure_filtered_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++) { @@ -376,22 +387,25 @@ 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: %f pa", (double)report.differential_pressure_filtered_pa); } /* 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"); } @@ -404,14 +418,17 @@ reset() { int fd = open(ETS_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); } @@ -422,8 +439,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(); @@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { ets_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { ets_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ets_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { ets_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { ets_airspeed::info(); + } ets_airspeed_usage(); exit(0); From 135543f03f95f2860640d4145c01ebfe0480af47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:49:09 +0200 Subject: [PATCH 05/11] Airspeed MEAS: fix code style --- src/drivers/meas_airspeed/meas_airspeed.cpp | 27 ++++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 45684b5906e1..1db80769cab5 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,7 +138,7 @@ class MEASAirspeed : public Airspeed extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), + CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), _t_system_power(-1), system_power{} @@ -189,9 +189,11 @@ MEASAirspeed::collect() break; case 1: - /* fallthrough */ + + /* fallthrough */ case 2: - /* fallthrough */ + + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); @@ -219,11 +221,11 @@ MEASAirspeed::collect() are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ - float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa_raw, temperature); + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa_raw, temperature); // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; @@ -276,6 +278,7 @@ MEASAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); @@ -304,6 +307,7 @@ MEASAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { debug("measure error"); } @@ -332,18 +336,23 @@ void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); } + if (_t_system_power == -1) { // not available return; } + bool updated = false; orb_check(_t_system_power, &updated); + if (updated) { orb_copy(ORB_ID(system_power), _t_system_power, &system_power); } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { // not valid, skip correction return; @@ -354,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) apply a piecewise linear correction, flattening at 0.5V from 5V */ float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -0.5f) { voltage_diff = -0.5f; } + diff_press_pa -= voltage_diff * slope; /* @@ -367,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) */ const float temp_slope = 0.887f; voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } From 6bb941218caf3d8aa70bcb180b219d85024b8a91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:05:09 +0200 Subject: [PATCH 06/11] Limit stack size of HIL app launcher --- src/drivers/hil/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index f1fc49fb3d21..17d42d035127 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -39,4 +39,6 @@ MODULE_COMMAND = hil SRCS = hil.cpp +MODULE_STACKSIZE = 1200 + MAXOPTIMIZATION = -Os From 4a839c7e7ea575b434d701c0db7c87afd7ce07c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:05:32 +0200 Subject: [PATCH 07/11] HIL: Do not start GPS deamon --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b21ca9ec8892..1027d90eac94 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -289,6 +289,7 @@ then if ver hwcmp PX4FMU_V1 then set FMU_MODE serial + set GPS no fi fi unset HIL @@ -319,6 +320,7 @@ then gps start fi fi + unset GPS unset GPS_FAKE # Needs to be this early for in-air-restarts From b235420f1700a5b4cddebd29d8aba441b1e340b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:06:04 +0200 Subject: [PATCH 08/11] HIL: Limit stack size of HIL app --- src/drivers/hil/hil.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 0fbabaf2f187..3cf6d2032a4c 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -231,7 +231,7 @@ HIL::init() _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 1000, (main_t)&HIL::task_main_trampoline, nullptr); From 74b2d21e9474c07f408d3a00850dd486b3991eb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:33:04 +0200 Subject: [PATCH 09/11] HIL: Do not start GPS regardless of platform --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1027d90eac94..3d36fe346c41 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -286,10 +286,10 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set GPS no if ver hwcmp PX4FMU_V1 then set FMU_MODE serial - set GPS no fi fi unset HIL From 47aedba691c8ca6ab42dda5600dcf42fb2746b85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:27:07 +0200 Subject: [PATCH 10/11] TECS: Fix altitude reset logic --- src/lib/external_lgpl/tecs/tecs.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 9e47590fd246..3c59add8640d 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -62,6 +62,10 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< _integ3_state = baro_altitude; _integ2_state = 0.0f; _integ1_state = 0.0f; + + // Reset the filter state as we just switched from non-altitude control + // to altitude control mode + _states_initalized = false; } _update_50hz_last_usec = now; From 3ccb35f338541c0f0aaf864e6e4f6181c9da19cc Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 23 Aug 2015 13:44:42 +0200 Subject: [PATCH 11/11] fix --- msg/tecs_status.msg | 4 ++-- src/lib/external_lgpl/tecs/tecs.cpp | 5 ++--- src/lib/external_lgpl/tecs/tecs.h | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 6 files changed, 13 insertions(+), 14 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 7e9d6670c935..4dcd16b2ca89 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -23,7 +23,7 @@ float32 energyDistributionError float32 totalEnergyRateError float32 energyDistributionRateError -float32 throttle_sp -float32 pitch_sp +float32 throttle_integ +float32 pitch_integ uint8 mode diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 3c59add8640d..7607f71c16dd 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -620,9 +620,8 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.energy_error_integ = _integ6_state; _tecs_state.energy_distribution_error_integ = _integ7_state; - - _tecs_state.throttle_sp = _throttle_dem; - _tecs_state.pitch_sp = _pitch_dem; + _tecs_state.throttle_integ = _integ6_state; + _tecs_state.pitch_integ = _integ7_state; _update_pitch_throttle_last_usec = now; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 0fbda0fe9a3d..2599e7a15294 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -155,8 +155,8 @@ class __EXPORT TECS float total_energy_rate_error; float energy_distribution_error; float energy_distribution_rate_error; - float throttle_sp; - float pitch_sp; + float throttle_integ; + float pitch_integ; enum ECL_TECS_MODE mode; }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e14b84fb39fd..1666c4850e63 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1937,8 +1937,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.energyDistributionError = s.energy_distribution_error; t.energyDistributionRateError = s.energy_distribution_rate_error; - t.throttle_sp = s.throttle_sp; - t.pitch_sp = s.pitch_sp; + t.throttle_integ = s.throttle_integ; + t.pitch_integ = s.pitch_integ; if (_tecs_status_pub > 0) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ea843f920213..e00417797c4c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1886,8 +1886,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; - log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp; - log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp; + log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; + log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1363b37ba721..95d4d96d5cb1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -368,8 +368,8 @@ struct log_TECS_s { float totalEnergyRateError; float energyDistributionError; float energyDistributionRateError; - float throttle_sp; - float pitch_sp; + float pitch_integ; + float throttle_integ; uint8_t mode; }; @@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"),