From 1235c1f0bdd61bd5bb60df6a8e20c28b5dd4ac39 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 3 Nov 2015 08:30:14 +0100 Subject: [PATCH 001/624] gps re-init after loss --- .../position_estimator_inav/position_estimator_inav_main.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 04f3c4d918..d10ff2d0df 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -817,6 +817,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; + ref_inited = false; //if gps gets lost it has to init again to avoid jumps mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } From d04a4969e211a2fad5d2c18fdb266a39bd5d773a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 1 Nov 2015 23:43:46 -0800 Subject: [PATCH 002/624] Integrated DriverFramework Refacored ADCSIM Compilation succeeds. Link not yet fixed. Signed-off-by: Mark Charlebois --- .gitmodules | 3 + cmake/posix/px4_impl_posix.cmake | 1 + src/lib/DriverFramework | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 123 +++++------------- 4 files changed, 41 insertions(+), 87 deletions(-) create mode 160000 src/lib/DriverFramework diff --git a/.gitmodules b/.gitmodules index 178cf86771..d47e011047 100644 --- a/.gitmodules +++ b/.gitmodules @@ -25,3 +25,6 @@ [submodule "unittests/googletest"] path = unittests/googletest url = https://github.com/google/googletest.git +[submodule "src/lib/DriverFramework"] + path = src/lib/DriverFramework + url = https://github.com/mcharleb/DriverFramework.git diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index d51b5527d5..7dbf128780 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -164,6 +164,7 @@ function(px4_os_add_flags) src/lib/eigen src/platforms/posix/include mavlink/include/mavlink + src/lib/DriverFramework/framework/include ) if(UNIX AND APPLE) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework new file mode 160000 index 0000000000..06e2616237 --- /dev/null +++ b/src/lib/DriverFramework @@ -0,0 +1 @@ +Subproject commit 06e26162373848b9ceb9de8b9006a0ebe952ea6d diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index d9f0dd4f15..6d725344bf 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -65,36 +65,33 @@ #include +#include "SyncObj.hpp" +#include "VirtDriverObj.hpp" -class ADCSIM : public device::VDev +using namespace DriverFramework; + +#define ADC_BASE_DEV_PATH "/dev/adc" + +class ADCSIM : public VirtDriverObj { public: ADCSIM(uint32_t channels); - ~ADCSIM(); + virtual ~ADCSIM(); - virtual int init(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t len); - -protected: - virtual int open_first(device::file_t *filp); - virtual int close_last(device::file_t *filp); + static int read(DriverHandle &h, adc_msg_s *sample, size_t num_samples); private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; + WorkHandle _call; perf_counter_t _sample_perf; unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ /** work trampoline */ - static void _tick_trampoline(void *arg); + static void _tick_trampoline(void *arg, WorkHandle &h); /** worker function */ - void _tick(); + virtual void _measure(); /** * Sample a single channel and return the measured value. @@ -105,12 +102,11 @@ private: */ uint16_t _sample(unsigned channel); - // update system_power ORB topic, only on FMUv2 - void update_system_power(void); + SyncObj m_lock; }; ADCSIM::ADCSIM(uint32_t channels) : - VDev("adcsim", ADCSIM0_DEVICE_PATH), + VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -150,75 +146,34 @@ ADCSIM::~ADCSIM() } } -int -ADCSIM::init() +int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) { - DEVICE_DEBUG("init done"); + ADCSIM *me = DriverMgr::getDriverObjByHandle(h); + if (me) { + if (num_samples > me->_channel_count) { + num_samples = me->_channel_count; + } + size_t len = num_samples * sizeof(adc_msg_s); - /* create the device node */ - return VDev::init(); -} - -int -ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - return -ENOTTY; -} - -ssize_t -ADCSIM::read(device::file_t *filp, char *buffer, size_t len) -{ - const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - - if (len > maxsize) { - len = maxsize; + /* block interrupts while copying samples to avoid racing with an update */ + me->m_lock.lock(); + memcpy((void *)sample, (void *)(me->_samples), len); + me->m_lock.unlock(); + return num_samples; } - /* block interrupts while copying samples to avoid racing with an update */ - memcpy(buffer, _samples, len); - - return len; -} - -int -ADCSIM::open_first(device::file_t *filp) -{ - /* get fresh data */ - _tick(); - - /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); - - return 0; -} - -int -ADCSIM::close_last(device::file_t *filp) -{ - hrt_cancel(&_call); - return 0; + return -1; } void -ADCSIM::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); -} - -void -ADCSIM::_tick() +ADCSIM::_measure() { + m_lock.lock(); /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } - - update_system_power(); -} - -void -ADCSIM::update_system_power(void) -{ + m_lock.unlock(); } uint16_t @@ -245,19 +200,19 @@ int test(void) { - int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); - if (fd < 0) { - PX4_ERR("can't open ADCSIM device"); + if (!h.isValid()) { + PX4_ERR("can't open ADCSIM device (%d)", h.getError()); return 1; } for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = px4_read(fd, data, sizeof(data)); + ssize_t count = ADCSIM::read(h, data, sizeof(data)); if (count < 0) { - PX4_ERR("read error"); + PX4_ERR("read error (%d)", h.getError()); return 1; } @@ -270,7 +225,7 @@ test(void) usleep(500000); } - px4_close(fd); + DriverMgr::releaseHandle(h); return 0; } } @@ -288,12 +243,6 @@ adcsim_main(int argc, char *argv[]) PX4_ERR("couldn't allocate the ADCSIM driver"); return 1; } - - if (g_adc->init() != OK) { - delete g_adc; - PX4_ERR("ADCSIM init failed"); - return 1; - } } if (argc > 1) { From 143fd4aec65724e6278734113a747a50e8a15df1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 00:06:12 -0800 Subject: [PATCH 003/624] Adding in DriverFramework Signed-off-by: Mark Charlebois --- src/firmware/posix/CMakeLists.txt | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 3 +-- src/platforms/posix/main.cpp | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 64f4775c33..4efaa2164a 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -14,6 +14,7 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} + ${driver_framework_libraries} pthread m rt -Wl,--end-group ) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 6d725344bf..0efbc94225 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -199,7 +199,6 @@ ADCSIM *g_adc; int test(void) { - DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); if (!h.isValid()) { @@ -245,7 +244,7 @@ adcsim_main(int argc, char *argv[]) } } - if (argc > 1) { + if (argc > 1 && g_adc) { if (!strcmp(argv[1], "test")) { ret = test(); } diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1b22627721..56301b603c 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -43,6 +43,9 @@ #include #include #include +#include "apps.h" +#include "px4_middleware.h" +#include "DriverFramework.hpp" namespace px4 { @@ -53,9 +56,6 @@ using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); -#include "apps.h" -#include "px4_middleware.h" - static bool _ExitFlag = false; extern "C" { void _SigIntHandler(int sig_num); @@ -184,6 +184,7 @@ int main(int argc, char **argv) } if (!error_detected) { + DriverFramework::Framework::initialize(); px4::init_once(); px4::init(argc, argv, "mainapp"); @@ -236,5 +237,6 @@ int main(int argc, char **argv) vector shutdown_cmd = { "shutdown" }; run_cmd(shutdown_cmd, true); + DriverFramework::Framework::shutdown(); } } From 1b7a6fffcaff8b08738d54e909e80e5990da2155 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 00:11:34 -0800 Subject: [PATCH 004/624] Removed unussed function Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 0efbc94225..f71f41d52f 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -87,9 +87,6 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ - /** work trampoline */ - static void _tick_trampoline(void *arg, WorkHandle &h); - /** worker function */ virtual void _measure(); From ce97aea1bf999002f0b91629cb82db15469b6faf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 20:59:41 -0800 Subject: [PATCH 005/624] Added Gyrosim Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 41 ++- .../posix/drivers/gyrosim/gyrosim.cpp | 246 +++++++----------- 3 files changed, 111 insertions(+), 178 deletions(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 06e2616237..3ddb6b5e34 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 06e26162373848b9ceb9de8b9006a0ebe952ea6d +Subproject commit 3ddb6b5e34d870b487dbd57949a7d9c718730701 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index f71f41d52f..a3d28f6d41 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -66,19 +66,19 @@ #include #include "SyncObj.hpp" -#include "VirtDriverObj.hpp" +#include "VirtDevObj.hpp" using namespace DriverFramework; #define ADC_BASE_DEV_PATH "/dev/adc" -class ADCSIM : public VirtDriverObj +class ADCSIM : public VirtDevObj { public: ADCSIM(uint32_t channels); virtual ~ADCSIM(); - static int read(DriverHandle &h, adc_msg_s *sample, size_t num_samples); + virtual ssize_t read(void *buffer, ssize_t len); private: WorkHandle _call; @@ -103,7 +103,7 @@ private: }; ADCSIM::ADCSIM(uint32_t channels) : - VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), + VirtDevObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -143,23 +143,21 @@ ADCSIM::~ADCSIM() } } -int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) +ssize_t +ADCSIM::read(void *buffer, ssize_t len) { - ADCSIM *me = DriverMgr::getDriverObjByHandle(h); - if (me) { - if (num_samples > me->_channel_count) { - num_samples = me->_channel_count; - } - size_t len = num_samples * sizeof(adc_msg_s); - - /* block interrupts while copying samples to avoid racing with an update */ - me->m_lock.lock(); - memcpy((void *)sample, (void *)(me->_samples), len); - me->m_lock.unlock(); - return num_samples; + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) { + len = maxsize; } + + /* block interrupts while copying samples to avoid racing with an update */ + m_lock.lock(); + memcpy(buffer, _samples, len); + m_lock.unlock(); - return -1; + return len; } void @@ -196,7 +194,8 @@ ADCSIM *g_adc; int test(void) { - DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); + DevHandle h; + DevMgr::getHandle(ADCSIM0_DEVICE_PATH, h); if (!h.isValid()) { PX4_ERR("can't open ADCSIM device (%d)", h.getError()); @@ -205,7 +204,7 @@ test(void) for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = ADCSIM::read(h, data, sizeof(data)); + ssize_t count = h.read(data, sizeof(data)); if (count < 0) { PX4_ERR("read error (%d)", h.getError()); @@ -221,7 +220,7 @@ test(void) usleep(500000); } - DriverMgr::releaseHandle(h); + DevMgr::releaseHandle(h); return 0; } } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0ac023285c..430e13a00a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -69,13 +69,17 @@ #include #include -#include +//#include #include -#include -#include +//#include +//#include #include #include +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -114,16 +118,17 @@ */ class GYROSIM_gyro; -class GYROSIM : public device::VDev +class GYROSIM : public VirtDevObj { public: GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation); virtual ~GYROSIM(); - virtual int init(); + int init(); + virtual int start(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -136,14 +141,14 @@ public: protected: friend class GYROSIM_gyro; - virtual ssize_t gyro_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t gyro_read(void *buffer, size_t buflen); + virtual int gyro_ioctl(unsigned long cmd, void *arg); private: GYROSIM_gyro *_gyro; uint8_t _product; /** product code */ - struct hrt_call _call; + WorkHandle _call; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -175,16 +180,6 @@ private: // last temperature reading for print_info() float _last_temperature; - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -192,21 +187,10 @@ private: */ int reset(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - /** * Fetch measurements from the sensor and update the report buffers. */ - void measure(); + virtual void _measure(); /** * Read a register from the GYROSIM @@ -286,19 +270,21 @@ private: #pragma pack(pop) uint8_t _regdata[108]; + + bool _pub_blocked = true; // Don't publish until initialized }; /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public device::VDev +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); virtual int init(); @@ -307,6 +293,7 @@ protected: void parent_poll_notify(); + virtual void _measure(); private: GYROSIM *_parent; orb_advert_t _gyro_topic; @@ -322,11 +309,9 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VDev("GYROSIM", path_accel), + VirtDevObj("GYROSIM", path_accel, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), - _call{}, - _call_interval(0), _accel_reports(nullptr), _accel_scale{}, _accel_range_scale(0.0f), @@ -349,17 +334,12 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _rotation(rotation), _last_temperature(0) { - // disable debug() calls - _debug_enabled = false; - // Don't publish until initialized - _pub_blocked = true; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ - _gyro->_device_id.devid = _device_id.devid; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; + _gyro->m_id.dev_id = m_id.dev_id; + _gyro->m_id.dev_id_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; // default accel scale factors _accel_scale.x_offset = 0; @@ -376,8 +356,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } GYROSIM::~GYROSIM() @@ -397,10 +375,6 @@ GYROSIM::~GYROSIM() delete _gyro_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); @@ -413,15 +387,6 @@ GYROSIM::init() { int ret; - /* do VDevinit first */ - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_WARN("VDev setup failed"); - return ret; - } - struct accel_report arp = {}; struct gyro_report grp = {}; @@ -467,13 +432,13 @@ GYROSIM::init() /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); + PX4_ERR("gyro init failed"); return ret; } - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + start(); - measure(); + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -571,13 +536,11 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) _sample_rate = 1000 / div; PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - _call_interval = 1000000 / _sample_rate; - hrt_cancel(&_call); - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + setSampleInterval(1000000 / _sample_rate); } ssize_t -GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(accel_report); @@ -589,7 +552,7 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _accel_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -620,7 +583,7 @@ int GYROSIM::self_test() { if (perf_event_count(_sample_perf) == 0) { - measure(); + _measure(); } /* return 0 on success, 1 else */ @@ -724,7 +687,7 @@ GYROSIM::gyro_self_test() } ssize_t -GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::gyro_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(gyro_report); @@ -736,7 +699,7 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _gyro_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -764,15 +727,17 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } int -GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::devIOCTL(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCRESET: return reset(); case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -789,15 +754,15 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)1000); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned ticks = 1000000 / ul_arg; /* check against maximum sane rate */ if (ticks < 1000) { @@ -829,11 +794,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_accel_reports->resize(arg)) { + if (!_accel_reports->resize(ul_arg)) { return -ENOMEM; } @@ -847,7 +812,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _sample_rate; case ACCELIOCSSAMPLERATE: - _set_sample_rate(arg); + _set_sample_rate(ul_arg); return OK; case ACCELIOCSLOWPASS: @@ -855,7 +820,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; + struct accel_scale *s = (struct accel_scale *) ul_arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -869,11 +834,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) ul_arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: - return set_accel_range(arg); + return set_accel_range(ul_arg); case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); @@ -883,28 +848,30 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } int -GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + return devIOCTL(cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_gyro_reports->resize(arg)) { + if (!_gyro_reports->resize(ul_arg)) { return -ENOMEM; } @@ -918,7 +885,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) return _sample_rate; case GYROIOCSSAMPLERATE: - _set_sample_rate(arg); + _set_sample_rate(ul_arg); return OK; case GYROIOCSLOWPASS: @@ -949,7 +916,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } @@ -1020,7 +987,7 @@ GYROSIM::set_accel_range(unsigned max_g_in) return OK; } -void +int GYROSIM::start() { /* make sure we are stopped first */ @@ -1031,28 +998,11 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - if (_call_interval > 0) { - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); - } + return DevObj::start(); } void -GYROSIM::stop() -{ - hrt_cancel(&_call); -} - -void -GYROSIM::measure_trampoline(void *arg) -{ - GYROSIM *dev = reinterpret_cast(arg); - - /* make another measurement */ - dev->measure(); -} - -void -GYROSIM::measure() +GYROSIM::_measure() { #if 0 @@ -1152,8 +1102,9 @@ GYROSIM::measure() /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + // FIXME + //poll_notify(POLLIN); + //_gyro->parent_poll_notify(); if (!(_pub_blocked)) { /* log the time of this report */ @@ -1211,7 +1162,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : - VDev("GYROSIM_gyro", path), + VirtDevObj("GYROSIM_gyro", path, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1), @@ -1219,55 +1170,37 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : { } -GYROSIM_gyro::~GYROSIM_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} - int GYROSIM_gyro::init() { - int ret; - - // do base class init - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - - return ret; + return start(); } +#if 0 void GYROSIM_gyro::parent_poll_notify() { poll_notify(POLLIN); } +#endif ssize_t -GYROSIM_gyro::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM_gyro::devRead(void *buffer, size_t buflen) { - return _parent->gyro_read(filp, buffer, buflen); + return _parent->gyro_read(buffer, buflen); } int -GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM_gyro::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + return (int)VirtDevObj::devIOCTL(cmd, arg); break; default: - return _parent->gyro_ioctl(filp, cmd, arg); + return _parent->gyro_ioctl(cmd, arg); } } @@ -1375,29 +1308,31 @@ test() ssize_t sz; /* get the driver */ - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h_accel; + DevMgr::getHandle(path_accel, h_accel); - if (fd < 0) { + if (!h_accel.isValid()) { PX4_ERR("%s open failed (try 'gyrosim start')", path_accel); return 1; } /* get the driver */ - int fd_gyro = px4_open(path_gyro, O_RDONLY); + DevHandle h_gyro; + DevMgr::getHandle(path_gyro, h_gyro); - if (fd_gyro < 0) { + if (!h_gyro.isValid()) { PX4_ERR("%s open failed", path_gyro); return 1; } /* reset to manual polling */ - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + if (h_accel.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MANUAL) < 0) { PX4_ERR("reset to manual polling"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &a_report, sizeof(a_report)); + sz = h_accel.read(&a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1417,7 +1352,7 @@ test() (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); + sz = h_gyro.read(&g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); @@ -1440,11 +1375,12 @@ test() /* XXX add poll-rate tests here too */ - px4_close(fd); + // Destructor would clean the up too + DevMgr::releaseHandle(h_accel); + DevMgr::releaseHandle(h_gyro); reset(); PX4_INFO("PASS"); - return 0; } @@ -1454,30 +1390,28 @@ test() int reset() { - const char *path_accel = MPU_DEVICE_PATH_ACCEL; - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h; + DevMgr::getHandle(MPU_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("reset failed"); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, (void *)0) < 0) { PX4_ERR("driver reset failed"); goto reset_fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); goto reset_fail; } - px4_close(fd); return 0; reset_fail: - px4_close(fd); return 1; } From 596a2789d6c965c5fa63e9b10b240de35cb2ba41 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 3 Nov 2015 01:24:02 -0800 Subject: [PATCH 006/624] gyrosim and adcsim now compile with DriverFramework enabled Only the posix build is tested for compilation Signed-off-by: Mark Charlebois --- CMakeLists.txt | 4 +++ src/firmware/posix/CMakeLists.txt | 3 +- src/lib/DriverFramework | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 5 ++-- .../posix/drivers/gyrosim/gyrosim.cpp | 29 +++++++++---------- 5 files changed, 22 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 05a3cd59b3..7538d910a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,9 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) + #============================================================================= # parameters # @@ -319,6 +322,7 @@ foreach(module ${config_module_list}) endforeach() add_subdirectory(src/firmware/${OS}) +add_subdirectory(src/lib/DriverFramework) if (config_io_board) add_subdirectory(src/modules/px4iofirmware) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 4efaa2164a..36bc434a77 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -14,7 +14,8 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} - ${driver_framework_libraries} + df_driver_framework + ${df_driver_libraries} pthread m rt -Wl,--end-group ) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 3ddb6b5e34..c2afd87807 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 3ddb6b5e34d870b487dbd57949a7d9c718730701 +Subproject commit c2afd878071e8fb0b1d80070e44ba98780492fc1 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index a3d28f6d41..66372ad214 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -78,10 +78,9 @@ public: ADCSIM(uint32_t channels); virtual ~ADCSIM(); - virtual ssize_t read(void *buffer, ssize_t len); + virtual ssize_t devRead(void *buffer, size_t len); private: - WorkHandle _call; perf_counter_t _sample_perf; unsigned _channel_count; @@ -144,7 +143,7 @@ ADCSIM::~ADCSIM() } ssize_t -ADCSIM::read(void *buffer, ssize_t len) +ADCSIM::devRead(void *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 430e13a00a..7aef8cf392 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -69,10 +69,10 @@ #include #include -//#include +#include #include -//#include -//#include +#include +#include #include #include @@ -277,11 +277,11 @@ private: /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public VirtDevObj +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - virtual ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro() {} virtual ssize_t devRead(void *buffer, size_t buflen); virtual int devIOCTL(unsigned long cmd, void *arg); @@ -293,12 +293,11 @@ protected: void parent_poll_notify(); - virtual void _measure(); + virtual void _measure() {}; private: GYROSIM *_parent; orb_advert_t _gyro_topic; int _gyro_orb_class_instance; - int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ GYROSIM_gyro(const GYROSIM_gyro &); @@ -385,7 +384,7 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { - int ret; + int ret = 1; struct accel_report arp = {}; @@ -1102,9 +1101,8 @@ GYROSIM::_measure() /* notify anyone waiting for data */ - // FIXME - //poll_notify(POLLIN); - //_gyro->parent_poll_notify(); + updateNotify(); + _gyro->parent_poll_notify(); if (!(_pub_blocked)) { /* log the time of this report */ @@ -1162,27 +1160,26 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : + // Set sample interval to 0 since device is read by parent VirtDevObj("GYROSIM_gyro", path, 0), _parent(parent), _gyro_topic(nullptr), - _gyro_orb_class_instance(-1), - _gyro_class_instance(-1) + _gyro_orb_class_instance(-1) { } + int GYROSIM_gyro::init() { return start(); } -#if 0 void GYROSIM_gyro::parent_poll_notify() { - poll_notify(POLLIN); + updateNotify(); } -#endif ssize_t GYROSIM_gyro::devRead(void *buffer, size_t buflen) From 6e19d9cb841d2e0349b9ec40a4de70d78bd9b53f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 3 Nov 2015 01:27:22 -0800 Subject: [PATCH 007/624] Commened out manditory use of Clang-3.6 Signed-off-by: Mark Charlebois --- CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7538d910a0..681bb8f328 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,8 +121,9 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) -SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) -SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) +# Use clang +#SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +#SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) #============================================================================= # parameters From a71f00686bcdfe0201e760349c2ba7df3ce457dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 11:06:28 +0100 Subject: [PATCH 008/624] Register the driver framework correctly --- CMakeLists.txt | 1 + src/platforms/posix/drivers/adcsim/CMakeLists.txt | 1 + src/platforms/posix/drivers/gyrosim/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 681bb8f328..8893341009 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -234,6 +234,7 @@ px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index 93cdd95a0f..fc6802b0c4 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_module( adcsim.cpp DEPENDS platforms__common + git_driverframework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index 335bee65d6..f776b34bea 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( gyrosim.cpp DEPENDS platforms__common + git_driverframework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From a71f83e30fd3f85995d55975213da0b033a200be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 11:53:26 +0100 Subject: [PATCH 009/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c2afd87807..2c2e80b5cc 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c2afd878071e8fb0b1d80070e44ba98780492fc1 +Subproject commit 2c2e80b5cc39ce5b96d3be19a1e1db3370ba7e85 From f4d9521fd67c54dc2962933db9a7b55d22ea300e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:05:19 +0100 Subject: [PATCH 010/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 2c2e80b5cc..dc52ed6d98 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 2c2e80b5cc39ce5b96d3be19a1e1db3370ba7e85 +Subproject commit dc52ed6d98ce007f86f6e0b7521636337753a239 From 97ee033fe0aa82a21b0e9566adb6355055a3cf85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:44:12 +0100 Subject: [PATCH 011/624] DriverFramework update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index dc52ed6d98..080a7cdccd 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit dc52ed6d98ce007f86f6e0b7521636337753a239 +Subproject commit 080a7cdccd283cea79ce7908774391aac60a41fa From b1f2812862775d2e84082e37a35de2cc130b05bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:51:05 +0100 Subject: [PATCH 012/624] Updated framework URL --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index d47e011047..da67485fe8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -27,4 +27,4 @@ url = https://github.com/google/googletest.git [submodule "src/lib/DriverFramework"] path = src/lib/DriverFramework - url = https://github.com/mcharleb/DriverFramework.git + url = https://github.com/PX4/DriverFramework.git From d9a7dd04a4b5c8625c9455914db364b5c0229e46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:52:04 +0100 Subject: [PATCH 013/624] Updated driver framework repo --- src/platforms/posix/drivers/adcsim/CMakeLists.txt | 6 ++++++ src/platforms/posix/drivers/gyrosim/CMakeLists.txt | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index fc6802b0c4..c1c925c15c 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -40,5 +40,11 @@ px4_add_module( DEPENDS platforms__common git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__adcsim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index f776b34bea..a1b42bd11c 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -41,5 +41,11 @@ px4_add_module( DEPENDS platforms__common git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__gyrosim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : From 2ec6363f67408dcfb425d56aac59cdc211a7cc4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:56:19 +0100 Subject: [PATCH 014/624] Fixed adc sim formatting --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 66372ad214..e8f1c592ec 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -146,11 +146,11 @@ ssize_t ADCSIM::devRead(void *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - + if (len > maxsize) { len = maxsize; } - + /* block interrupts while copying samples to avoid racing with an update */ m_lock.lock(); memcpy(buffer, _samples, len); @@ -163,10 +163,12 @@ void ADCSIM::_measure() { m_lock.lock(); + /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } + m_lock.unlock(); } From ed4e602511c3d86a1f67fc254dd54cb02461b4d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:57:44 +0100 Subject: [PATCH 015/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 080a7cdccd..e1582be979 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 080a7cdccd283cea79ce7908774391aac60a41fa +Subproject commit e1582be979cb903ef81ebd150b7667fde91763fe From 3b0f18e96d41f1d67866b71c5ce1e108db1159d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:13:25 +0100 Subject: [PATCH 016/624] Fix missing rt lib --- cmake/posix/px4_impl_posix.cmake | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index 7dbf128780..28b27f87d1 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -176,6 +176,10 @@ if(UNIX AND APPLE) -include ${PX4_INCLUDE_DIR}visibility.h ) + set(added_exe_linker_flags + -lpthread + ) + else() set(added_definitions @@ -185,11 +189,12 @@ else() -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h ) -endif() set(added_exe_linker_flags - -lpthread + -lpthread -lrt ) + +endif() # Add the toolchain specific flags From 8e7a4725c6d0406d3b2ed4104fec4af7e69d0053 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:14:26 +0100 Subject: [PATCH 017/624] Driverframework: Update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index e1582be979..c07f902805 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit e1582be979cb903ef81ebd150b7667fde91763fe +Subproject commit c07f9028054bdad970c6283066703c59c4b3a35a From 28e08d31ad79c3e51698dea0c757ba97c295c0ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:16:28 +0100 Subject: [PATCH 018/624] Travis CI: Select CLANG --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index e6b5239344..c94da476e4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,6 +3,8 @@ language: cpp +compiler: clang + matrix: fast_finish: true include: From 4e295e1549b44d58166b8b506fe24e9b4889deac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:17:27 +0100 Subject: [PATCH 019/624] Revert CLANG --- .travis.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index c94da476e4..e6b5239344 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,8 +3,6 @@ language: cpp -compiler: clang - matrix: fast_finish: true include: From 450ad27b52141d98d7acae771fd3d22ec9528a05 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:18:11 +0100 Subject: [PATCH 020/624] Update Driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c07f902805..89d4bb5725 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c07f9028054bdad970c6283066703c59c4b3a35a +Subproject commit 89d4bb57258dcfc78c48ed3466d77fdbc0b4471e From d9ba63a41f06010cedd9595d75b410cf769cdd25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:49:43 +0100 Subject: [PATCH 021/624] Add C++ flags --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 89d4bb5725..ca4970ab63 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 89d4bb57258dcfc78c48ed3466d77fdbc0b4471e +Subproject commit ca4970ab6329110a28272fb808539333a38dc173 From cf8e289bcec75b605956ec5deec68e38ea1e5bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:54:06 +0100 Subject: [PATCH 022/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index ca4970ab63..d58b46695b 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit ca4970ab6329110a28272fb808539333a38dc173 +Subproject commit d58b46695b702173459465f862d52a216229772b From 9c4f8f56acf92c41f141a3cd86995ff62ad0dceb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:57:29 +0100 Subject: [PATCH 023/624] Fix Cxx errors --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index d58b46695b..32786683a0 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit d58b46695b702173459465f862d52a216229772b +Subproject commit 32786683a0a0cbe3003fd55d6fc6c61999707b95 From 8b80a24d9d7d1159763d2b3fb208d4efb8f26fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Nov 2015 09:01:36 +0100 Subject: [PATCH 024/624] Update driver framework version --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 32786683a0..c4be5e70b3 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 32786683a0a0cbe3003fd55d6fc6c61999707b95 +Subproject commit c4be5e70b36c86868520472f93fd945d7f0d8986 From 40c079efa8f7d8cc402890384cb9ce4e01f6a1c7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 9 Nov 2015 10:26:28 -0800 Subject: [PATCH 025/624] Ported accelsim, gyrosim and adcsim to DriverFramework Need to handle _pub_blocked flag in DriverFramework as a PX4 specific implementation. Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 250 +++++------------- src/platforms/posix/drivers/adcsim/adcsim.cpp | 2 +- .../posix/drivers/gyrosim/gyrosim.cpp | 20 +- 3 files changed, 84 insertions(+), 188 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 5017bb111c..ca231bffb9 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include #include @@ -63,15 +62,9 @@ #include #include #include - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include #define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" -#define ACCELSIM_DEVICE_PATH_ACCEL_EXT "/dev/sim_accel_ext" #define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" #define ADDR_WHO_AM_I 0x0F @@ -87,10 +80,11 @@ static const int ERROR = -1; extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } +using namespace DriverFramework; class ACCELSIM_mag; -class ACCELSIM : public device::VDev +class ACCELSIM : public VirtDevObj { public: ACCELSIM(const char *path, enum Rotation rotation); @@ -98,8 +92,8 @@ public: virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); /** * dump register values @@ -109,8 +103,8 @@ public: protected: friend class ACCELSIM_mag; - ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); - int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t mag_read(void *buffer, size_t buflen); + int mag_ioctl(unsigned long cmd, void *arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -123,8 +117,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - ringbuffer::RingBuffer *_accel_reports; - ringbuffer::RingBuffer *_mag_reports; + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -168,14 +162,9 @@ private: // reset /** - * Start automatic measurement. + * Override Start automatic measurement. */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); + virtual int start(); /** * Reset chip. @@ -184,28 +173,10 @@ private: */ void reset(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); - /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + virtual void _measure(); /** * Fetch mag measurements from the sensor and update the report ring. @@ -267,7 +238,7 @@ private: /** * Helper class implementing the mag driver node. */ -class ACCELSIM_mag : public device::VDev +class ACCELSIM_mag : public VirtDevObj { public: ACCELSIM_mag(ACCELSIM *parent); @@ -288,9 +259,7 @@ private: int _mag_orb_class_instance; int _mag_class_instance; - void measure(); - - void measure_trampoline(void *arg); + virtual void _measure(); /* this class does not allow copying due to ptr data members */ ACCELSIM_mag(const ACCELSIM_mag &); @@ -299,7 +268,7 @@ private: ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : - VDev("ACCELSIM", path), + VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1000), _mag(new ACCELSIM_mag(this)), _accel_call{}, _mag_call{}, @@ -333,15 +302,11 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _constant_accel_count(0), _last_temperature(0) { - // enable debug() calls - _debug_enabled = false; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; /* Prime _mag with parents devid. */ - _mag->_device_id.devid = _device_id.devid; - _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; - + _mag->m_id.dev_id = m_id.dev_id; + _mag->m_id.dev_id_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; // default scale factors _accel_scale.x_offset = 0.0f; @@ -373,10 +338,6 @@ ACCELSIM::~ACCELSIM() delete _mag_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - delete _mag; /* delete the perf counter */ @@ -390,13 +351,13 @@ ACCELSIM::~ACCELSIM() int ACCELSIM::init() { - int ret = ERROR; + int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; /* do SIM init first */ - if (VDev::init() != OK) { + if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } @@ -416,7 +377,7 @@ ACCELSIM::init() reset(); - /* do VDev init for the mag device node */ + /* do init for the mag device node */ ret = _mag->init(); if (ret != OK) { @@ -425,7 +386,7 @@ ACCELSIM::init() } /* fill report structures */ - measure(); + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _mag_reports->get(&mrp); @@ -438,9 +399,6 @@ ACCELSIM::init() PX4_WARN("ADVERT ERR"); } - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -490,7 +448,7 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } ssize_t -ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); accel_report *arb = reinterpret_cast(buffer); @@ -518,7 +476,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* manual measurement */ - measure(); + _measure(); /* measurement will have generated a report, copy it out */ if (_accel_reports->get(arb)) { @@ -529,7 +487,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } ssize_t -ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM::mag_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast(buffer); @@ -559,7 +517,7 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) /* manual measurement */ _mag_reports->flush(); - _mag->measure(); + _mag->_measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) { @@ -570,12 +528,13 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) } int -ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM::devIOCTL(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -592,10 +551,10 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -603,7 +562,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + unsigned period = 1000000 / ul_arg; /* check against maximum sane rate */ if (period < 500) { @@ -611,7 +570,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -636,11 +595,11 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_accel_reports->resize(arg)) { + if (!_accel_reports->resize(ul_arg)) { return -ENOMEM; } @@ -655,13 +614,13 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSSAMPLERATE: - return accel_set_samplerate(arg); + return accel_set_samplerate(ul_arg); case ACCELIOCGSAMPLERATE: return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)ul_arg); } case ACCELIOCSSCALE: { @@ -680,7 +639,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSRANGE: /* arg needs to be in G */ - return accel_set_range(arg); + return accel_set_range(ul_arg); case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ @@ -688,7 +647,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &(_accel_scale), sizeof(_accel_scale)); return OK; case ACCELIOCSELFTEST: @@ -696,17 +655,18 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } int -ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM::mag_ioctl(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -725,7 +685,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); + return mag_ioctl(SENSORIOCSPOLLRATE, (void *)100); /* adjust to a legal polling interval in Hz */ default: { @@ -733,7 +693,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_mag_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + unsigned period = 1000000 / ul_arg; /* check against maximum sane rate (1ms) */ if (period < 10000) { @@ -765,11 +725,11 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_mag_reports->resize(arg)) { + if (!_mag_reports->resize(ul_arg)) { return -ENOMEM; } @@ -784,7 +744,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; case MAGIOCSSAMPLERATE: - return mag_set_samplerate(arg); + return mag_set_samplerate(ul_arg); case MAGIOCGSAMPLERATE: return _mag_samplerate; @@ -805,7 +765,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; case MAGIOCSRANGE: - return mag_set_range(arg); + return mag_set_range(ul_arg); case MAGIOCGRANGE: return _mag_range_ga; @@ -822,7 +782,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } @@ -868,7 +828,7 @@ ACCELSIM::mag_set_samplerate(unsigned frequency) return OK; } -void +int ACCELSIM::start() { /* make sure we are stopped first */ @@ -878,49 +838,11 @@ ACCELSIM::start() _accel_reports->flush(); _mag_reports->flush(); - /* start polling at the specified rate */ - //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); - - // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0 - if (_call_mag_interval == 0) { - //PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0"); - _call_mag_interval = 10000; // Max 100Hz - } - - //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this); + return VirtDevObj::start(); } void -ACCELSIM::stop() -{ - hrt_cancel(&_accel_call); - hrt_cancel(&_mag_call); -} - -void -ACCELSIM::measure_trampoline(void *arg) -{ - //PX4_INFO("ACCELSIM::measure_trampoline"); - ACCELSIM *dev = (ACCELSIM *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -ACCELSIM::mag_measure_trampoline(void *arg) -{ - //PX4_INFO("ACCELSIM::mag_measure_trampoline"); - ACCELSIM *dev = (ACCELSIM *)arg; - - /* make another measurement */ - dev->mag_measure(); -} - -void -ACCELSIM::measure() +ACCELSIM::_measure() { /* status register and data as read back from the device */ @@ -989,7 +911,7 @@ ACCELSIM::measure() _accel_reports->force(&accel_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this); if (!(_pub_blocked)) { /* publish it */ @@ -1078,7 +1000,7 @@ ACCELSIM::mag_measure() _mag_reports->force(&mag_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this) if (!(_pub_blocked)) { /* publish it */ @@ -1092,7 +1014,7 @@ ACCELSIM::mag_measure() } ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : - VDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG), + VirtDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), _parent(parent), _mag_topic(nullptr), _mag_orb_class_instance(-1), @@ -1102,59 +1024,33 @@ ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : ACCELSIM_mag::~ACCELSIM_mag() { - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } -} - -int -ACCELSIM_mag::init() -{ - int ret; - - ret = VDev::init(); - - if (ret != OK) { - goto out; - } - - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - -out: - return ret; } ssize_t -ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM_mag::devRead(char *buffer, size_t buflen) { - return _parent->mag_read(filp, buffer, buflen); + return _parent->mag_read(buffer, buflen); } int -ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + return (int)VirtDevObj::ioctl(cmd, arg); break; default: - return _parent->mag_ioctl(filp, cmd, arg); + return _parent->mag_ioctl(cmd, arg); } } void -ACCELSIM_mag::measure() +ACCELSIM_mag::_measure() { _parent->mag_measure(); } -void -ACCELSIM_mag::measure_trampoline(void *arg) -{ - _parent->mag_measure_trampoline(arg); -} - /** * Local functions in support of the shell command. */ @@ -1176,8 +1072,6 @@ void usage(); int start(enum Rotation rotation) { - int fd, fd_mag; - if (g_dev != nullptr) { PX4_WARN("already started"); return 0; @@ -1197,24 +1091,26 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); + DevHandle h; + DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); goto fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + DevMgr::releaseHandle(h); goto fail; } - fd_mag = px4_open(ACCELSIM_DEVICE_PATH_MAG, O_RDONLY); + DevHandle h_mag; + DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ - if (0 <= fd_mag) { - if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (!h_mag.isValid()) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } @@ -1222,8 +1118,8 @@ start(enum Rotation rotation) PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - px4_close(fd); - px4_close(fd_mag); + DevMgr::releaseHandle(h); + DevMgr::releaseHandle(h_mag); return 0; fail: diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index e8f1c592ec..4326314438 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -102,7 +102,7 @@ private: }; ADCSIM::ADCSIM(uint32_t channels) : - VirtDevObj("adcsim", ADC_BASE_DEV_PATH, 10000), + VirtDevObj("adcsim", "/dev/adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 7aef8cf392..52e3254b7f 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -308,7 +308,7 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VirtDevObj("GYROSIM", path_accel, 1000), + VirtDevObj("GYROSIM", path_accel, nullptr, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), _accel_reports(nullptr), @@ -484,7 +484,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) { + if (sim == nullptr) { PX4_WARN("failed accessing simulator"); return ENODEV; } @@ -1161,7 +1161,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : // Set sample interval to 0 since device is read by parent - VirtDevObj("GYROSIM_gyro", path, 0), + VirtDevObj("GYROSIM_gyro", path, nullptr, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1) @@ -1226,10 +1226,10 @@ void usage(); int start(enum Rotation rotation) { - int fd; GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; + DevHandle h; if (*g_dev_ptr != nullptr) { /* if already started, the still command succeeded */ @@ -1249,18 +1249,18 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(path_accel, O_RDONLY); + DevMgr::getHandle(path_accel, h); - if (fd < 0) { + if (!h.isValid()) { goto fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + DevMgr::releaseHandle(h); goto fail; } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; fail: @@ -1469,7 +1469,7 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - const char *myoptarg = NULL; + const char *myoptarg = nullptr; while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { From 7ddd5712c53da99715ff14602510f44fe56fe8b6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 1 Nov 2015 23:43:46 -0800 Subject: [PATCH 026/624] Integrated DriverFramework Refacored ADCSIM Compilation succeeds. Link not yet fixed. Signed-off-by: Mark Charlebois --- .gitmodules | 3 + cmake/posix/px4_impl_posix.cmake | 1 + src/lib/DriverFramework | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 124 +++++------------- 4 files changed, 41 insertions(+), 88 deletions(-) create mode 160000 src/lib/DriverFramework diff --git a/.gitmodules b/.gitmodules index 744dcd2787..60cdcb1105 100644 --- a/.gitmodules +++ b/.gitmodules @@ -28,3 +28,6 @@ [submodule "src/lib/matrix"] path = src/lib/matrix url = https://github.com/PX4/Matrix.git +[submodule "src/lib/DriverFramework"] + path = src/lib/DriverFramework + url = https://github.com/PX4/DriverFramework.git diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index d51b5527d5..7dbf128780 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -164,6 +164,7 @@ function(px4_os_add_flags) src/lib/eigen src/platforms/posix/include mavlink/include/mavlink + src/lib/DriverFramework/framework/include ) if(UNIX AND APPLE) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework new file mode 160000 index 0000000000..06e2616237 --- /dev/null +++ b/src/lib/DriverFramework @@ -0,0 +1 @@ +Subproject commit 06e26162373848b9ceb9de8b9006a0ebe952ea6d diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index c72cc1a494..6d725344bf 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -65,36 +65,33 @@ #include +#include "SyncObj.hpp" +#include "VirtDriverObj.hpp" -class ADCSIM : public device::VDev +using namespace DriverFramework; + +#define ADC_BASE_DEV_PATH "/dev/adc" + +class ADCSIM : public VirtDriverObj { public: ADCSIM(uint32_t channels); - ~ADCSIM(); + virtual ~ADCSIM(); - virtual int init(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t len); - -protected: - virtual int open_first(device::file_t *filp); - virtual int close_last(device::file_t *filp); + static int read(DriverHandle &h, adc_msg_s *sample, size_t num_samples); private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; + WorkHandle _call; perf_counter_t _sample_perf; unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ /** work trampoline */ - static void _tick_trampoline(void *arg); + static void _tick_trampoline(void *arg, WorkHandle &h); /** worker function */ - void _tick(); + virtual void _measure(); /** * Sample a single channel and return the measured value. @@ -105,13 +102,11 @@ private: */ uint16_t _sample(unsigned channel); - // update system_power ORB topic, only on FMUv2 - void update_system_power(void); + SyncObj m_lock; }; ADCSIM::ADCSIM(uint32_t channels) : - VDev("adcsim", ADCSIM0_DEVICE_PATH), - _call(), + VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -151,75 +146,34 @@ ADCSIM::~ADCSIM() } } -int -ADCSIM::init() +int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) { - DEVICE_DEBUG("init done"); + ADCSIM *me = DriverMgr::getDriverObjByHandle(h); + if (me) { + if (num_samples > me->_channel_count) { + num_samples = me->_channel_count; + } + size_t len = num_samples * sizeof(adc_msg_s); - /* create the device node */ - return VDev::init(); -} - -int -ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - return -ENOTTY; -} - -ssize_t -ADCSIM::read(device::file_t *filp, char *buffer, size_t len) -{ - const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - - if (len > maxsize) { - len = maxsize; + /* block interrupts while copying samples to avoid racing with an update */ + me->m_lock.lock(); + memcpy((void *)sample, (void *)(me->_samples), len); + me->m_lock.unlock(); + return num_samples; } - /* block interrupts while copying samples to avoid racing with an update */ - memcpy(buffer, _samples, len); - - return len; -} - -int -ADCSIM::open_first(device::file_t *filp) -{ - /* get fresh data */ - _tick(); - - /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); - - return 0; -} - -int -ADCSIM::close_last(device::file_t *filp) -{ - hrt_cancel(&_call); - return 0; + return -1; } void -ADCSIM::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); -} - -void -ADCSIM::_tick() +ADCSIM::_measure() { + m_lock.lock(); /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } - - update_system_power(); -} - -void -ADCSIM::update_system_power(void) -{ + m_lock.unlock(); } uint16_t @@ -246,19 +200,19 @@ int test(void) { - int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); - if (fd < 0) { - PX4_ERR("can't open ADCSIM device"); + if (!h.isValid()) { + PX4_ERR("can't open ADCSIM device (%d)", h.getError()); return 1; } for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = px4_read(fd, data, sizeof(data)); + ssize_t count = ADCSIM::read(h, data, sizeof(data)); if (count < 0) { - PX4_ERR("read error"); + PX4_ERR("read error (%d)", h.getError()); return 1; } @@ -271,7 +225,7 @@ test(void) usleep(500000); } - px4_close(fd); + DriverMgr::releaseHandle(h); return 0; } } @@ -289,12 +243,6 @@ adcsim_main(int argc, char *argv[]) PX4_ERR("couldn't allocate the ADCSIM driver"); return 1; } - - if (g_adc->init() != OK) { - delete g_adc; - PX4_ERR("ADCSIM init failed"); - return 1; - } } if (argc > 1) { From 9074d3dd8eb3d1a4a3c23b06d16b8d3493943c0b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 00:06:12 -0800 Subject: [PATCH 027/624] Adding in DriverFramework Signed-off-by: Mark Charlebois --- src/firmware/posix/CMakeLists.txt | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 3 +-- src/platforms/posix/main.cpp | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index fb7490ffe2..48c4702785 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -14,6 +14,7 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} + ${driver_framework_libraries} pthread m rt -Wl,--end-group ) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 6d725344bf..0efbc94225 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -199,7 +199,6 @@ ADCSIM *g_adc; int test(void) { - DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); if (!h.isValid()) { @@ -245,7 +244,7 @@ adcsim_main(int argc, char *argv[]) } } - if (argc > 1) { + if (argc > 1 && g_adc) { if (!strcmp(argv[1], "test")) { ret = test(); } diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1b22627721..56301b603c 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -43,6 +43,9 @@ #include #include #include +#include "apps.h" +#include "px4_middleware.h" +#include "DriverFramework.hpp" namespace px4 { @@ -53,9 +56,6 @@ using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); -#include "apps.h" -#include "px4_middleware.h" - static bool _ExitFlag = false; extern "C" { void _SigIntHandler(int sig_num); @@ -184,6 +184,7 @@ int main(int argc, char **argv) } if (!error_detected) { + DriverFramework::Framework::initialize(); px4::init_once(); px4::init(argc, argv, "mainapp"); @@ -236,5 +237,6 @@ int main(int argc, char **argv) vector shutdown_cmd = { "shutdown" }; run_cmd(shutdown_cmd, true); + DriverFramework::Framework::shutdown(); } } From dc3d8053865506236821a37294d36e4f39dc5ec6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 00:11:34 -0800 Subject: [PATCH 028/624] Removed unussed function Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 0efbc94225..f71f41d52f 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -87,9 +87,6 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ - /** work trampoline */ - static void _tick_trampoline(void *arg, WorkHandle &h); - /** worker function */ virtual void _measure(); From 847b60484194713b58d5cd3dbdbe40bd673ccc69 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 20:59:41 -0800 Subject: [PATCH 029/624] Added Gyrosim Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 41 ++- .../posix/drivers/gyrosim/gyrosim.cpp | 246 +++++++----------- 3 files changed, 111 insertions(+), 178 deletions(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 06e2616237..3ddb6b5e34 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 06e26162373848b9ceb9de8b9006a0ebe952ea6d +Subproject commit 3ddb6b5e34d870b487dbd57949a7d9c718730701 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index f71f41d52f..a3d28f6d41 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -66,19 +66,19 @@ #include #include "SyncObj.hpp" -#include "VirtDriverObj.hpp" +#include "VirtDevObj.hpp" using namespace DriverFramework; #define ADC_BASE_DEV_PATH "/dev/adc" -class ADCSIM : public VirtDriverObj +class ADCSIM : public VirtDevObj { public: ADCSIM(uint32_t channels); virtual ~ADCSIM(); - static int read(DriverHandle &h, adc_msg_s *sample, size_t num_samples); + virtual ssize_t read(void *buffer, ssize_t len); private: WorkHandle _call; @@ -103,7 +103,7 @@ private: }; ADCSIM::ADCSIM(uint32_t channels) : - VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), + VirtDevObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -143,23 +143,21 @@ ADCSIM::~ADCSIM() } } -int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) +ssize_t +ADCSIM::read(void *buffer, ssize_t len) { - ADCSIM *me = DriverMgr::getDriverObjByHandle(h); - if (me) { - if (num_samples > me->_channel_count) { - num_samples = me->_channel_count; - } - size_t len = num_samples * sizeof(adc_msg_s); - - /* block interrupts while copying samples to avoid racing with an update */ - me->m_lock.lock(); - memcpy((void *)sample, (void *)(me->_samples), len); - me->m_lock.unlock(); - return num_samples; + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) { + len = maxsize; } + + /* block interrupts while copying samples to avoid racing with an update */ + m_lock.lock(); + memcpy(buffer, _samples, len); + m_lock.unlock(); - return -1; + return len; } void @@ -196,7 +194,8 @@ ADCSIM *g_adc; int test(void) { - DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); + DevHandle h; + DevMgr::getHandle(ADCSIM0_DEVICE_PATH, h); if (!h.isValid()) { PX4_ERR("can't open ADCSIM device (%d)", h.getError()); @@ -205,7 +204,7 @@ test(void) for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = ADCSIM::read(h, data, sizeof(data)); + ssize_t count = h.read(data, sizeof(data)); if (count < 0) { PX4_ERR("read error (%d)", h.getError()); @@ -221,7 +220,7 @@ test(void) usleep(500000); } - DriverMgr::releaseHandle(h); + DevMgr::releaseHandle(h); return 0; } } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0ac023285c..430e13a00a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -69,13 +69,17 @@ #include #include -#include +//#include #include -#include -#include +//#include +//#include #include #include +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -114,16 +118,17 @@ */ class GYROSIM_gyro; -class GYROSIM : public device::VDev +class GYROSIM : public VirtDevObj { public: GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation); virtual ~GYROSIM(); - virtual int init(); + int init(); + virtual int start(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -136,14 +141,14 @@ public: protected: friend class GYROSIM_gyro; - virtual ssize_t gyro_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t gyro_read(void *buffer, size_t buflen); + virtual int gyro_ioctl(unsigned long cmd, void *arg); private: GYROSIM_gyro *_gyro; uint8_t _product; /** product code */ - struct hrt_call _call; + WorkHandle _call; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -175,16 +180,6 @@ private: // last temperature reading for print_info() float _last_temperature; - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -192,21 +187,10 @@ private: */ int reset(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - /** * Fetch measurements from the sensor and update the report buffers. */ - void measure(); + virtual void _measure(); /** * Read a register from the GYROSIM @@ -286,19 +270,21 @@ private: #pragma pack(pop) uint8_t _regdata[108]; + + bool _pub_blocked = true; // Don't publish until initialized }; /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public device::VDev +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); virtual int init(); @@ -307,6 +293,7 @@ protected: void parent_poll_notify(); + virtual void _measure(); private: GYROSIM *_parent; orb_advert_t _gyro_topic; @@ -322,11 +309,9 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VDev("GYROSIM", path_accel), + VirtDevObj("GYROSIM", path_accel, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), - _call{}, - _call_interval(0), _accel_reports(nullptr), _accel_scale{}, _accel_range_scale(0.0f), @@ -349,17 +334,12 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _rotation(rotation), _last_temperature(0) { - // disable debug() calls - _debug_enabled = false; - // Don't publish until initialized - _pub_blocked = true; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ - _gyro->_device_id.devid = _device_id.devid; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; + _gyro->m_id.dev_id = m_id.dev_id; + _gyro->m_id.dev_id_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; // default accel scale factors _accel_scale.x_offset = 0; @@ -376,8 +356,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } GYROSIM::~GYROSIM() @@ -397,10 +375,6 @@ GYROSIM::~GYROSIM() delete _gyro_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); @@ -413,15 +387,6 @@ GYROSIM::init() { int ret; - /* do VDevinit first */ - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_WARN("VDev setup failed"); - return ret; - } - struct accel_report arp = {}; struct gyro_report grp = {}; @@ -467,13 +432,13 @@ GYROSIM::init() /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); + PX4_ERR("gyro init failed"); return ret; } - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + start(); - measure(); + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -571,13 +536,11 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) _sample_rate = 1000 / div; PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - _call_interval = 1000000 / _sample_rate; - hrt_cancel(&_call); - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + setSampleInterval(1000000 / _sample_rate); } ssize_t -GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(accel_report); @@ -589,7 +552,7 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _accel_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -620,7 +583,7 @@ int GYROSIM::self_test() { if (perf_event_count(_sample_perf) == 0) { - measure(); + _measure(); } /* return 0 on success, 1 else */ @@ -724,7 +687,7 @@ GYROSIM::gyro_self_test() } ssize_t -GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::gyro_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(gyro_report); @@ -736,7 +699,7 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _gyro_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -764,15 +727,17 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } int -GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::devIOCTL(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCRESET: return reset(); case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -789,15 +754,15 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)1000); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned ticks = 1000000 / ul_arg; /* check against maximum sane rate */ if (ticks < 1000) { @@ -829,11 +794,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_accel_reports->resize(arg)) { + if (!_accel_reports->resize(ul_arg)) { return -ENOMEM; } @@ -847,7 +812,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _sample_rate; case ACCELIOCSSAMPLERATE: - _set_sample_rate(arg); + _set_sample_rate(ul_arg); return OK; case ACCELIOCSLOWPASS: @@ -855,7 +820,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; + struct accel_scale *s = (struct accel_scale *) ul_arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -869,11 +834,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) ul_arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: - return set_accel_range(arg); + return set_accel_range(ul_arg); case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); @@ -883,28 +848,30 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } int -GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + return devIOCTL(cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_gyro_reports->resize(arg)) { + if (!_gyro_reports->resize(ul_arg)) { return -ENOMEM; } @@ -918,7 +885,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) return _sample_rate; case GYROIOCSSAMPLERATE: - _set_sample_rate(arg); + _set_sample_rate(ul_arg); return OK; case GYROIOCSLOWPASS: @@ -949,7 +916,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } @@ -1020,7 +987,7 @@ GYROSIM::set_accel_range(unsigned max_g_in) return OK; } -void +int GYROSIM::start() { /* make sure we are stopped first */ @@ -1031,28 +998,11 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - if (_call_interval > 0) { - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); - } + return DevObj::start(); } void -GYROSIM::stop() -{ - hrt_cancel(&_call); -} - -void -GYROSIM::measure_trampoline(void *arg) -{ - GYROSIM *dev = reinterpret_cast(arg); - - /* make another measurement */ - dev->measure(); -} - -void -GYROSIM::measure() +GYROSIM::_measure() { #if 0 @@ -1152,8 +1102,9 @@ GYROSIM::measure() /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + // FIXME + //poll_notify(POLLIN); + //_gyro->parent_poll_notify(); if (!(_pub_blocked)) { /* log the time of this report */ @@ -1211,7 +1162,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : - VDev("GYROSIM_gyro", path), + VirtDevObj("GYROSIM_gyro", path, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1), @@ -1219,55 +1170,37 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : { } -GYROSIM_gyro::~GYROSIM_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} - int GYROSIM_gyro::init() { - int ret; - - // do base class init - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - - return ret; + return start(); } +#if 0 void GYROSIM_gyro::parent_poll_notify() { poll_notify(POLLIN); } +#endif ssize_t -GYROSIM_gyro::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM_gyro::devRead(void *buffer, size_t buflen) { - return _parent->gyro_read(filp, buffer, buflen); + return _parent->gyro_read(buffer, buflen); } int -GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM_gyro::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + return (int)VirtDevObj::devIOCTL(cmd, arg); break; default: - return _parent->gyro_ioctl(filp, cmd, arg); + return _parent->gyro_ioctl(cmd, arg); } } @@ -1375,29 +1308,31 @@ test() ssize_t sz; /* get the driver */ - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h_accel; + DevMgr::getHandle(path_accel, h_accel); - if (fd < 0) { + if (!h_accel.isValid()) { PX4_ERR("%s open failed (try 'gyrosim start')", path_accel); return 1; } /* get the driver */ - int fd_gyro = px4_open(path_gyro, O_RDONLY); + DevHandle h_gyro; + DevMgr::getHandle(path_gyro, h_gyro); - if (fd_gyro < 0) { + if (!h_gyro.isValid()) { PX4_ERR("%s open failed", path_gyro); return 1; } /* reset to manual polling */ - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + if (h_accel.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MANUAL) < 0) { PX4_ERR("reset to manual polling"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &a_report, sizeof(a_report)); + sz = h_accel.read(&a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1417,7 +1352,7 @@ test() (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); + sz = h_gyro.read(&g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); @@ -1440,11 +1375,12 @@ test() /* XXX add poll-rate tests here too */ - px4_close(fd); + // Destructor would clean the up too + DevMgr::releaseHandle(h_accel); + DevMgr::releaseHandle(h_gyro); reset(); PX4_INFO("PASS"); - return 0; } @@ -1454,30 +1390,28 @@ test() int reset() { - const char *path_accel = MPU_DEVICE_PATH_ACCEL; - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h; + DevMgr::getHandle(MPU_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("reset failed"); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, (void *)0) < 0) { PX4_ERR("driver reset failed"); goto reset_fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); goto reset_fail; } - px4_close(fd); return 0; reset_fail: - px4_close(fd); return 1; } From de1a7b30ce47f8fe9ee82cef86294b1187118d86 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 3 Nov 2015 01:24:02 -0800 Subject: [PATCH 030/624] gyrosim and adcsim now compile with DriverFramework enabled Only the posix build is tested for compilation Signed-off-by: Mark Charlebois --- CMakeLists.txt | 4 +++ src/firmware/posix/CMakeLists.txt | 3 +- src/lib/DriverFramework | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 5 ++-- .../posix/drivers/gyrosim/gyrosim.cpp | 29 +++++++++---------- 5 files changed, 22 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b395b6261..bbdff7c6c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,9 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) + #============================================================================= # parameters # @@ -320,6 +323,7 @@ foreach(module ${config_module_list}) endforeach() add_subdirectory(src/firmware/${OS}) +add_subdirectory(src/lib/DriverFramework) if (config_io_board) add_subdirectory(src/modules/px4iofirmware) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 48c4702785..208b0b689d 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -14,7 +14,8 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} - ${driver_framework_libraries} + df_driver_framework + ${df_driver_libraries} pthread m rt -Wl,--end-group ) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 3ddb6b5e34..c2afd87807 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 3ddb6b5e34d870b487dbd57949a7d9c718730701 +Subproject commit c2afd878071e8fb0b1d80070e44ba98780492fc1 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index a3d28f6d41..66372ad214 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -78,10 +78,9 @@ public: ADCSIM(uint32_t channels); virtual ~ADCSIM(); - virtual ssize_t read(void *buffer, ssize_t len); + virtual ssize_t devRead(void *buffer, size_t len); private: - WorkHandle _call; perf_counter_t _sample_perf; unsigned _channel_count; @@ -144,7 +143,7 @@ ADCSIM::~ADCSIM() } ssize_t -ADCSIM::read(void *buffer, ssize_t len) +ADCSIM::devRead(void *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 430e13a00a..7aef8cf392 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -69,10 +69,10 @@ #include #include -//#include +#include #include -//#include -//#include +#include +#include #include #include @@ -277,11 +277,11 @@ private: /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public VirtDevObj +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - virtual ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro() {} virtual ssize_t devRead(void *buffer, size_t buflen); virtual int devIOCTL(unsigned long cmd, void *arg); @@ -293,12 +293,11 @@ protected: void parent_poll_notify(); - virtual void _measure(); + virtual void _measure() {}; private: GYROSIM *_parent; orb_advert_t _gyro_topic; int _gyro_orb_class_instance; - int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ GYROSIM_gyro(const GYROSIM_gyro &); @@ -385,7 +384,7 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { - int ret; + int ret = 1; struct accel_report arp = {}; @@ -1102,9 +1101,8 @@ GYROSIM::_measure() /* notify anyone waiting for data */ - // FIXME - //poll_notify(POLLIN); - //_gyro->parent_poll_notify(); + updateNotify(); + _gyro->parent_poll_notify(); if (!(_pub_blocked)) { /* log the time of this report */ @@ -1162,27 +1160,26 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : + // Set sample interval to 0 since device is read by parent VirtDevObj("GYROSIM_gyro", path, 0), _parent(parent), _gyro_topic(nullptr), - _gyro_orb_class_instance(-1), - _gyro_class_instance(-1) + _gyro_orb_class_instance(-1) { } + int GYROSIM_gyro::init() { return start(); } -#if 0 void GYROSIM_gyro::parent_poll_notify() { - poll_notify(POLLIN); + updateNotify(); } -#endif ssize_t GYROSIM_gyro::devRead(void *buffer, size_t buflen) From 84e9380b1046dfc94674cde2cf0b8e3e1b8cdcd4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 3 Nov 2015 01:27:22 -0800 Subject: [PATCH 031/624] Commened out manditory use of Clang-3.6 Signed-off-by: Mark Charlebois --- CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bbdff7c6c6..5f638109b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,8 +121,9 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) -SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) -SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) +# Use clang +#SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +#SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) #============================================================================= # parameters From 67132b0b166b7af653e7b8303693456046abecea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 11:06:28 +0100 Subject: [PATCH 032/624] Register the driver framework correctly --- CMakeLists.txt | 1 + src/platforms/posix/drivers/adcsim/CMakeLists.txt | 1 + src/platforms/posix/drivers/gyrosim/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f638109b4..9f2e6dfdda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -234,6 +234,7 @@ px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix") diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index 93cdd95a0f..fc6802b0c4 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_module( adcsim.cpp DEPENDS platforms__common + git_driverframework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index 335bee65d6..f776b34bea 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( gyrosim.cpp DEPENDS platforms__common + git_driverframework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From d10b0eea516afb65bfdd2d0a4307c274ba4bb9fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 11:53:26 +0100 Subject: [PATCH 033/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c2afd87807..2c2e80b5cc 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c2afd878071e8fb0b1d80070e44ba98780492fc1 +Subproject commit 2c2e80b5cc39ce5b96d3be19a1e1db3370ba7e85 From 634ab66470b29542f3b507b49e8fb22793cc2c9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:05:19 +0100 Subject: [PATCH 034/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 2c2e80b5cc..dc52ed6d98 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 2c2e80b5cc39ce5b96d3be19a1e1db3370ba7e85 +Subproject commit dc52ed6d98ce007f86f6e0b7521636337753a239 From 4c2bac2431a2afc4d00521ce86e56bf7d0e502dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:44:12 +0100 Subject: [PATCH 035/624] DriverFramework update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index dc52ed6d98..080a7cdccd 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit dc52ed6d98ce007f86f6e0b7521636337753a239 +Subproject commit 080a7cdccd283cea79ce7908774391aac60a41fa From 7faffdc945841c6e0504130b92799fe227b2e1f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:52:04 +0100 Subject: [PATCH 036/624] Updated driver framework repo --- src/platforms/posix/drivers/adcsim/CMakeLists.txt | 6 ++++++ src/platforms/posix/drivers/gyrosim/CMakeLists.txt | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index fc6802b0c4..c1c925c15c 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -40,5 +40,11 @@ px4_add_module( DEPENDS platforms__common git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__adcsim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index f776b34bea..a1b42bd11c 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -41,5 +41,11 @@ px4_add_module( DEPENDS platforms__common git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__gyrosim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : From eb13b518b890e43f917b4c79c5996efac740455f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:56:19 +0100 Subject: [PATCH 037/624] Fixed adc sim formatting --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 66372ad214..e8f1c592ec 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -146,11 +146,11 @@ ssize_t ADCSIM::devRead(void *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - + if (len > maxsize) { len = maxsize; } - + /* block interrupts while copying samples to avoid racing with an update */ m_lock.lock(); memcpy(buffer, _samples, len); @@ -163,10 +163,12 @@ void ADCSIM::_measure() { m_lock.lock(); + /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } + m_lock.unlock(); } From 4f9bbf3d780aaf2ed7ee5f155911439c4ef946ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:57:44 +0100 Subject: [PATCH 038/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 080a7cdccd..e1582be979 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 080a7cdccd283cea79ce7908774391aac60a41fa +Subproject commit e1582be979cb903ef81ebd150b7667fde91763fe From 54d1302555054754e1d1a799c6b139cb53910ed4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:13:25 +0100 Subject: [PATCH 039/624] Fix missing rt lib --- cmake/posix/px4_impl_posix.cmake | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index 7dbf128780..28b27f87d1 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -176,6 +176,10 @@ if(UNIX AND APPLE) -include ${PX4_INCLUDE_DIR}visibility.h ) + set(added_exe_linker_flags + -lpthread + ) + else() set(added_definitions @@ -185,11 +189,12 @@ else() -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h ) -endif() set(added_exe_linker_flags - -lpthread + -lpthread -lrt ) + +endif() # Add the toolchain specific flags From c2c5977462ede993228e05743f801fb42c5cf193 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:14:26 +0100 Subject: [PATCH 040/624] Driverframework: Update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index e1582be979..c07f902805 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit e1582be979cb903ef81ebd150b7667fde91763fe +Subproject commit c07f9028054bdad970c6283066703c59c4b3a35a From b01bd8c48f13690cb5b63006d1b96c62399684e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:16:28 +0100 Subject: [PATCH 041/624] Travis CI: Select CLANG --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index e6b5239344..c94da476e4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,6 +3,8 @@ language: cpp +compiler: clang + matrix: fast_finish: true include: From a3f30d0eebff9f1799d2d6b4ccdc20087b0ae097 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:17:27 +0100 Subject: [PATCH 042/624] Revert CLANG --- .travis.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index c94da476e4..e6b5239344 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,8 +3,6 @@ language: cpp -compiler: clang - matrix: fast_finish: true include: From 70711979253f16e6ef4e19487ca7067ca4b3e7d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:18:11 +0100 Subject: [PATCH 043/624] Update Driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c07f902805..89d4bb5725 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c07f9028054bdad970c6283066703c59c4b3a35a +Subproject commit 89d4bb57258dcfc78c48ed3466d77fdbc0b4471e From 18a06e115e1646efe79a596a1569964db2760f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:49:43 +0100 Subject: [PATCH 044/624] Add C++ flags --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 89d4bb5725..ca4970ab63 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 89d4bb57258dcfc78c48ed3466d77fdbc0b4471e +Subproject commit ca4970ab6329110a28272fb808539333a38dc173 From 8fe91cb1722d9d941cdd6ca4716fcf1a564a169b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:54:06 +0100 Subject: [PATCH 045/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index ca4970ab63..d58b46695b 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit ca4970ab6329110a28272fb808539333a38dc173 +Subproject commit d58b46695b702173459465f862d52a216229772b From 0ee3ad377115c4776e01bb467cf0fe9717dbbe2d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:57:29 +0100 Subject: [PATCH 046/624] Fix Cxx errors --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index d58b46695b..32786683a0 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit d58b46695b702173459465f862d52a216229772b +Subproject commit 32786683a0a0cbe3003fd55d6fc6c61999707b95 From 72319f6b48cd275c44afa7a0b298d6c65dd8677d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Nov 2015 09:01:36 +0100 Subject: [PATCH 047/624] Update driver framework version --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 32786683a0..c4be5e70b3 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 32786683a0a0cbe3003fd55d6fc6c61999707b95 +Subproject commit c4be5e70b36c86868520472f93fd945d7f0d8986 From efd9995a10e7e508be8bc0a82ac7d71f0d689a44 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 10 Nov 2015 07:45:13 -0800 Subject: [PATCH 048/624] Ported more simulated sensors to DriverFramework Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 16 +- src/drivers/drv_device.h | 6 + src/lib/DriverFramework | 2 +- .../commander/state_machine_helper.cpp | 28 ++-- .../posix/drivers/accelsim/accelsim.cpp | 28 ++-- src/platforms/posix/drivers/barosim/baro.cpp | 153 +++++++++--------- .../posix/drivers/barosim/baro_sim.cpp | 6 +- src/platforms/posix/drivers/barosim/barosim.h | 15 +- .../posix/drivers/gyrosim/gyrosim.cpp | 2 +- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 73 ++++----- 10 files changed, 168 insertions(+), 161 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 60761ed511..12aa08b154 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -45,6 +45,10 @@ #include #include +#include "DevMgr.hpp" + +using namespace DriverFramework; + namespace device { @@ -512,13 +516,21 @@ VDev *VDev::getDev(const char *path) void VDev::showDevices() { int i = 0; - PX4_INFO("Devices:"); + PX4_INFO("PX4 Devices:"); for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } } + PX4_INFO("DF Devices:"); + + std::string devname; + for (unsigned int index=0; i == 0; ++i) { + if (DevMgr::getNextDeviceName(index, devname) == 0) { + PX4_INFO(" %s", devname.c_str()); + } + } } void VDev::showTopics() @@ -566,4 +578,4 @@ const char *VDev::devList(unsigned int *next) return NULL; } -} // namespace device \ No newline at end of file +} // namespace device diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 02dbe7af67..af0f4982b3 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -46,6 +46,7 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#ifdef __PX4_NUTTX /* * ioctl() definitions */ @@ -65,6 +66,11 @@ */ #define DEVIOCGDEVICEID _DEVICEIOC(2) +#else +#include "DevObj.hpp" + +#endif + #ifdef __PX4_POSIX #ifndef SIOCDEVPRIVATE diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c4be5e70b3..0d91f67f2e 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c4be5e70b36c86868520472f93fd945d7f0d8986 +Subproject commit 0d91f67f2e8a1ae91a5c853d5cae8b30e67a635e diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21789c9c66..6ac5046ac7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,10 @@ #include "state_machine_helper.h" #include "commander_helper.h" #include "PreflightCheck.h" +#ifndef __PX4_NUTTX +#include "DevMgr.hpp" +using namespace DriverFramework; +#endif // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which @@ -463,30 +467,30 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta #else - const char *devname; - unsigned int handle = 0; + std::string devname; + unsigned int index = 0; for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) + if (DevMgr::getNextDeviceName(index, devname) < 0) { break; + } /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { + if (!strcmp("/dev/mavlink", devname.c_str())) { continue; } + DevHandle h; + DevMgr::getHandle(devname.c_str(), h); - int sensfd = px4_open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); + if (!h.isValid()) { + warn("failed opening device %s", devname.c_str()); continue; } - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); + int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1); + DevMgr::releaseHandle(h); - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); } ret = TRANSITION_CHANGED; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index ca231bffb9..7a82cfdff3 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include #include @@ -244,10 +243,8 @@ public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - virtual int init(); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); protected: friend class ACCELSIM; @@ -913,7 +910,7 @@ ACCELSIM::_measure() /* notify anyone waiting for data */ DevMgr::updateNotify(*this); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { /* publish it */ // The first call to measure() is from init() and _accel_topic is not @@ -1000,9 +997,9 @@ ACCELSIM::mag_measure() _mag_reports->force(&mag_report); /* notify anyone waiting for data */ - DevMgr::updateNotify(*this) + DevMgr::updateNotify(*this); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } @@ -1014,7 +1011,7 @@ ACCELSIM::mag_measure() } ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : - VirtDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), + VirtDevObj("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), _parent(parent), _mag_topic(nullptr), _mag_orb_class_instance(-1), @@ -1027,7 +1024,7 @@ ACCELSIM_mag::~ACCELSIM_mag() } ssize_t -ACCELSIM_mag::devRead(char *buffer, size_t buflen) +ACCELSIM_mag::devRead(void *buffer, size_t buflen) { return _parent->mag_read(buffer, buflen); } @@ -1037,7 +1034,7 @@ ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VirtDevObj::ioctl(cmd, arg); + return (int)VirtDevObj::devIOCTL(cmd, arg); break; default: @@ -1077,6 +1074,9 @@ start(enum Rotation rotation) return 0; } + DevHandle h; + DevHandle h_mag; + /* create the driver */ g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); @@ -1091,7 +1091,6 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - DevHandle h; DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); if (!h.isValid()) { @@ -1099,18 +1098,17 @@ start(enum Rotation rotation) goto fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); DevMgr::releaseHandle(h); goto fail; } - DevHandle h_mag; DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ if (!h_mag.isValid()) { - if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 975fa3687c..02b9bb9183 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -67,6 +66,7 @@ #include #include "barosim.h" +#include "VirtDevObj.hpp" enum BAROSIM_BUS { BAROSIM_BUS_SIM_EXTERNAL @@ -86,16 +86,16 @@ enum BAROSIM_BUS { #define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ #define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -class BAROSIM : public device::VDev +class BAROSIM : public VirtDevObj { public: - BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); + BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path); ~BAROSIM(); virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); /** * Diagnostics - print some basic information about the driver. @@ -103,7 +103,7 @@ public: void print_info(); protected: - Device *_interface; + DevObj *_interface; barosim::prom_s _prom; @@ -182,7 +182,10 @@ protected: * * @return OK if the measurement command was successful. */ - virtual int measure(); + int measure(); + + // Unused + virtual void _measure() {} /** * Collect the result of the most recent measurement. @@ -195,8 +198,8 @@ protected: */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : - VDev("BAROSIM", path), +BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) : + VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 0), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -224,10 +227,6 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) { - unregister_class_devname(get_devname(), _class_instance); - } - /* free any existing reports */ if (_reports != nullptr) { delete _reports; @@ -246,12 +245,12 @@ int BAROSIM::init() { int ret; - DEVICE_DEBUG("BAROSIM::init"); + //PX4_DEBUG("BAROSIM::init"); - ret = VDev::init(); + ret = VirtDevObj::init(); if (ret != OK) { - DEVICE_DEBUG("VDev init failed"); + PX4_ERR("VirtDevObj init failed"); goto out; } @@ -259,14 +258,11 @@ BAROSIM::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - DEVICE_DEBUG("can't get memory for reports"); + PX4_ERR("can't get memory for reports"); ret = -ENOMEM; goto out; } - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ _measure_phase = 0; @@ -325,7 +321,7 @@ out: } ssize_t -BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) +BAROSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); struct baro_report *brp = reinterpret_cast(buffer); @@ -397,12 +393,14 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } int -BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +BAROSIM::devIOCTL(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCSPOLLRATE: - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -440,7 +438,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned long ticks = USEC2TICK(1000000 / arg); + unsigned long ticks = USEC2TICK(1000000 / ul_arg); /* check against maximum rate */ if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { @@ -468,11 +466,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_reports->resize(arg)) { + if (!_reports->resize(ul_arg)) { return -ENOMEM; } @@ -492,11 +490,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) { + if ((ul_arg < 80000) || (ul_arg > 120000)) { return -EINVAL; } - _msl_pressure = arg; + _msl_pressure = ul_arg; return OK; case BAROIOCGMSLPRESSURE: @@ -508,7 +506,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } void @@ -542,7 +540,7 @@ void BAROSIM::cycle() { int ret; - unsigned dummy; + void *dummy = nullptr; /* collection phase? */ if (_collect_phase) { @@ -552,7 +550,7 @@ BAROSIM::cycle() if (ret != OK) { /* issue a reset command to the sensor */ - _interface->dev_ioctl(IOCTL_RESET, dummy); + _interface->devIOCTL(IOCTL_RESET, (void *)dummy); /* reset the collection state machine and try again */ start_cycle(); return; @@ -586,7 +584,7 @@ BAROSIM::cycle() if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ - _interface->dev_ioctl(IOCTL_RESET, dummy); + _interface->devIOCTL(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; @@ -613,12 +611,12 @@ BAROSIM::measure() /* * In phase zero, request temperature; in other phases, request pressure. */ - unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + unsigned long addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; /* * Send the command to begin measuring. */ - ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); + ret = _interface->devIOCTL(IOCTL_MEASURE, (void *)addr); if (OK != ret) { perf_count(_comms_errors); @@ -650,7 +648,7 @@ BAROSIM::collect() report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); + ret = _interface->devRead((void *)&baro_report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); @@ -672,7 +670,7 @@ BAROSIM::collect() report.timestamp = hrt_absolute_time(); /* publish it */ - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); @@ -687,7 +685,7 @@ BAROSIM::collect() } /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this); } /* update the measurement state machine */ @@ -810,7 +808,7 @@ start_bus(struct barosim_bus_option &bus) } prom_u prom_buf; - device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + DevObj *interface = bus.interface_constructor(prom_buf, bus.busnum); if (interface->init() != OK) { delete interface; @@ -827,21 +825,22 @@ start_bus(struct barosim_bus_option &bus) return false; } - int fd = px4_open(bus.devpath, O_RDONLY); + DevHandle h; + DevMgr::getHandle(bus.devpath, h); /* set the poll rate to default, starts automatic data collection */ - if (fd == -1) { + if (!h.isValid()) { PX4_ERR("can't open baro device"); return false; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + DevMgr::releaseHandle(h); PX4_ERR("failed setting default poll rate"); return false; } - px4_close(fd); + DevMgr::releaseHandle(h); return true; } @@ -882,17 +881,16 @@ test() ssize_t sz; int ret; - int fd; + DevHandle h; + DevMgr::getHandle(bus.devpath, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { - PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); + if (!h.isValid()) { + PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("immediate read failed"); @@ -906,38 +904,37 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); /* set the queue depth to 10 */ - if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, (void *)10UL)) { PX4_ERR("failed to set queue depth"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } /* start the sensor polling at 2Hz */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)2UL)) { PX4_ERR("failed to set 2Hz poll rate"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { - px4_pollfd_struct_t fds; + UpdateList in_set, out_set; + in_set.push_back(&h); /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 2000); + ret = DevMgr::waitForUpdate(in_set, out_set, 1000); if (ret != 1) { PX4_WARN("timed out waiting for sensor data"); } /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("periodic read failed"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } @@ -948,7 +945,7 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); } - px4_close(fd); + DevMgr::releaseHandle(h); PX4_INFO("PASS"); return 0; } @@ -960,21 +957,21 @@ int reset() { struct barosim_bus_option &bus = bus_options[0]; - int fd; - fd = px4_open(bus.devpath, O_RDONLY); + DevHandle h; + DevMgr::getHandle(bus.devpath, h); - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("failed "); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); return 1; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); return 1; } @@ -1008,20 +1005,19 @@ calibrate(unsigned altitude) { struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; - float pressure; - float p1; + float pressure; + float p1; - int fd; + DevHandle h; + DevMgr::getHandle(bus.devpath, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; } /* start the sensor polling at max */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MAX)) { PX4_ERR("failed to set poll rate"); return 1; } @@ -1030,14 +1026,13 @@ calibrate(unsigned altitude) pressure = 0.0f; for (unsigned i = 0; i < 20; i++) { - px4_pollfd_struct_t fds; int ret; ssize_t sz; + UpdateList in_set, out_set; + in_set.push_back(&h); /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 1000); + ret = DevMgr::waitForUpdate(in_set, out_set, 1000); if (ret != 1) { PX4_ERR("timed out waiting for sensor data"); @@ -1045,7 +1040,7 @@ calibrate(unsigned altitude) } /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("sensor read failed"); @@ -1073,12 +1068,12 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; - if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + if (h.ioctl(BAROIOCSMSLPRESSURE, (void *)((unsigned long)(p1))) != OK) { PX4_WARN("BAROIOCSMSLPRESSURE"); return 1; } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 252d1428c9..5fc38aaa1c 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -54,7 +54,7 @@ #include "barosim.h" #include "board_config.h" -device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf); +DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf); class BARO_SIM : public device::SIM { @@ -94,10 +94,10 @@ private: }; -device::Device * +DevObj * BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) { - return new BARO_SIM(busnum, prom_buf); + return reinterpret_cast(new BARO_SIM(busnum, prom_buf)); } BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index bac176d400..db7f009a51 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -36,13 +36,14 @@ * * Shared defines for the ms5611 driver. */ +#include "DevObj.hpp" -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ #define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ #define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ #define IOCTL_RESET 2 @@ -79,6 +80,8 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ +using namespace DriverFramework; + /* interface factories */ -extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); -typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); +extern DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); +typedef DevObj *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 52e3254b7f..151777de6b 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -426,7 +426,7 @@ GYROSIM::init() _gyro_scale.z_scale = 1.0f; - /* do VDev init for the gyro device node, keep it optional */ + /* do init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index b58cccb5bd..a1ef3b3624 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -109,17 +109,18 @@ #include +#include "VirtDevObj.hpp" -class ToneAlarm : public device::VDev +using namespace DriverFramework; + +class ToneAlarm : public VirtDevObj { public: ToneAlarm(); ~ToneAlarm(); - virtual int init(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len); + virtual int devIOCTL(unsigned long cmd, void *arg); + virtual ssize_t devWrite(const void *buffer, size_t len); inline const char *name(int tune) { return _tune_names[tune]; @@ -196,6 +197,8 @@ private: // static void next_trampoline(void *arg); + // Unused + virtual void _measure() {} }; // semitone offsets from C for the characters 'A'-'G' @@ -208,7 +211,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - VDev("tone_alarm", TONEALARM0_DEVICE_PATH), + VirtDevObj("tone_alarm", TONEALARM0_DEVICE_PATH, nullptr, 0), _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), @@ -251,21 +254,6 @@ ToneAlarm::~ToneAlarm() { } -int -ToneAlarm::init() -{ - int ret; - - ret = VDev::init(); - - if (ret != OK) { - return ret; - } - - DEVICE_DEBUG("ready"); - return OK; -} - unsigned ToneAlarm::note_to_divisor(unsigned note) { @@ -653,21 +641,21 @@ ToneAlarm::next_trampoline(void *arg) int -ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ToneAlarm::devIOCTL(unsigned long cmd, void *arg) { int result = OK; - DEVICE_DEBUG("ioctl %i %lu", cmd, arg); + unsigned long ul_arg = (unsigned long)arg; -// irqstate_t flags = irqsave(); + PX4_DEBUG("ToneAlarm::devIOCTL %i %lu", cmd, ul_arg); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - PX4_INFO("TONE_SET_ALARM %lu", arg); + PX4_INFO("TONE_SET_ALARM %lu", ul_arg); - if (arg < TONE_NUMBER_OF_TUNES) { - if (arg == TONE_STOP_TUNE) { + if (ul_arg < TONE_NUMBER_OF_TUNES) { + if (ul_arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; @@ -676,10 +664,10 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) } else { /* always interrupt alarms, unless they are repeating and already playing */ - if (!(_repeat && _default_tune_number == arg)) { + if (!(_repeat && _default_tune_number == ul_arg)) { /* play the selected tune */ - _default_tune_number = arg; - start_tune(_default_tunes[arg]); + _default_tune_number = ul_arg; + start_tune(_default_tunes[ul_arg]); } } @@ -694,18 +682,16 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } -// irqrestore(flags); - /* give it to the superclass if we didn't like it */ if (result == -ENOTTY) { - result = VDev::ioctl(filp, cmd, arg); + result = VirtDevObj::devIOCTL(cmd, arg); } return result; } ssize_t -ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) +ToneAlarm::devWrite(const void *buffer, size_t len) { // sanity-check the buffer for length and nul-termination if (len > _tune_max) { @@ -726,13 +712,15 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) _user_tune = nullptr; } + const char *buf = reinterpret_cast(buffer); + // if the new tune is empty, we're done - if (buffer[0] == '\0') { + if (buf[0] == '\0') { return OK; } // allocate a copy of the new tune - _user_tune = strndup(buffer, len); + _user_tune = strndup(buf, len); if (_user_tune == nullptr) { return -ENOMEM; @@ -778,17 +766,18 @@ play_tune(unsigned tune) int play_string(const char *str, bool free_buffer) { - int fd, ret; + int ret; - fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); + DevHandle h; + DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h); - if (fd < 0) { - PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + if (!h.isValid()) { + PX4_WARN("Error: failed to get handle to %s", TONEALARM0_DEVICE_PATH); return 1; } - ret = px4_write(fd, str, strlen(str) + 1); - px4_close(fd); + ret = h.write(str, strlen(str) + 1); + DevMgr::releaseHandle(h); if (free_buffer) { free((void *)str); From f6798c6b851ddf5476a875634888fe2200d09c87 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:24:38 +0100 Subject: [PATCH 049/624] Gyro sim: Fix merge fail --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 9eb2601bfe..151777de6b 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1161,11 +1161,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : // Set sample interval to 0 since device is read by parent -<<<<<<< HEAD - VirtDevObj("GYROSIM_gyro", path, 0), -======= VirtDevObj("GYROSIM_gyro", path, nullptr, 0), ->>>>>>> 40c079efa8f7d8cc402890384cb9ce4e01f6a1c7 _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1) From 5552a98de44e84fe8231ab829a0bf8b577d501f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:24:50 +0100 Subject: [PATCH 050/624] Updated driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 0d91f67f2e..9a039c64a3 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 0d91f67f2e8a1ae91a5c853d5cae8b30e67a635e +Subproject commit 9a039c64a37e1b1b19e8de2db926d1953b65cfd8 From dda6ec3d5f48f59273415879093ec99fa69dcc6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:28:26 +0100 Subject: [PATCH 051/624] Fix missing errno header --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 7a82cfdff3..8fd91580b0 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include From 260eaa9d950d3089fcfb995f6bff07e273f528f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:29:09 +0100 Subject: [PATCH 052/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 9a039c64a3..8673518203 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 9a039c64a37e1b1b19e8de2db926d1953b65cfd8 +Subproject commit 8673518203fdb601fc21f10ee94184090be9e95f From cc8f159bcc8a5d52eab430edc68604f489a4e7ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:31:38 +0100 Subject: [PATCH 053/624] REVERT THIS COMMIT LATER: Temporarily disable the device listing --- src/drivers/device/vdev.cpp | 13 +++--- .../commander/state_machine_helper.cpp | 42 ++++++++++--------- 2 files changed, 29 insertions(+), 26 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 76cca2ce36..8983934804 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -550,13 +550,14 @@ void VDev::showDevices() pthread_mutex_unlock(&devmutex); PX4_INFO("DF Devices:"); + // TODO NOT IMPLEMENTED - std::string devname; - for (unsigned int index=0; i == 0; ++i) { - if (DevMgr::getNextDeviceName(index, devname) == 0) { - PX4_INFO(" %s", devname.c_str()); - } - } + // std::string devname; + // for (unsigned int index=0; i == 0; ++i) { + // if (DevMgr::getNextDeviceName(index, devname) == 0) { + // PX4_INFO(" %s", devname.c_str()); + // } + // } } void VDev::showTopics() diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6ac5046ac7..b95ee30639 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -467,31 +467,33 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta #else - std::string devname; - unsigned int index = 0; - for(;;) { - if (DevMgr::getNextDeviceName(index, devname) < 0) { - break; - } + // TODO NOT IMPLEMENTED - /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname.c_str())) { - continue; - } + // std::string devname; + // unsigned int index = 0; + // for(;;) { + // if (DevMgr::getNextDeviceName(index, devname) < 0) { + // break; + // } - DevHandle h; - DevMgr::getHandle(devname.c_str(), h); + // /* skip mavlink */ + // if (!strcmp("/dev/mavlink", devname.c_str())) { + // continue; + // } - if (!h.isValid()) { - warn("failed opening device %s", devname.c_str()); - continue; - } + // DevHandle h; + // DevMgr::getHandle(devname.c_str(), h); - int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1); - DevMgr::releaseHandle(h); + // if (!h.isValid()) { + // warn("failed opening device %s", devname.c_str()); + // continue; + // } - printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); - } + // int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1); + // DevMgr::releaseHandle(h); + + // printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); + // } ret = TRANSITION_CHANGED; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); From b50c332c763588d19270e406089580cbb9d40df3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:32:40 +0100 Subject: [PATCH 054/624] Accel sim: Code style --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 8fd91580b0..300ba1bbeb 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -529,6 +529,7 @@ int ACCELSIM::devIOCTL(unsigned long cmd, void *arg) { unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { @@ -661,6 +662,7 @@ int ACCELSIM::mag_ioctl(unsigned long cmd, void *arg) { unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { From a3f2c7ac13a634943539b1ce0793b9e516f9077b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 15:32:48 +0100 Subject: [PATCH 055/624] Baro sim: Code style --- src/platforms/posix/drivers/barosim/baro.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 02b9bb9183..4353b83f9a 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -264,8 +264,10 @@ BAROSIM::init() } struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ _measure_phase = 0; + _reports->flush(); _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, From 643fccb0149984cbd4dc8337c786faf39f4e1014 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:56:23 +0100 Subject: [PATCH 056/624] Accel sim: No module file --- src/platforms/posix/drivers/accelsim/module.mk | 7 ------- 1 file changed, 7 deletions(-) delete mode 100644 src/platforms/posix/drivers/accelsim/module.mk diff --git a/src/platforms/posix/drivers/accelsim/module.mk b/src/platforms/posix/drivers/accelsim/module.mk deleted file mode 100644 index dc980c5cc8..0000000000 --- a/src/platforms/posix/drivers/accelsim/module.mk +++ /dev/null @@ -1,7 +0,0 @@ -# -# Simulated accel/mag driver -# - -MODULE_COMMAND = accelsim -SRCS = accelsim.cpp - From 3607d80de2382f7fe967819a45a11c8cdf02ec11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:56:38 +0100 Subject: [PATCH 057/624] ADC sim: no module file --- src/platforms/posix/drivers/adcsim/module.mk | 42 -------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/platforms/posix/drivers/adcsim/module.mk diff --git a/src/platforms/posix/drivers/adcsim/module.mk b/src/platforms/posix/drivers/adcsim/module.mk deleted file mode 100644 index a8ad55effc..0000000000 --- a/src/platforms/posix/drivers/adcsim/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# STM32 ADC driver -# - -MODULE_COMMAND = adcsim - -SRCS = adcsim.cpp - -MAXOPTIMIZATION = -Os From 50820157cee3bb227bfe745d7ac6eb958888f68f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:56:51 +0100 Subject: [PATCH 058/624] Baro sim: No module file --- src/platforms/posix/drivers/barosim/module.mk | 42 ------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/platforms/posix/drivers/barosim/module.mk diff --git a/src/platforms/posix/drivers/barosim/module.mk b/src/platforms/posix/drivers/barosim/module.mk deleted file mode 100644 index 5a4cb4ebb9..0000000000 --- a/src/platforms/posix/drivers/barosim/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# MS5611 driver -# - -MODULE_COMMAND = barosim - -SRCS = baro.cpp baro_sim.cpp - -MAXOPTIMIZATION = -Os From 72bd43d97239b586ff57b3e3e64490b5dd3ffe41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:57:09 +0100 Subject: [PATCH 059/624] GPS sim: no module file --- src/platforms/posix/drivers/gpssim/module.mk | 42 -------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/platforms/posix/drivers/gpssim/module.mk diff --git a/src/platforms/posix/drivers/gpssim/module.mk b/src/platforms/posix/drivers/gpssim/module.mk deleted file mode 100644 index 8db69770fe..0000000000 --- a/src/platforms/posix/drivers/gpssim/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Simulated GPS driver -# - -MODULE_COMMAND = gpssim - -SRCS = gpssim.cpp - -MAXOPTIMIZATION = -Os From aac1afd83dee3ec370e93227644cf174beb5bb08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:57:23 +0100 Subject: [PATCH 060/624] Gyro sim: No module file --- src/platforms/posix/drivers/gyrosim/module.mk | 46 ------------------- 1 file changed, 46 deletions(-) delete mode 100644 src/platforms/posix/drivers/gyrosim/module.mk diff --git a/src/platforms/posix/drivers/gyrosim/module.mk b/src/platforms/posix/drivers/gyrosim/module.mk deleted file mode 100644 index 7bd37c2e2a..0000000000 --- a/src/platforms/posix/drivers/gyrosim/module.mk +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the MPU6000 driver. -# - -MODULE_COMMAND = gyrosim - -SRCS = gyrosim.cpp - -MODULE_STACKSIZE = 1200 - -EXTRACXXFLAGS = -Weffc++ - -MAXOPTIMIZATION = -Os From df08ee52cf33a15b3c37bfe8769a68e205d0d6c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:57:57 +0100 Subject: [PATCH 061/624] Tone alarm: no module file --- .../posix/drivers/tonealrmsim/module.mk | 42 ------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/platforms/posix/drivers/tonealrmsim/module.mk diff --git a/src/platforms/posix/drivers/tonealrmsim/module.mk b/src/platforms/posix/drivers/tonealrmsim/module.mk deleted file mode 100644 index 948b3912a5..0000000000 --- a/src/platforms/posix/drivers/tonealrmsim/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Tone alarm driver -# - -MODULE_COMMAND = tone_alarm - -SRCS = tone_alarm.cpp - -MAXOPTIMIZATION = -Os From 4c47fa70a8c0179f976665bae5f3d6f9b1c1b1b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Nov 2015 23:58:14 +0100 Subject: [PATCH 062/624] Airspeed: No module file --- .../posix/drivers/airspeedsim/module.mk | 41 ------------------- 1 file changed, 41 deletions(-) delete mode 100644 src/platforms/posix/drivers/airspeedsim/module.mk diff --git a/src/platforms/posix/drivers/airspeedsim/module.mk b/src/platforms/posix/drivers/airspeedsim/module.mk deleted file mode 100644 index fb12fa04a2..0000000000 --- a/src/platforms/posix/drivers/airspeedsim/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the generic airspeed driver. -# -MODULE_COMMAND = measairspeedsim - -SRCS = airspeedsim.cpp meas_airspeed_sim.cpp - -MAXOPTIMIZATION = -Os From c5c783bad93d6c396371e1b0141dc687d5eb8371 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 00:04:32 +0100 Subject: [PATCH 063/624] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 8673518203..44403c8eb5 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 8673518203fdb601fc21f10ee94184090be9e95f +Subproject commit 44403c8eb5c6c26a33d086a200d017c57d658095 From b05294d4b4cc46b8c859dc3e584c84a319bfda29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 00:05:45 +0100 Subject: [PATCH 064/624] Accel sim: Fix ioctl interface --- .../posix/drivers/accelsim/accelsim.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 300ba1bbeb..e039e6b65c 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -93,7 +93,7 @@ public: virtual int init(); virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, void *arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * dump register values @@ -104,7 +104,7 @@ protected: friend class ACCELSIM_mag; ssize_t mag_read(void *buffer, size_t buflen); - int mag_ioctl(unsigned long cmd, void *arg); + int mag_ioctl(unsigned long cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -245,7 +245,7 @@ public: ~ACCELSIM_mag(); virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, void *arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); protected: friend class ACCELSIM; @@ -526,7 +526,7 @@ ACCELSIM::mag_read(void *buffer, size_t buflen) } int -ACCELSIM::devIOCTL(unsigned long cmd, void *arg) +ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) { unsigned long ul_arg = (unsigned long)arg; @@ -550,10 +550,10 @@ ACCELSIM::devIOCTL(unsigned long cmd, void *arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return devIOCTL(SENSORIOCSPOLLRATE, (void *)1600); + return devIOCTL(SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return devIOCTL(SENSORIOCSPOLLRATE, (void *)ACCELSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -659,14 +659,14 @@ ACCELSIM::devIOCTL(unsigned long cmd, void *arg) } int -ACCELSIM::mag_ioctl(unsigned long cmd, void *arg) +ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) { unsigned long ul_arg = (unsigned long)arg; switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (ul_arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -685,7 +685,7 @@ ACCELSIM::mag_ioctl(unsigned long cmd, void *arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ - return mag_ioctl(SENSORIOCSPOLLRATE, (void *)100); + return mag_ioctl(SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { @@ -725,11 +725,11 @@ ACCELSIM::mag_ioctl(unsigned long cmd, void *arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((ul_arg < 1) || (ul_arg > 100)) { + if ((arg < 1) || (arg > 100)) { return -EINVAL; } - if (!_mag_reports->resize(ul_arg)) { + if (!_mag_reports->resize(arg)) { return -ENOMEM; } @@ -765,7 +765,7 @@ ACCELSIM::mag_ioctl(unsigned long cmd, void *arg) return OK; case MAGIOCSRANGE: - return mag_set_range(ul_arg); + return mag_set_range(arg); case MAGIOCGRANGE: return _mag_range_ga; @@ -1033,7 +1033,7 @@ ACCELSIM_mag::devRead(void *buffer, size_t buflen) } int -ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg) +ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) { switch (cmd) { case DEVIOCGDEVICEID: From 6c43b6c964e2441cbcf9545c37e8fd17e03bdb81 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 00:06:18 +0100 Subject: [PATCH 065/624] Baro sim: fix baro ioctl interface --- src/platforms/posix/drivers/barosim/baro.cpp | 23 ++++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 4353b83f9a..a77f372c77 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -95,7 +95,7 @@ public: virtual int init(); virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, void *arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -395,14 +395,13 @@ BAROSIM::devRead(void *buffer, size_t buflen) } int -BAROSIM::devIOCTL(unsigned long cmd, void *arg) +BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { - unsigned long ul_arg = (unsigned long)arg; switch (cmd) { case SENSORIOCSPOLLRATE: - switch (ul_arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -440,7 +439,7 @@ BAROSIM::devIOCTL(unsigned long cmd, void *arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned long ticks = USEC2TICK(1000000 / ul_arg); + unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { @@ -468,11 +467,11 @@ BAROSIM::devIOCTL(unsigned long cmd, void *arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((ul_arg < 1) || (ul_arg > 100)) { + if ((arg < 1) || (arg > 100)) { return -EINVAL; } - if (!_reports->resize(ul_arg)) { + if (!_reports->resize(arg)) { return -ENOMEM; } @@ -492,11 +491,11 @@ BAROSIM::devIOCTL(unsigned long cmd, void *arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((ul_arg < 80000) || (ul_arg > 120000)) { + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; } - _msl_pressure = ul_arg; + _msl_pressure = arg; return OK; case BAROIOCGMSLPRESSURE: @@ -542,7 +541,7 @@ void BAROSIM::cycle() { int ret; - void *dummy = nullptr; + unsigned long dummy = 0; /* collection phase? */ if (_collect_phase) { @@ -552,7 +551,7 @@ BAROSIM::cycle() if (ret != OK) { /* issue a reset command to the sensor */ - _interface->devIOCTL(IOCTL_RESET, (void *)dummy); + _interface->devIOCTL(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; @@ -618,7 +617,7 @@ BAROSIM::measure() /* * Send the command to begin measuring. */ - ret = _interface->devIOCTL(IOCTL_MEASURE, (void *)addr); + ret = _interface->devIOCTL(IOCTL_MEASURE, addr); if (OK != ret) { perf_count(_comms_errors); From fd0584e95de8c8a3e3fb23585f0331a4d0001ebf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 00:08:01 +0100 Subject: [PATCH 066/624] Gyro sim IOCTL fix --- .../posix/drivers/gyrosim/gyrosim.cpp | 43 +++++++++---------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 151777de6b..8901d2bf63 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -128,7 +128,7 @@ public: virtual int start(); virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, void *arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -142,7 +142,7 @@ protected: friend class GYROSIM_gyro; virtual ssize_t gyro_read(void *buffer, size_t buflen); - virtual int gyro_ioctl(unsigned long cmd, void *arg); + virtual int gyro_ioctl(unsigned long cmd, unsigned long arg); private: GYROSIM_gyro *_gyro; @@ -284,7 +284,7 @@ public: virtual ~GYROSIM_gyro() {} virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, void *arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); virtual int init(); @@ -726,9 +726,8 @@ GYROSIM::gyro_read(void *buffer, size_t buflen) } int -GYROSIM::devIOCTL(unsigned long cmd, void *arg) +GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { - unsigned long ul_arg = (unsigned long)arg; switch (cmd) { @@ -736,7 +735,7 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) return reset(); case SENSORIOCSPOLLRATE: { - switch (ul_arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -753,15 +752,15 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return devIOCTL(SENSORIOCSPOLLRATE, (void *)1000); + return devIOCTL(SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: - return devIOCTL(SENSORIOCSPOLLRATE, (void *)GYROSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / ul_arg; + unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) { @@ -793,11 +792,11 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((ul_arg < 1) || (ul_arg > 100)) { + if ((arg < 1) || (arg > 100)) { return -EINVAL; } - if (!_accel_reports->resize(ul_arg)) { + if (!_accel_reports->resize(arg)) { return -ENOMEM; } @@ -811,7 +810,7 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) return _sample_rate; case ACCELIOCSSAMPLERATE: - _set_sample_rate(ul_arg); + _set_sample_rate(arg); return OK; case ACCELIOCSLOWPASS: @@ -819,7 +818,7 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) ul_arg; + struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -833,11 +832,11 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale *) ul_arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: - return set_accel_range(ul_arg); + return set_accel_range(arg); case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); @@ -852,10 +851,8 @@ GYROSIM::devIOCTL(unsigned long cmd, void *arg) } int -GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) +GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg) { - unsigned long ul_arg = (unsigned long)arg; - switch (cmd) { /* these are shared with the accel side */ @@ -866,11 +863,11 @@ GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((ul_arg < 1) || (ul_arg > 100)) { + if ((arg < 1) || (arg > 100)) { return -EINVAL; } - if (!_gyro_reports->resize(ul_arg)) { + if (!_gyro_reports->resize(arg)) { return -ENOMEM; } @@ -884,7 +881,7 @@ GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) return _sample_rate; case GYROIOCSSAMPLERATE: - _set_sample_rate(ul_arg); + _set_sample_rate(arg); return OK; case GYROIOCSLOWPASS: @@ -1161,7 +1158,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : // Set sample interval to 0 since device is read by parent - VirtDevObj("GYROSIM_gyro", path, nullptr, 0), + VirtDevObj("GYROSIM_gyro", path, "", 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1) @@ -1188,7 +1185,7 @@ GYROSIM_gyro::devRead(void *buffer, size_t buflen) } int -GYROSIM_gyro::devIOCTL(unsigned long cmd, void *arg) +GYROSIM_gyro::devIOCTL(unsigned long cmd, unsigned long arg) { switch (cmd) { From fa2b1ffa144ba396e7180c1ef76733cc6a03de13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 00:09:26 +0100 Subject: [PATCH 067/624] Tone alarm: IOCTL interface fix --- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index a1ef3b3624..6adffb9bf9 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -119,7 +119,7 @@ public: ToneAlarm(); ~ToneAlarm(); - virtual int devIOCTL(unsigned long cmd, void *arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); virtual ssize_t devWrite(const void *buffer, size_t len); inline const char *name(int tune) { @@ -641,21 +641,19 @@ ToneAlarm::next_trampoline(void *arg) int -ToneAlarm::devIOCTL(unsigned long cmd, void *arg) +ToneAlarm::devIOCTL(unsigned long cmd, unsigned long arg) { int result = OK; - unsigned long ul_arg = (unsigned long)arg; - - PX4_DEBUG("ToneAlarm::devIOCTL %i %lu", cmd, ul_arg); + PX4_DEBUG("ToneAlarm::devIOCTL %i %lu", cmd, arg); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - PX4_INFO("TONE_SET_ALARM %lu", ul_arg); + PX4_INFO("TONE_SET_ALARM %lu", arg); - if (ul_arg < TONE_NUMBER_OF_TUNES) { - if (ul_arg == TONE_STOP_TUNE) { + if (arg < TONE_NUMBER_OF_TUNES) { + if (arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; @@ -664,10 +662,10 @@ ToneAlarm::devIOCTL(unsigned long cmd, void *arg) } else { /* always interrupt alarms, unless they are repeating and already playing */ - if (!(_repeat && _default_tune_number == ul_arg)) { + if (!(_repeat && _default_tune_number == arg)) { /* play the selected tune */ - _default_tune_number = ul_arg; - start_tune(_default_tunes[ul_arg]); + _default_tune_number = arg; + start_tune(_default_tunes[arg]); } } @@ -860,18 +858,19 @@ tone_alarm_main(int argc, char *argv[]) } /* if it looks like a PLAY string... */ - else if (strlen(argv1) > 2) { + else if (argv1 && (strlen(argv1) > 2)) { if (*argv1 == 'M') { ret = play_string(argv1, false); } } else { /* It might be a tune name */ - for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) { if (!strcmp(g_dev->name(tune), argv1)) { ret = play_tune(tune); return ret; } + } PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); ret = 1; From fc0ff0d2f674ceb14c8612f339786a071ff214dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 10:31:37 +0100 Subject: [PATCH 068/624] Baro sim: Initialize struct --- src/platforms/posix/drivers/barosim/baro.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index a77f372c77..de6433f8e3 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -245,6 +245,7 @@ int BAROSIM::init() { int ret; + struct baro_report brp = {}; //PX4_DEBUG("BAROSIM::init"); ret = VirtDevObj::init(); @@ -263,8 +264,6 @@ BAROSIM::init() goto out; } - struct baro_report brp; - /* do a first measurement cycle to populate reports with valid data */ _measure_phase = 0; From 65cf8caef2fe262dbcb58c7455b8c3e6548e9f77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 11:16:42 +0100 Subject: [PATCH 069/624] POSIX Device: Fix IOCTL interface --- src/drivers/device/device_posix.cpp | 2 +- src/drivers/device/vdev.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 088d7ccdf8..cd3bad09ce 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -96,7 +96,7 @@ Device::dev_write(unsigned offset, void *data, unsigned count) } int -Device::dev_ioctl(unsigned operation, unsigned &arg) +Device::dev_ioctl(unsigned operation, unsigned arg) { switch (operation) { case DEVIOCGDEVICEID: diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index d1fb8ff8e1..419c92e456 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -128,7 +128,7 @@ public: * @param arg An argument to the operation. * @return Negative errno on error, OK or positive value on success. */ - virtual int dev_ioctl(unsigned operation, unsigned &arg); + virtual int dev_ioctl(unsigned operation, unsigned arg); /* device bus types for DEVID From cd7834292a6206ea9667bbc5c02e9c32e8a10757 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 11:17:10 +0100 Subject: [PATCH 070/624] MAVLink log: Use right context for checking --- src/modules/systemlib/mavlink_log.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 56201588a2..f7388091e7 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -36,7 +35,7 @@ * @file mavlink_log.c * MAVLink text logging. * - * @author Lorenz Meier + * @author Lorenz Meier */ #include @@ -106,7 +105,7 @@ __EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int seve va_start(ap, fmt); int end = (lb->start + lb->count) % lb->size; lb->elems[end].severity = severity; - vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap); + vsnprintf(lb->elems[end].text, sizeof(lb->elems[end].text), fmt, ap); va_end(ap); /* increase count */ From a26d7c35cda740a2456539e9eb111c71a6b90372 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 11:17:28 +0100 Subject: [PATCH 071/624] Baro sim: use right IOCTL API --- src/platforms/posix/drivers/barosim/baro_sim.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 5fc38aaa1c..22574c27dc 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -64,7 +64,7 @@ public: virtual int init(); virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned &arg); + virtual int dev_ioctl(unsigned operation, unsigned arg); virtual int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); @@ -143,7 +143,7 @@ BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) } int -BARO_SIM::dev_ioctl(unsigned operation, unsigned &arg) +BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) { int ret; @@ -208,6 +208,10 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); + } else if (send_len == 1 || send[0] != 12) { + /* measure command */ + return 0; + } else if (send_len != 1 || send[0] != 0) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; From 1fa20819d8413280066ea087149ac5b78f1e0450 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 11:18:30 +0100 Subject: [PATCH 072/624] sdlog2: Fix string overflow in file name --- src/modules/sdlog2/sdlog2.c | 34 ++++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index be08851c25..692549b39a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -156,7 +156,9 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char log_dir[32]; +#define LOG_BASE_PATH_LEN 64 + +static char log_dir[LOG_BASE_PATH_LEN]; /* statistics counters */ static uint64_t start_time = 0; @@ -444,15 +446,15 @@ int create_log_dir() } /* print logging path, important to find log file later */ - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir); return 0; } int open_log_file() { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -480,7 +482,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -492,10 +494,10 @@ int open_log_file() #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name); } return fd; @@ -504,8 +506,8 @@ int open_log_file() int open_perf_file(const char* str) { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -532,7 +534,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -544,7 +546,7 @@ int open_perf_file(const char* str) #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } @@ -671,7 +673,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir"); return; } @@ -761,7 +763,7 @@ void sdlog2_stop_log() /* free log writer performance counter */ perf_free(perf_write); - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped"); sdlog2_status(); } @@ -1911,7 +1913,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } } @@ -1936,7 +1938,7 @@ int check_free_space() /* use a threshold of 50 MiB */ if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] no space on MicroSD: %u MiB", + "[log] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return PX4_ERROR; @@ -1944,7 +1946,7 @@ int check_free_space() /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] space on MicroSD low: %u MiB", + "[log] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; From 40b488d69336eb5d3ca7102ecca87f1b217eaa8e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Nov 2015 12:54:54 -0800 Subject: [PATCH 073/624] Updated to latest DriverFramework Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 8 ++++---- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 3 ++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index e039e6b65c..141751fc03 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1110,13 +1110,13 @@ start(enum Rotation rotation) DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ - if (!h_mag.isValid()) { - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + if (h_mag.isValid()) { + if (h_mag.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } } else { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } DevMgr::releaseHandle(h); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 8901d2bf63..3f3046f3d4 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1169,7 +1169,8 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : int GYROSIM_gyro::init() { - return start(); + int ret = VirtDevObj::init(); + return ret ? ret : start(); } void From dbe3b0e52b45977910b49e5b1de74ebd25ac614a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 13:51:39 -0800 Subject: [PATCH 074/624] Re-enabling code to handle DF framework Updated to latest DriverFramework and changed ioctl args to unsigned int from void *. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 16 +++-- src/lib/DriverFramework | 2 +- .../commander/state_machine_helper.cpp | 64 +++++++++++++------ src/modules/simulator/simulator.cpp | 13 ++-- .../posix/drivers/accelsim/accelsim.cpp | 4 +- src/platforms/posix/drivers/barosim/baro.cpp | 12 ++-- .../posix/drivers/gyrosim/gyrosim.cpp | 8 +-- 7 files changed, 75 insertions(+), 44 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8983934804..0b8044274c 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -552,12 +552,16 @@ void VDev::showDevices() PX4_INFO("DF Devices:"); // TODO NOT IMPLEMENTED - // std::string devname; - // for (unsigned int index=0; i == 0; ++i) { - // if (DevMgr::getNextDeviceName(index, devname) == 0) { - // PX4_INFO(" %s", devname.c_str()); - // } - // } + std::string devname; + unsigned int index = 0; + i = 0; + do { + // Each look increments index and returns -1 if end reached + i = DevMgr::getNextDeviceName(index, devname); + if (i == 0) { + PX4_INFO(" %s", devname.c_str()); + } + } while (i==0); } void VDev::showTopics() diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 44403c8eb5..a09333b9e4 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 44403c8eb5c6c26a33d086a200d017c57d658095 +Subproject commit a09333b9e418b84b51788ec9f349e4d4b9253448 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index b95ee30639..f794972e8a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -467,33 +467,55 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta #else - // TODO NOT IMPLEMENTED + // Handle VDev devices + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; - // std::string devname; - // unsigned int index = 0; - // for(;;) { - // if (DevMgr::getNextDeviceName(index, devname) < 0) { - // break; - // } + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } - // /* skip mavlink */ - // if (!strcmp("/dev/mavlink", devname.c_str())) { - // continue; - // } - // DevHandle h; - // DevMgr::getHandle(devname.c_str(), h); + int sensfd = px4_open(devname, 0); - // if (!h.isValid()) { - // warn("failed opening device %s", devname.c_str()); - // continue; - // } + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } - // int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1); - // DevMgr::releaseHandle(h); + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); - // printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); - // } + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + + + // Handle DF devices + std::string dfdevname; + unsigned int index = 0; + for(;;) { + if (DevMgr::getNextDeviceName(index, dfdevname) < 0) { + break; + } + + DevHandle h; + DevMgr::getHandle(dfdevname.c_str(), h); + + if (!h.isValid()) { + warn("failed opening device %s", dfdevname.c_str()); + continue; + } + + int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1); + DevMgr::releaseHandle(h); + + printf("Disabling %s: %s\n", dfdevname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); + } ret = TRANSITION_CHANGED; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 1656a3451f..4b34355e56 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -135,10 +135,12 @@ int Simulator::start(int argc, char *argv[]) // Update sensor data _instance->pollForMAVLinkMessages(false); #endif - - } else { + } else if (argv[2][1] == 'p') { // Update sensor data _instance->pollForMAVLinkMessages(true); + } else { + _instance->initializeSensorData(); + _instance->_initialized = true; } } else { @@ -151,9 +153,10 @@ int Simulator::start(int argc, char *argv[]) static void usage() { - PX4_WARN("Usage: simulator {start -[sc] |stop}"); + PX4_WARN("Usage: simulator {start -[spt] |stop}"); PX4_WARN("Simulate raw sensors: simulator start -s"); PX4_WARN("Publish sensors combined: simulator start -p"); + PX4_WARN("Dummy unit test data: simulator start -t"); } __BEGIN_DECLS @@ -167,7 +170,9 @@ extern "C" { int ret = 0; if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { + if (strcmp(argv[2], "-s") == 0 || + strcmp(argv[2], "-p") == 0 || + strcmp(argv[2], "-t") == 0) { if (g_sim_task >= 0) { warnx("Simulator already started"); return 0; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 141751fc03..f95e508b21 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1101,7 +1101,7 @@ start(enum Rotation rotation) goto fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); DevMgr::releaseHandle(h); goto fail; @@ -1111,7 +1111,7 @@ start(enum Rotation rotation) /* don't fail if mag dev cannot be opened */ if (h_mag.isValid()) { - if (h_mag.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); } diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index de6433f8e3..578846a58e 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -834,7 +834,7 @@ start_bus(struct barosim_bus_option &bus) return false; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { DevMgr::releaseHandle(h); PX4_ERR("failed setting default poll rate"); return false; @@ -904,14 +904,14 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); /* set the queue depth to 10 */ - if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, (void *)10UL)) { + if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, 10UL)) { PX4_ERR("failed to set queue depth"); DevMgr::releaseHandle(h); return 1; } /* start the sensor polling at 2Hz */ - if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)2UL)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, 2UL)) { PX4_ERR("failed to set 2Hz poll rate"); DevMgr::releaseHandle(h); return 1; @@ -971,7 +971,7 @@ reset() return 1; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); return 1; } @@ -1017,7 +1017,7 @@ calibrate(unsigned altitude) } /* start the sensor polling at max */ - if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MAX)) { + if (OK != h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { PX4_ERR("failed to set poll rate"); return 1; } @@ -1068,7 +1068,7 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; - if (h.ioctl(BAROIOCSMSLPRESSURE, (void *)((unsigned long)(p1))) != OK) { + if (h.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(p1)) != OK) { PX4_WARN("BAROIOCSMSLPRESSURE"); return 1; } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 3f3046f3d4..7dc5032eb4 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1253,7 +1253,7 @@ start(enum Rotation rotation) goto fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { DevMgr::releaseHandle(h); goto fail; } @@ -1321,7 +1321,7 @@ test() } /* reset to manual polling */ - if (h_accel.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MANUAL) < 0) { + if (h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { PX4_ERR("reset to manual polling"); return 1; } @@ -1394,12 +1394,12 @@ reset() } - if (h.ioctl(SENSORIOCRESET, (void *)0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); goto reset_fail; } - if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); goto reset_fail; } From 3f20d78dfe618fa45bb2e393cf450ff29458118f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 14:01:39 -0800 Subject: [PATCH 075/624] Fixed code style Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 10 ++++++---- src/modules/simulator/simulator.cpp | 4 +++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0b8044274c..63ee720bdf 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -555,13 +555,15 @@ void VDev::showDevices() std::string devname; unsigned int index = 0; i = 0; + do { // Each look increments index and returns -1 if end reached - i = DevMgr::getNextDeviceName(index, devname); + i = DevMgr::getNextDeviceName(index, devname); + if (i == 0) { - PX4_INFO(" %s", devname.c_str()); - } - } while (i==0); + PX4_INFO(" %s", devname.c_str()); + } + } while (i == 0); } void VDev::showTopics() diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 4b34355e56..44b3bcf0e1 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -135,9 +135,11 @@ int Simulator::start(int argc, char *argv[]) // Update sensor data _instance->pollForMAVLinkMessages(false); #endif + } else if (argv[2][1] == 'p') { // Update sensor data _instance->pollForMAVLinkMessages(true); + } else { _instance->initializeSensorData(); _instance->_initialized = true; @@ -170,7 +172,7 @@ extern "C" { int ret = 0; if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || + if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0 || strcmp(argv[2], "-t") == 0) { if (g_sim_task >= 0) { From 82b3ea2501bfaa8c605de4bd0202bf9565ef84e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 17 Nov 2015 02:00:42 +0300 Subject: [PATCH 076/624] Removed an excessive template disambiguator in the virtual CAN driver class (fixes 3190) --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 956caf11ee..d395f2b76c 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -494,9 +494,8 @@ public: const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template - construct - (allocator, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].construct(allocator, clock_, driver_mutex_, quota_per_queue); } } From 618626103f8c8cb4daa9eeabdfd3a88dabca53d3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 16:57:57 -0800 Subject: [PATCH 077/624] Use DevMgr::getNextDevicePath() Also, a fix for number of args passed for accelsim. Updated to newest version of DriverFramework. DF drvices show up now in list_devices Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 8 +++----- src/lib/DriverFramework | 2 +- src/modules/commander/state_machine_helper.cpp | 10 +++++----- src/platforms/posix/drivers/accelsim/accelsim.cpp | 4 ++++ 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 63ee720bdf..470b8388b3 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -550,18 +550,16 @@ void VDev::showDevices() pthread_mutex_unlock(&devmutex); PX4_INFO("DF Devices:"); - // TODO NOT IMPLEMENTED - - std::string devname; + std::string dev_path, instance_path; unsigned int index = 0; i = 0; do { // Each look increments index and returns -1 if end reached - i = DevMgr::getNextDeviceName(index, devname); + i = DevMgr::getNextDevicePath(index, dev_path, instance_path); if (i == 0) { - PX4_INFO(" %s", devname.c_str()); + PX4_INFO(" %s (%s)", dev_path.c_str(), instance_path.c_str()); } } while (i == 0); } diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index a09333b9e4..45878058a7 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit a09333b9e418b84b51788ec9f349e4d4b9253448 +Subproject commit 45878058a7b37a9bba370396526b95cd7c2a3ab9 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f794972e8a..a135e9dfca 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -496,25 +496,25 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta // Handle DF devices - std::string dfdevname; + std::string df_dev_path, df_dev_instance_path; unsigned int index = 0; for(;;) { - if (DevMgr::getNextDeviceName(index, dfdevname) < 0) { + if (DevMgr::getNextDevicePath(index, df_dev_path, df_dev_instance_path) < 0) { break; } DevHandle h; - DevMgr::getHandle(dfdevname.c_str(), h); + DevMgr::getHandle(df_dev_path.c_str(), h); if (!h.isValid()) { - warn("failed opening device %s", dfdevname.c_str()); + warn("failed opening device %s", df_dev_path.c_str()); continue; } int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1); DevMgr::releaseHandle(h); - printf("Disabling %s: %s\n", dfdevname.c_str(), (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", df_dev_path.c_str(), (block_ret == OK) ? "OK" : "ERROR"); } ret = TRANSITION_CHANGED; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index f95e508b21..5928ca834b 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1182,6 +1182,10 @@ accelsim_main(int argc, char *argv[]) } } + if (argc <= 1) { + accelsim::usage(); + return 1; + } const char *verb = argv[myoptind]; /* From b6963be9eb9ffe34961026c77da04b2bf0a9e682 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 17:35:54 -0800 Subject: [PATCH 078/624] Fixed gyrosim Was not calling base class init() Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 7dc5032eb4..4adb3db9d7 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -308,7 +308,7 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VirtDevObj("GYROSIM", path_accel, nullptr, 1000), + VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), _accel_reports(nullptr), @@ -390,6 +390,13 @@ GYROSIM::init() struct gyro_report grp = {}; + ret = VirtDevObj::init(); + if (ret != 0) { + PX4_WARN("Base class init failed"); + ret = 1; + goto out; + } + /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); @@ -1158,7 +1165,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : // Set sample interval to 0 since device is read by parent - VirtDevObj("GYROSIM_gyro", path, "", 0), + VirtDevObj("GYROSIM_gyro", path, GYRO_BASE_DEVICE_PATH, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1) From befec98e1792c0451ac4d2b9ae0d4a18a4fd3d9c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 17:42:33 -0800 Subject: [PATCH 079/624] Fixed adcsim Was missing call of base class init(). Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 4326314438..8ae066e5f5 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -239,6 +239,12 @@ adcsim_main(int argc, char *argv[]) PX4_ERR("couldn't allocate the ADCSIM driver"); return 1; } + + ret = g_adc->init(); + if (ret != 0) { + PX4_ERR("ADCSIM init failed (%d)", ret); + return 1; + } } if (argc > 1 && g_adc) { From 62eba58fb4d3361ae84ded5ec7b82803f9f1eb71 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 17:46:22 -0800 Subject: [PATCH 080/624] Code format fixes Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 1 + src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 5928ca834b..1ba06148cb 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1186,6 +1186,7 @@ accelsim_main(int argc, char *argv[]) accelsim::usage(); return 1; } + const char *verb = argv[myoptind]; /* diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 8ae066e5f5..5cd9fff61f 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -241,6 +241,7 @@ adcsim_main(int argc, char *argv[]) } ret = g_adc->init(); + if (ret != 0) { PX4_ERR("ADCSIM init failed (%d)", ret); return 1; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 4adb3db9d7..c44b10e879 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -391,6 +391,7 @@ GYROSIM::init() struct gyro_report grp = {}; ret = VirtDevObj::init(); + if (ret != 0) { PX4_WARN("Base class init failed"); ret = 1; From 1bcb93180b31c03789e59e3e71a260d5d3b607f6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 23:24:59 -0800 Subject: [PATCH 081/624] Set flags for building the proper DriverFramework target The DF_TARGET cmake variable needs to be set to (linux, darwin, qurt, nuttx) The define __DF_LINUX or __DF_QURT needs to be set. The __DF_DARWIN target is untested Signed-off-by: Mark Charlebois --- CMakeLists.txt | 14 ++++++++++++++ cmake/nuttx/px4_impl_nuttx.cmake | 1 + cmake/posix/px4_impl_posix.cmake | 2 ++ src/lib/DriverFramework | 2 +- src/lib/dspal | 2 +- 5 files changed, 19 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f2e6dfdda..64c73efa67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,6 +150,16 @@ list(GET config_args 1 BOARD) list(GET config_args 2 LABEL) set(target_name "${OS}-${BOARD}-${LABEL}") +if("${OS}" STREQUAL "posix") + if (APPLE) + set(DF_TARGET darwin) + else() + set(DF_TARGET linux) + endif() +else() + set(DF_TARGET ${OS}) +endif() + message(STATUS "${target_name}") # switch to ros CMake file if building ros @@ -327,6 +337,10 @@ endforeach() add_subdirectory(src/firmware/${OS}) add_subdirectory(src/lib/DriverFramework) +if("${OS}" STREQUAL "nuttx") + add_dependencies(df_driver_framework nuttx_export_${BOARD}) +endif() + if (config_io_board) add_subdirectory(src/modules/px4iofirmware) endif() diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index af7391f45c..14bd2ee3d6 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -429,6 +429,7 @@ function(px4_os_add_flags) ) set(added_definitions -D__PX4_NUTTX + -D__DF_NUTTX ) set(added_c_flags -nodefaultlibs diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index 28b27f87d1..ca6033448d 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -171,6 +171,7 @@ if(UNIX AND APPLE) set(added_definitions -D__PX4_POSIX -D__PX4_DARWIN + -D__DF_DARWIN -DCLOCK_MONOTONIC=1 -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h @@ -185,6 +186,7 @@ else() set(added_definitions -D__PX4_POSIX -D__PX4_LINUX + -D__DF_LINUX -DCLOCK_MONOTONIC=1 -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 45878058a7..3bbaf1a46d 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 45878058a7b37a9bba370396526b95cd7c2a3ab9 +Subproject commit 3bbaf1a46dfdb0b3506dd4a0e4441f17b53c1944 diff --git a/src/lib/dspal b/src/lib/dspal index c8e885aac5..93b8d36619 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit c8e885aac51aa34855bb3880d3dc916b9e278083 +Subproject commit 93b8d366191fb24486e0484ca4f020c85ed6cfee From 8a6b7198973134be27eba1c67b4569b34c5764e2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 23:35:39 -0800 Subject: [PATCH 082/624] Updated DriverFramework Fixed failure due to float promotiion to double Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 3bbaf1a46d..231ec6c443 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 3bbaf1a46dfdb0b3506dd4a0e4441f17b53c1944 +Subproject commit 231ec6c443458d2cd8e1f9068527c71bbee06d4d From 8882e10278c724db189e4746b137d0e361b91538 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Nov 2015 23:41:53 -0800 Subject: [PATCH 083/624] Updated DriverFramework Fixed static function that was not marked static Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 231ec6c443..12373364a3 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 231ec6c443458d2cd8e1f9068527c71bbee06d4d +Subproject commit 12373364a3e78a20abad6cd63b36e355d712d9e4 From dc7369a1d0aca5c24b8523b2540f7a76a13b6aab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:05:24 +0100 Subject: [PATCH 084/624] EKF: Report horizontal acceleration --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- src/modules/ekf_att_pos_estimator/estimator_22states.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f4fcf78e25..5505b4c239 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -852,7 +852,9 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Accelerations in Body Frame */ _ctrl_state.x_acc = _ekf->accel.x; _ctrl_state.y_acc = _ekf->accel.y; - _ctrl_state.z_acc = _ekf->accel.z; + _ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13]; + + _ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal(); /* Velocity in Body Frame */ Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 6eb325d531..c0a51afc0a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -382,6 +382,8 @@ public: void get_covariance(float c[28]); + float getAccNavMagHorizontal() { return _accNavMagHorizontal; } + protected: /** From f02dc3c95f716277a319699aeea233343574b8eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:06:00 +0100 Subject: [PATCH 085/624] Fixed wing land detector: Use control state, use horizontal acceleration for takeoff detect --- .../land_detector/FixedwingLandDetector.cpp | 63 +++++++++++-------- .../land_detector/FixedwingLandDetector.h | 14 +++-- src/modules/land_detector/LandDetector.cpp | 7 ++- .../land_detector/land_detector_params.c | 13 ++++ 4 files changed, 63 insertions(+), 34 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index f8066241c5..7614ccd3d1 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -36,6 +36,7 @@ * Land detection algorithm for fixedwings * * @author Johan Jansen + * @author Lorenz Meier */ #include "FixedwingLandDetector.h" @@ -48,30 +49,31 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition( {}), - _airspeedSub(-1), - _vehicleStatusSub(-1), - _armingSub(-1), - _airspeed{}, - _vehicleStatus{}, - _arming{}, - _parameterSub(-1), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _controlStateSub(-1), + _controlState{}, + _vehicleStatusSub(-1), + _armingSub(-1), + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _accel_x_integral(0.0f), + _lastTime(0), + _lastXAccel(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); + _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX"); } void FixedwingLandDetector::initialize() { // Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); @@ -80,14 +82,16 @@ void FixedwingLandDetector::initialize() void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } bool FixedwingLandDetector::update() { + _lastXAccel = _controlState.x_acc; + _lastTime = _controlState.timestamp; + // First poll for new data from our subscriptions updateSubscriptions(); @@ -99,29 +103,35 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel * + _controlState.x_vel + _controlState.y_vel * _controlState.y_vel); if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } - val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel); if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; } - } - if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _controlState.airspeed; + + if (_lastTime != 0) { + /* a leaking integrator prevents biases from building up, but + * gives a mostly correct response for short impulses + */ + _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; + } } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate - && _airspeed_filtered < _params.maxAirSpeed) { + && _airspeed_filtered < _params.maxAirSpeed + && _accel_x_integral < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -133,6 +143,8 @@ bool FixedwingLandDetector::update() _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } + _lastTime = _controlState.timestamp; + return landDetected; } @@ -151,5 +163,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 325faee794..cce99cb9fc 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,8 +42,7 @@ #define __FIXED_WING_LAND_DETECTOR_H__ #include "LandDetector.h" -#include -#include +#include #include #include #include @@ -83,21 +82,21 @@ private: param_t maxVelocity; param_t maxClimbRate; param_t maxAirSpeed; + param_t maxIntVelocity; } _paramHandle; struct { float maxVelocity; float maxClimbRate; float maxAirSpeed; + float maxIntVelocity; } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; + int _controlStateSub; /**< notification of local position */ + struct control_state_s _controlState; /**< the result from local position subscription */ int _vehicleStatusSub; int _armingSub; - struct airspeed_s _airspeed; struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; int _parameterSub; @@ -105,6 +104,9 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; + float _accel_x_integral; + uint64_t _lastTime; + float _lastXAccel; uint64_t _landDetectTrigger; }; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 3040e23407..5140a2809c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,14 +57,14 @@ _work{} { LandDetector::~LandDetector() { - work_cancel(LPWORK, &_work); + work_cancel(HPWORK, &_work); _taskShouldExit = true; } int LandDetector::start() { /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); return 0; } @@ -107,10 +107,11 @@ void LandDetector::cycle() // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); + //warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF"); } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index d9201eb18a..690467b97c 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); */ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); +/** + * Fixedwing max short-term velocity + * + * Maximum velocity integral in flight direction allowed in the landed state (m/s) + * + * @min 2 + * @max 10 + * @unit m/s + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f); + /** * Airspeed max * From 515d81b8d65dbab25aadf7543893621c918c9a82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:13:56 +0100 Subject: [PATCH 086/624] Land detector: Move back to more agile raw airspeed --- src/modules/land_detector/FixedwingLandDetector.cpp | 9 +++++++-- src/modules/land_detector/FixedwingLandDetector.h | 5 ++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7614ccd3d1..31c1195281 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -50,11 +50,13 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _controlStateSub(-1), - _controlState{}, _vehicleStatusSub(-1), _armingSub(-1), + _airspeedSub(-1), + _controlState{}, _vehicleStatus{}, _arming{}, + _airspeed{}, _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), @@ -76,6 +78,7 @@ void FixedwingLandDetector::initialize() _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); updateParameterCache(true); } @@ -85,6 +88,7 @@ void FixedwingLandDetector::updateSubscriptions() orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() @@ -117,7 +121,7 @@ bool FixedwingLandDetector::update() _velocity_z_filtered = val; } - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _controlState.airspeed; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; if (_lastTime != 0) { /* a leaking integrator prevents biases from building up, but @@ -125,6 +129,7 @@ bool FixedwingLandDetector::update() */ _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; } + } // crude land detector for fixedwing diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index cce99cb9fc..32179c8698 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -46,6 +46,7 @@ #include #include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -94,11 +95,13 @@ private: private: int _controlStateSub; /**< notification of local position */ - struct control_state_s _controlState; /**< the result from local position subscription */ int _vehicleStatusSub; int _armingSub; + int _airspeedSub; + struct control_state_s _controlState; /**< the result from local position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; + struct airspeed_s _airspeed; int _parameterSub; float _velocity_xy_filtered; From de46d8e872a3c6a6b6ee65669b036dfd6760b859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:17:50 +0100 Subject: [PATCH 087/624] Land detector: Code cleanup --- .../land_detector/FixedwingLandDetector.cpp | 20 +++++-------------- .../land_detector/FixedwingLandDetector.h | 4 +--- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 31c1195281..cff46c1a3f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -61,9 +61,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), - _accel_x_integral(0.0f), - _lastTime(0), - _lastXAccel(0.0f), + _accel_horz_lp(0.0f), _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); @@ -93,9 +91,6 @@ void FixedwingLandDetector::updateSubscriptions() bool FixedwingLandDetector::update() { - _lastXAccel = _controlState.x_acc; - _lastTime = _controlState.timestamp; - // First poll for new data from our subscriptions updateSubscriptions(); @@ -123,12 +118,9 @@ bool FixedwingLandDetector::update() _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - if (_lastTime != 0) { - /* a leaking integrator prevents biases from building up, but - * gives a mostly correct response for short impulses - */ - _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; - } + // a leaking lowpass prevents biases from building up, but + // gives a mostly correct response for short impulses + _accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f; } @@ -136,7 +128,7 @@ bool FixedwingLandDetector::update() if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate && _airspeed_filtered < _params.maxAirSpeed - && _accel_x_integral < _params.maxIntVelocity) { + && _accel_horz_lp < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -148,8 +140,6 @@ bool FixedwingLandDetector::update() _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } - _lastTime = _controlState.timestamp; - return landDetected; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 32179c8698..5c5b18a9e5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -107,9 +107,7 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; - float _accel_x_integral; - uint64_t _lastTime; - float _lastXAccel; + float _accel_horz_lp; uint64_t _landDetectTrigger; }; From 2cc9f03c93223be4e3e155b806c59d2ab1df6b59 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 17 Nov 2015 16:30:27 +0100 Subject: [PATCH 088/624] SITL: fixed control callback --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 993958a050..373e7505b6 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -548,7 +548,7 @@ PWMSim::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; if (_armed) { - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; } else { /* clamp actuator to zero if not armed */ From dfdf7dce4d6dac46016fdd849968e6eb8d08a461 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:38:11 +0100 Subject: [PATCH 089/624] new mode for fake gps --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ src/modules/mavlink/mavlink_main.cpp | 9 ++++++++- src/modules/mavlink/mavlink_main.h | 1 + 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4b25987be..411eb0d892 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -564,6 +564,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 fi + if param compare SYS_COMPANION 257600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x + fi # Sensors on the PWM interface bank # clear pins 5 and 6 if param compare SENS_EN_LL40LS 1 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a2f1d583b..1b04eea1a0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(myoptarg, "magic") == 0) { + _mode = MAVLINK_MODE_MAGIC; + } else if (strcmp(myoptarg, "config") == 0) { _mode = MAVLINK_MODE_CONFIG; } @@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); - /* camera trigger is rate limited at the source, do not limit here */ + //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); break; @@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("EXTENDED_SYS_STATE", 1.0f); break; + case MAVLINK_MODE_MAGIC: + //stream nothing + break; + case MAVLINK_MODE_CONFIG: // Enable a number of interesting streams we want via USB configure_stream("SYS_STATUS", 1.0f); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 95e0fdb91a..37c66b5963 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -149,6 +149,7 @@ public: MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, MAVLINK_MODE_OSD, + MAVLINK_MODE_MAGIC, MAVLINK_MODE_CONFIG }; From b0515ef07dabd5e26e84332ba8501d40a1c7f15b Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:47:32 +0100 Subject: [PATCH 090/624] added a disabling flag for mocap when using fake gps --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 23 +++++++++++++------ .../position_estimator_inav_params.c | 17 ++++++++++++-- .../position_estimator_inav_params.h | 2 ++ 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 2d5315b92b..60429f5d22 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() px4_add_module( MODULE modules__position_estimator_inav diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4696d4712b..32cb74ec8b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -586,8 +586,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - //this flag is not working --> - flow_accurate = true; //already checked if flow_q > 0.3 /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ @@ -817,7 +815,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; - ref_inited = false; //if gps gets lost it has to init again to avoid jumps mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } @@ -841,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; @@ -866,7 +863,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); - /* reset position estimate when GPS becomes good */ + /* reset position estimate and ref when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; @@ -880,6 +877,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) est_i += EST_BUF_SIZE; } + //mavlink_log_info(mavlink_fd, "est_i = %d\n", (int)est_i); + /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; @@ -954,9 +953,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* increase EPH/EPV on each step */ + if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 + eph = 0.001; + } if (eph < max_eph_epv) { eph *= 1.0f + dt; } + if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 + epv = 0.001; + } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } @@ -969,11 +974,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; + if(params.disable_mocap) { //disable mocap if fake gps is used + use_mocap = false; + } /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - + //mavlink_log_info(mavlink_fd, "eph = %2.6f\t gps eph = %2.4f\n", (double)eph, (double)gps.eph); bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { @@ -1167,7 +1176,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); - //mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow); } if (use_gps_xy) { @@ -1265,6 +1273,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index be8a136c4d..f303abaf6d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); * Weight (cutoff frequency) for optical flow (velocity) measurements. * * @min 0.0 - * @max 30.0 + * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f); /** * XY axis weight for resetting velocity @@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); */ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); +/** + * Disable mocap (set 0 if using fake gps) + * + * Disable mocap + * + * @min 0 + * @max 1 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); + /** * Disable vision input * @@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->delay_gps = param_find("INAV_DELAY_GPS"); h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); + h->disable_mocap = param_find("INAV_DISAB_MOCAP"); return 0; } @@ -394,6 +406,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->delay_gps, &(p->delay_gps)); param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); + param_get(h->disable_mocap, &(p->disable_mocap)); return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 43601568f8..07b78104bf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -67,6 +67,7 @@ struct position_estimator_inav_params { float delay_gps; float flow_module_offset_x; float flow_module_offset_y; + int32_t disable_mocap; }; struct position_estimator_inav_param_handles { @@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles { param_t delay_gps; param_t flow_module_offset_x; param_t flow_module_offset_y; + param_t disable_mocap; }; #define CBRK_NO_VISION_KEY 328754 From 8044c5aaf660f174e878bce0116d599165ad9558 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:59:44 +0100 Subject: [PATCH 091/624] remove unnecessary lines --- .../position_estimator_inav/position_estimator_inav_main.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 32cb74ec8b..d8f29f1082 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -863,7 +863,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); - /* reset position estimate and ref when GPS becomes good */ + /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; @@ -877,8 +877,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) est_i += EST_BUF_SIZE; } - //mavlink_log_info(mavlink_fd, "est_i = %d\n", (int)est_i); - /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; @@ -982,7 +980,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - //mavlink_log_info(mavlink_fd, "eph = %2.6f\t gps eph = %2.4f\n", (double)eph, (double)gps.eph); + bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { From 20ec727d9fdbec46be75d21a838fef9e1c20cb88 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:22:00 +0200 Subject: [PATCH 092/624] added option for direct yaw control with rudder for fixed wing --- msg/vehicle_attitude_setpoint.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 7bbb670b31..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -21,3 +21,5 @@ float32 thrust # Thrust in Newton the power system should generate bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) From ee4249f30f72b11552d84fe98fe5c1d46a3624c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:22:39 +0200 Subject: [PATCH 093/624] extended ecl yaw controller for yaw tracking with rudder --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 +++++++++++++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 10 ++++-- 2 files changed, 41 insertions(+), 3 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 4849bdad79..240a426bd0 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,6 +67,9 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); + case COORD_METHOD_HEADING: + return control_heading(ctl_data); + default: static hrt_abstime last_print = 0; @@ -238,3 +241,34 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } + +float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 81d8210549..e3e529b30f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -76,14 +76,16 @@ public: _coordinated_method = coordinated_method; } -protected: - float _coordinated_min_speed; - enum { COORD_METHOD_OPEN = 0, COORD_METHOD_CLOSEACC = 1, + COORD_METHOD_HEADING = 2 }; +protected: + float _coordinated_min_speed; + float _max_rate; + int32_t _coordinated_method; float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); @@ -92,6 +94,8 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); + float control_heading(const struct ECL_ControlData &ctl_data); + }; #endif // ECL_YAW_CONTROLLER_H From f43d50fbc97a852dff084be29cf8580085f51cad Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:35:40 +0200 Subject: [PATCH 094/624] implemented runway takeoff for fw --- .../fw_att_control/fw_att_control_main.cpp | 8 + .../fw_pos_control_l1_main.cpp | 202 ++++++++++++------ .../fw_pos_control_l1_params.c | 9 + 3 files changed, 150 insertions(+), 69 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 3ed5b3a5a3..081645c3ae 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -811,6 +811,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; @@ -824,6 +825,7 @@ FixedwingAttitudeControl::task_main() /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -952,12 +954,18 @@ FixedwingAttitudeControl::task_main() control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; + control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + if (_att_sp.fw_control_yaw == true) { + // this method controls heading directly with rudder. Used for auto takeoff on runway + _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); + } + /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); 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 f21bfc5f2f..dbf26912ca 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 @@ -195,6 +195,8 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ + bool _takeoff_initialized; + hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ /* takeoff/launch states */ @@ -278,6 +280,8 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; + int do_runway_takeoff; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -326,6 +330,8 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t do_runway_takeoff; + } _parameter_handles; /**< handles for interesting parameters */ @@ -520,6 +526,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), + _takeoff_initialized(false), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -563,6 +571,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -666,6 +676,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -1054,6 +1066,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1338,94 +1352,144 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); - last_sent = hrt_absolute_time(); + if (_parameters.do_runway_takeoff == true) { + if (!_takeoff_initialized) { + _hdg_hold_yaw = _att.yaw; + _takeoff_runway_start = hrt_absolute_time(); + _takeoff_initialized = true; } - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - - /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); - } else { - /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } - - /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. */ - - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use - * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff - * meters */ - if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { + _att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value + // ramp up throttle to max throttle + _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max; + } else { + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, true, - math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::max(math::radians(_pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_TAKEOFF, takeoff_pitch_max_deg != _parameters.pitch_limit_max); - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); - - } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max); } - } else { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this + // start to navigate to takeoff waypoint as soon as we are high enough in the air + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } else { + // for takeoff we want heading control with rudder, roll and pitch stabilization to zero + // throttle should be ramped up to max + _att_sp.roll_body = 0.0f; + _att_sp.yaw_body = _hdg_hold_yaw; + _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + } + + + } else { + /* Perform launch detection */ + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the launch detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + + } else { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); + } + } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); + } } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..2865ec7ce6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,6 +426,15 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @group L1 Control + */ +PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); + /** * Flare, minimum pitch * From 059e40f78028cdf210f3dff2a2e0f7fcc86b2e8a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 01:57:09 +0200 Subject: [PATCH 095/624] - fixed throttle ramp-up - added parameter to specify which heading to keep on runway - validate terrain alt before using it --- .../fw_pos_control_l1_main.cpp | 76 +++++++++++++++---- .../fw_pos_control_l1_params.c | 9 +++ 2 files changed, 70 insertions(+), 15 deletions(-) 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 dbf26912ca..56da2f7c6e 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 @@ -196,6 +196,7 @@ private: bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ bool _takeoff_initialized; + bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -281,6 +282,7 @@ private: int land_use_terrain_estimate; int do_runway_takeoff; + int runway_takeoff_heading; } _parameters; /**< local copies of interesting parameters */ @@ -331,6 +333,7 @@ private: param_t land_use_terrain_estimate; param_t do_runway_takeoff; + param_t runway_takeoff_heading; } _parameter_handles; /**< handles for interesting parameters */ @@ -397,6 +400,11 @@ private: */ float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos); + /** * Check if we are in a takeoff situation */ @@ -527,7 +535,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_useterrain(false), _was_in_air(false), _takeoff_initialized(false), - _takeoff_runway_start(0), + _takeoff_on_runway(false), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -572,6 +581,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); + _parameter_handles.runway_takeoff_heading = param_find("FW_TKOFF_HDG"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -677,6 +687,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); + param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -970,6 +981,15 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +{ + if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { + return global_pos.terrain_alt; + } + + return takeoff_alt; +} + bool FixedwingPositionControl::update_desired_altitude(float dt) { /* @@ -1357,19 +1377,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_yaw = _att.yaw; _takeoff_runway_start = hrt_absolute_time(); _takeoff_initialized = true; + _takeoff_on_runway = true; + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _global_pos.alt; } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { - _att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value - // ramp up throttle to max throttle - _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max; + if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value + _takeoff_on_runway = true; + _att_sp.pitch_body = math::radians(10.0f); // XXX takeoff: magic value } else { - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + // min airspeed reached, let tecs control it + _takeoff_on_runway = false; + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), + calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, @@ -1384,20 +1408,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi takeoff_pitch_max_deg != _parameters.pitch_limit_max); _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max); } - if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this + // update navigation + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); + //PX4_WARN("talt: %f", (double)terrain_alt); + + if (_global_pos.alt - terrain_alt > 4.0f) { // XXX takeoff: magic value + // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) // start to navigate to takeoff waypoint as soon as we are high enough in the air - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); } else { // for takeoff we want heading control with rudder, roll and pitch stabilization to zero // throttle should be ramped up to max _att_sp.roll_body = 0.0f; - _att_sp.yaw_body = _hdg_hold_yaw; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + //_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + + if (_parameters.runway_takeoff_heading == 0) { + // fix heading in the direction the airframe points + _att_sp.yaw_body = _hdg_hold_yaw; + //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); + } else if (_parameters.runway_takeoff_heading == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing + //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); + } } @@ -1703,10 +1741,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_parameters.do_runway_takeoff) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _parameters.do_runway_takeoff && _takeoff_on_runway == true) { + /* Ramp-up thrust and stay at max */ + _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : _parameters.throttle_max; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; @@ -1889,6 +1933,8 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); + _takeoff_initialized = false; + _takeoff_on_runway = false; } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 2865ec7ce6..29fbeed386 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -435,6 +435,15 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0); */ PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @group L1 Control + */ +PARAM_DEFINE_INT32(FW_TKOFF_HDG, 0); + /** * Flare, minimum pitch * From ea884b34f0f13168fee6d36d0b61b0a027f6f8ef Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 02:06:37 +0200 Subject: [PATCH 096/624] use and reset the new yaw control method --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 ++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 3 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 081645c3ae..de8ee9173e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -964,6 +964,8 @@ FixedwingAttitudeControl::task_main() if (_att_sp.fw_control_yaw == true) { // this method controls heading directly with rudder. Used for auto takeoff on runway _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); + } else { + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); } /* Run attitude controllers */ 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 56da2f7c6e..c8832d830a 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 @@ -1424,7 +1424,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // for takeoff we want heading control with rudder, roll and pitch stabilization to zero // throttle should be ramped up to max _att_sp.roll_body = 0.0f; - //_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly if (_parameters.runway_takeoff_heading == 0) { // fix heading in the direction the airframe points From 24179a0d936c0ba3b93f4fc438f4a9546c42a096 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 10:29:14 +0200 Subject: [PATCH 097/624] consitent parameter naming for runway takeoff, added parameters for important values --- .../fw_pos_control_l1_main.cpp | 29 +++++++++---- .../fw_pos_control_l1_params.c | 41 ++++++++++++++++++- 2 files changed, 61 insertions(+), 9 deletions(-) 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 c8832d830a..6771921b2f 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 @@ -283,6 +283,9 @@ private: int do_runway_takeoff; int runway_takeoff_heading; + float runway_takeoff_nav_alt; + float runway_takeoff_throttle; + float runway_takeoff_pitch_sp; } _parameters; /**< local copies of interesting parameters */ @@ -334,6 +337,9 @@ private: param_t do_runway_takeoff; param_t runway_takeoff_heading; + param_t runway_takeoff_nav_alt; + param_t runway_takeoff_throttle; + param_t runway_takeoff_pitch_sp; } _parameter_handles; /**< handles for interesting parameters */ @@ -580,8 +586,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_TKOFF_HDG"); + _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); + _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); + _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); + _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); + _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -688,6 +697,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); + param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); + param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); + param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -1385,7 +1397,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(10.0f); // XXX takeoff: magic value + _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); } else { // min airspeed reached, let tecs control it _takeoff_on_runway = false; @@ -1397,11 +1409,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? _parameters.throttle_cruise, true, math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), + math::radians(10.0f)), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_TAKEOFF, @@ -1415,7 +1428,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > 4.0f) { // XXX takeoff: magic value + if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) // start to navigate to takeoff waypoint as soon as we are high enough in the air _att_sp.roll_body = _l1_control.nav_roll(); @@ -1750,7 +1763,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _parameters.do_runway_takeoff && _takeoff_on_runway == true) { /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : _parameters.throttle_max; + _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? + hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : + _parameters.runway_takeoff_throttle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 29fbeed386..5e05c6b9ef 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -431,18 +431,55 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0); * * 0: disabled, 1: enabled * + * @min 0 + * @max 1 * @group L1 Control */ -PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); +PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); /** * Specifies which heading should be held during runnway takeoff. * * 0: airframe heading, 1: heading towards takeoff waypoint * + * @min 0 + * @max 1 * @group L1 Control */ -PARAM_DEFINE_INT32(FW_TKOFF_HDG, 0); +PARAM_DEFINE_INT32(FW_RNW_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until FW_RNW_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see FW_RNW_HDG). + * + * @min 0.0 + * @max 100.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be limited to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); /** * Flare, minimum pitch From b93f34c99bb5d71bc4260541235b9e906d9b2796 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 10:42:35 +0200 Subject: [PATCH 098/624] use separate heading parameter beacause the other one gets reset in auto --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 6771921b2f..16ed16ad10 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 @@ -197,6 +197,7 @@ private: bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ bool _takeoff_initialized; bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ + float _runway_takeoff_hdg_hold; hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -542,7 +543,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _was_in_air(false), _takeoff_initialized(false), _takeoff_on_runway(false), - _takeoff_runway_start(0), + _runway_takeoff_hdg_hold(0), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -1386,7 +1388,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.do_runway_takeoff == true) { if (!_takeoff_initialized) { - _hdg_hold_yaw = _att.yaw; + _runway_takeoff_hdg_hold = _att.yaw; _takeoff_runway_start = hrt_absolute_time(); _takeoff_initialized = true; _takeoff_on_runway = true; @@ -1441,7 +1443,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.runway_takeoff_heading == 0) { // fix heading in the direction the airframe points - _att_sp.yaw_body = _hdg_hold_yaw; + _att_sp.yaw_body = _runway_takeoff_hdg_hold; //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); } else if (_parameters.runway_takeoff_heading == 1) { // or head into the direction of the takeoff waypoint From 1f8ebb71f51ed67aef6a14cc300d771fca789bb9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 14 Sep 2015 10:08:33 +0200 Subject: [PATCH 099/624] fix comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5e05c6b9ef..76b22f8350 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -461,7 +461,7 @@ PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); /** * Max throttle during runway takeoff. - * (Can be limited to test taxi on runway) + * (Can be used to test taxi on runway) * * @min 0.0 * @max 1.0 From 51ef8541739ffb864fd4797874df15fb5314fac0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 16 Sep 2015 01:00:44 +0200 Subject: [PATCH 100/624] extracted runway takeoff logic into external class --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 208 ++++++++++++++++++ src/lib/runway_takeoff/RunwayTakeoff.h | 115 ++++++++++ src/lib/runway_takeoff/module.mk | 41 ++++ .../runway_takeoff/runway_takeoff_params.c | 100 +++++++++ .../fw_pos_control_l1_main.cpp | 153 +++++-------- .../fw_pos_control_l1_params.c | 55 ----- 6 files changed, 520 insertions(+), 152 deletions(-) create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.cpp create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.h create mode 100644 src/lib/runway_takeoff/module.mk create mode 100644 src/lib/runway_takeoff/runway_takeoff_params.c diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000000..e1814cecc1 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _min_airspeed_scaling(1.3f), + _runway_takeoff_enabled(this, "TKOFF"), + _runway_takeoff_heading(this, "HDG"), + _runway_takeoff_nav_alt(this, "NAV_ALT"), + _runway_takeoff_throttle(this, "MAX_THR"), + _runway_takeoff_pitch_sp(this, "PSP"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +{ + if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { + _climbout = true; + } + + else { + _climbout = false; + } + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > 1e6) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + } + + break; + + default: + return; + } +} + +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw with rudder until we have enough ground clearance + return _state < RunwayTakeoffState::FLY; +} + +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_takeoff_pitch_sp.get()); + } + + return tecsPitch; +} + +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + if (_state < RunwayTakeoffState::FLY) { + return 0.0f; + } + + return navigatorRoll; +} + +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_state < RunwayTakeoffState::FLY) { + if (_runway_takeoff_heading.get() == 0) { + // fix heading in the direction the airframe points + return _init_yaw; + + } else if (_runway_takeoff_heading.get() == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + return navigatorYaw; + } + } + + return navigatorYaw; +} + +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + _runway_takeoff_throttle.get(); + return throttle < _runway_takeoff_throttle.get() ? + throttle : + _runway_takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _runway_takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + return _state <= RunwayTakeoffState::TAKEOFF; +} + +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_climbout) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000000..7a4dd17f50 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< */ + CLAMPED_TO_RUNWAY = 1, /**< */ + TAKEOFF = 2, /**< */ + FLY = 3 /**< */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw); + void update(float airspeed, float alt_agl, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinPitch(float sp_min, float climbout_min, float min); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + + /** magic values **/ + float _min_airspeed_scaling; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _runway_takeoff_heading; + control::BlockParamFloat _runway_takeoff_nav_alt; + control::BlockParamFloat _runway_takeoff_throttle; + control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/module.mk new file mode 100644 index 0000000000..95b9aea53e --- /dev/null +++ b/src/lib/runway_takeoff/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013, 2014 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. +# +############################################################################ + +# +# RunwayTakeoff Library +# + +SRCS = RunwayTakeoff.cpp \ + runway_takeoff_params.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000000..6ccc21e76a --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). If this is lower + * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); 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 16ed16ad10..34f414f0d6 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 @@ -95,6 +95,7 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #include +#include static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode @@ -195,12 +196,10 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ - bool _takeoff_initialized; - bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ - float _runway_takeoff_hdg_hold; - hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + runwaytakeoff::RunwayTakeoff _runway_takeoff; + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -282,12 +281,6 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; - int do_runway_takeoff; - int runway_takeoff_heading; - float runway_takeoff_nav_alt; - float runway_takeoff_throttle; - float runway_takeoff_pitch_sp; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -336,12 +329,6 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; - param_t do_runway_takeoff; - param_t runway_takeoff_heading; - param_t runway_takeoff_nav_alt; - param_t runway_takeoff_throttle; - param_t runway_takeoff_pitch_sp; - } _parameter_handles; /**< handles for interesting parameters */ @@ -541,11 +528,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), - _takeoff_initialized(false), - _takeoff_on_runway(false), - _runway_takeoff_hdg_hold(0), - _takeoff_runway_start(0), _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -588,12 +572,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); - _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); - _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); - _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -697,12 +675,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); - param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); - param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); - param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); - param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); - param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); - _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -752,6 +724,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -1386,72 +1360,54 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (_parameters.do_runway_takeoff == true) { - if (!_takeoff_initialized) { - _runway_takeoff_hdg_hold = _att.yaw; - _takeoff_runway_start = hrt_absolute_time(); - _takeoff_initialized = true; - _takeoff_on_runway = true; + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + _runway_takeoff.init(_att.yaw); + /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value - _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); - } else { - // min airspeed reached, let tecs control it - _takeoff_on_runway = false; - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, - true, - math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); - - _att_sp.pitch_body = _tecs.get_pitch_demand(); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); } // update navigation _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { - // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) - // start to navigate to takeoff waypoint as soon as we are high enough in the air - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - } else { - // for takeoff we want heading control with rudder, roll and pitch stabilization to zero - // throttle should be ramped up to max - _att_sp.roll_body = 0.0f; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + // update runway takeoff helper + _runway_takeoff.update( + _airspeed.true_airspeed_m_s, + _global_pos.alt - terrain_alt, + _mavlink_fd); - if (_parameters.runway_takeoff_heading == 0) { - // fix heading in the direction the airframe points - _att_sp.yaw_body = _runway_takeoff_hdg_hold; - //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); - } else if (_parameters.runway_takeoff_heading == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing - //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); - } - } + // update tecs + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); } else { @@ -1553,6 +1509,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ + // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1754,23 +1711,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && - !_parameters.do_runway_takeoff) { + !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - _parameters.do_runway_takeoff && _takeoff_on_runway == true) { - /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? - hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : - _parameters.runway_takeoff_throttle; + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && @@ -1950,8 +1910,7 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); - _takeoff_initialized = false; - _takeoff_on_runway = false; + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 76b22f8350..5870039c24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,61 +426,6 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); -/** - * Enable or disable runway takeoff with landing gear - * - * 0: disabled, 1: enabled - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); - -/** - * Specifies which heading should be held during runnway takeoff. - * - * 0: airframe heading, 1: heading towards takeoff waypoint - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_HDG, 0); - -/** - * Altitude AGL at which navigation towards takeoff waypoint starts. - * Until FW_RNW_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see FW_RNW_HDG). - * - * @min 0.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); - -/** - * Max throttle during runway takeoff. - * (Can be used to test taxi on runway) - * - * @min 0.0 - * @max 1.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); - -/** - * Pitch setpoint during runway takeoff. - * A taildragger with stearable wheel might need to pitch up - * a little to keep it's wheel on the ground before airspeed - * to takeoff is reached. - * - * @min 0.0 - * @max 20.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); - /** * Flare, minimum pitch * From 6c31421889f5bc4fe1b389a72713cd12274774fe Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 23 Sep 2015 12:30:38 +0200 Subject: [PATCH 101/624] extracted heading controller --- .../attitude_fw/ecl_heading_controller.cpp | 163 ++++++++++++++++++ .../ecl/attitude_fw/ecl_heading_controller.h | 70 ++++++++ .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 ---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 4 +- .../fw_att_control/fw_att_control_main.cpp | 26 ++- .../fw_pos_control_l1_main.cpp | 19 +- 7 files changed, 265 insertions(+), 56 deletions(-) create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.h diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp new file mode 100644 index 0000000000..97dfa2f617 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_heading_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_HeadingController::ECL_HeadingController() : + ECL_Controller("heading") +{ +} + +ECL_HeadingController::~ECL_HeadingController() +{ +} + +float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float airspeed = ctl_data.airspeed; + + if (!PX4_ISFINITE(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; + } + + + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + + float id = _rate_error * dt; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * + ctl_data.scaler; //scaler is proportional to 1/airspeed + warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + + +float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_heading_controller.h new file mode 100644 index 0000000000..5dd79cd7d4 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_HeadingController : + public ECL_Controller +{ +public: + ECL_HeadingController(); + + ~ECL_HeadingController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 240a426bd0..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,9 +67,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); - case COORD_METHOD_HEADING: - return control_heading(ctl_data); - default: static hrt_abstime last_print = 0; @@ -241,34 +238,3 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } - -float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = yaw_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - } - - return _rate_setpoint; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index e3e529b30f..511b5fd36f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -78,8 +78,7 @@ public: enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, - COORD_METHOD_HEADING = 2 + COORD_METHOD_CLOSEACC = 1 }; protected: @@ -94,8 +93,6 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); - float control_heading(const struct ECL_ControlData &ctl_data); - }; #endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index e1814cecc1..d7c33d7094 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -105,7 +105,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { _state = RunwayTakeoffState::TAKEOFF; - mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } break; @@ -113,7 +113,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { _state = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } break; 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 de8ee9173e..938347ff95 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include /** @@ -248,6 +249,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_HeadingController _heading_ctrl; /** @@ -502,6 +504,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* heading control parameters */ + _heading_ctrl.set_k_p(_parameters.y_p); + _heading_ctrl.set_k_i(_parameters.y_i); + _heading_ctrl.set_k_ff(_parameters.y_ff); + _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); + _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + return OK; } @@ -961,18 +970,14 @@ FixedwingAttitudeControl::task_main() control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - if (_att_sp.fw_control_yaw == true) { - // this method controls heading directly with rudder. Used for auto takeoff on runway - _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); - } else { - _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); - } + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _heading_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1012,7 +1017,14 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { + yaw_u = _heading_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ 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 34f414f0d6..7f7d0b15b3 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 @@ -960,7 +960,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint * for the whole landing */ if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } return global_pos.terrain_alt; @@ -1229,7 +1229,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); @@ -1290,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } } @@ -1314,7 +1314,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -1336,7 +1336,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } } else { @@ -1367,7 +1367,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } // update navigation @@ -1417,7 +1417,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1509,7 +1509,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1750,7 +1749,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled()) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 234a200e6081bc19c0489a5bd266ed4d00cee917 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 15:57:51 +0200 Subject: [PATCH 102/624] renamed heading controller to wheel controller, added groundspeed dependency and separate parameters --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/ecl/attitude_fw/ecl_controller.h | 1 + ...ontroller.cpp => ecl_wheel_controller.cpp} | 55 +++++++---------- ...ng_controller.h => ecl_wheel_controller.h} | 8 +-- src/lib/runway_takeoff/RunwayTakeoff.cpp | 7 ++- src/lib/runway_takeoff/RunwayTakeoff.h | 2 +- .../fw_att_control/fw_att_control_main.cpp | 51 ++++++++++++---- .../fw_att_control/fw_att_control_params.c | 60 +++++++++++++++++++ .../fw_pos_control_l1_main.cpp | 7 ++- 9 files changed, 138 insertions(+), 55 deletions(-) rename src/lib/ecl/attitude_fw/{ecl_heading_controller.cpp => ecl_wheel_controller.cpp} (67%) rename src/lib/ecl/attitude_fw/{ecl_heading_controller.h => ecl_wheel_controller.h} (95%) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 3bbb5efa7b..bcfe0f41da 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) +bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index ac1ac88d04..0817204ccc 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,6 +75,7 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; + float ground_speed; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp similarity index 67% rename from src/lib/ecl/attitude_fw/ecl_heading_controller.cpp rename to src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 97dfa2f617..b0df2a95d6 100644 --- a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -32,13 +32,13 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.cpp + * @file ecl_wheel_controller.cpp * Implementation of a simple orthogonal coordinated turn yaw PID controller. * * Authors and acknowledgements in header. */ -#include "ecl_heading_controller.h" +#include "ecl_wheel_controller.h" #include #include #include @@ -47,22 +47,20 @@ #include #include -ECL_HeadingController::ECL_HeadingController() : - ECL_Controller("heading") +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") { } -ECL_HeadingController::~ECL_HeadingController() +ECL_WheelController::~ECL_WheelController() { } -float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data) +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.ground_speed))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -80,27 +78,21 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_ } /* input conditioning */ - float airspeed = ctl_data.airspeed; - - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); - - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; + float min_speed = 1.0f; + /* assume minimum speed to prevent oscillations */ + float speed = min_speed; + if (ctl_data.ground_speed > speed) { + speed = ctl_data.ground_speed; } - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + float scaler = 1.0f / speed; /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -122,23 +114,22 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_ float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * - ctl_data.scaler; //scaler is proportional to 1/airspeed - warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", + (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ return math::constrain(_last_output, -1.0f, 1.0f); } -float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { + PX4_ISFINITE(ctl_data.yaw))) { perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); + warnx("not controlling wheel"); return _rate_setpoint; } diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h similarity index 95% rename from src/lib/ecl/attitude_fw/ecl_heading_controller.h rename to src/lib/ecl/attitude_fw/ecl_wheel_controller.h index 5dd79cd7d4..f153a8f8f1 100644 --- a/src/lib/ecl/attitude_fw/ecl_heading_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.h + * @file ecl_wheel_controller.h * Definition of a simple orthogonal coordinated turn yaw PID controller. * * @author Lorenz Meier @@ -54,13 +54,13 @@ #include "ecl_controller.h" -class __EXPORT ECL_HeadingController : +class __EXPORT ECL_WheelController : public ECL_Controller { public: - ECL_HeadingController(); + ECL_WheelController(); - ~ECL_HeadingController(); + ~ECL_WheelController(); float control_attitude(const struct ECL_ControlData &ctl_data); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index d7c33d7094..5067260d32 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -123,10 +123,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlYaw() +bool RunwayTakeoff::controlWheel() { - // keep controlling yaw with rudder until we have enough ground clearance - return _state < RunwayTakeoffState::FLY; + // keep controlling wheel until takeoff + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -140,6 +140,7 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { + // until we have enough ground clearance, set roll to 0 if (_state < RunwayTakeoffState::FLY) { return 0.0f; } diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 7a4dd17f50..2f84afee2b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -75,7 +75,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlYaw(); + bool controlWheel(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); 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 938347ff95..144a8ed048 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,7 +82,7 @@ #include #include #include -#include +#include #include /** @@ -182,6 +182,11 @@ private: float y_coordinated_min_speed; int32_t y_coordinated_method; float y_rmax; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_rmax; float airspeed_min; float airspeed_trim; @@ -223,6 +228,11 @@ private: param_t y_coordinated_min_speed; param_t y_coordinated_method; param_t y_rmax; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -249,7 +259,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; - ECL_HeadingController _heading_ctrl; + ECL_WheelController _wheel_ctrl; /** @@ -382,6 +392,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); + _parameter_handles.w_p = param_find("FW_WR_P"); + _parameter_handles.w_i = param_find("FW_WR_I"); + _parameter_handles.w_ff = param_find("FW_WR_FF"); + _parameter_handles.w_integrator_max = param_find("FW_WR_IMAX"); + _parameter_handles.w_rmax = param_find("FW_W_RMAX"); + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); @@ -460,6 +476,12 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); + param_get(_parameter_handles.w_i, &(_parameters.w_i)); + param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); + param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); + param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); @@ -504,12 +526,12 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); - /* heading control parameters */ - _heading_ctrl.set_k_p(_parameters.y_p); - _heading_ctrl.set_k_i(_parameters.y_i); - _heading_ctrl.set_k_ff(_parameters.y_ff); - _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); - _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_parameters.w_p); + _wheel_ctrl.set_k_i(_parameters.w_i); + _wheel_ctrl.set_k_ff(_parameters.w_ff); + _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); + _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); return OK; } @@ -846,6 +868,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { @@ -871,6 +894,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { @@ -890,6 +914,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else { /* @@ -940,6 +965,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ @@ -969,6 +995,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); @@ -977,7 +1005,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude - _heading_ctrl.control_attitude(control_input); + _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1018,8 +1046,8 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_yaw == true) { - yaw_u = _heading_ctrl.control_bodyrate(control_input); + if (_att_sp.fw_control_wheel == true) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); } else { @@ -1031,6 +1059,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] += yaw_manual; if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 51c06fabb9..d5fdcd5e01 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @min 0.005 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); + /** * Roll rate feed forward * @@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + /** * Minimal speed for yaw coordination * 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 7f7d0b15b3..08deeb60a9 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 @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1406,9 +1406,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); - _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.fw_control_wheel = _runway_takeoff.controlWheel(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); - + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, + (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ } else { /* Perform launch detection */ From 0c875dd6d1091f6f49cea6dbc0a0b9513a378566 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 20:56:42 +0200 Subject: [PATCH 103/624] calculate shortest yaw error --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index b0df2a95d6..12d2668f87 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -135,6 +135,9 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da /* Calculate the error */ float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /*warnx("yaw_error: %.4f", (double)yaw_error);*/ /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = yaw_error / _tc; From e987082292b82e2dce5065dc343867521741f47c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 21:53:10 +0200 Subject: [PATCH 104/624] split takeoff into 2 phases, reseting integrators when still on runway --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 29 +++++++++++++++---- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++++---- .../runway_takeoff/runway_takeoff_params.c | 6 ++-- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1_main.cpp | 10 +++++-- 6 files changed, 43 insertions(+), 18 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index bcfe0f41da..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 5067260d32..77d2047882 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -59,6 +59,7 @@ RunwayTakeoff::RunwayTakeoff() : _init_yaw(0), _climbout(false), _min_airspeed_scaling(1.3f), + _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), _runway_takeoff_heading(this, "HDG"), _runway_takeoff_nav_alt(this, "NAV_ALT"), @@ -111,7 +112,15 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + if (alt_agl > _runway_takeoff_nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -123,10 +132,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlWheel() +bool RunwayTakeoff::controlYaw() { - // keep controlling wheel until takeoff - return _state < RunwayTakeoffState::TAKEOFF; + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -141,10 +150,17 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 - if (_state < RunwayTakeoffState::FLY) { + if (_state < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } + return navigatorRoll; } @@ -186,7 +202,8 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) bool RunwayTakeoff::resetIntegrators() { - return _state <= RunwayTakeoffState::TAKEOFF; + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2f84afee2b..265ff24cb3 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -55,10 +55,11 @@ namespace runwaytakeoff { enum RunwayTakeoffState { - THROTTLE_RAMP = 0, /**< */ - CLAMPED_TO_RUNWAY = 1, /**< */ - TAKEOFF = 2, /**< */ - FLY = 3 /**< */ + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ }; class __EXPORT RunwayTakeoff : public control::SuperBlock @@ -75,7 +76,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlWheel(); + bool controlYaw(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); @@ -98,6 +99,7 @@ private: /** magic values **/ float _min_airspeed_scaling; + float _max_takeoff_roll; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 6ccc21e76a..d3ce5d238d 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -66,10 +66,10 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); PARAM_DEFINE_INT32(RWTO_HDG, 0); /** - * Altitude AGL at which navigation towards takeoff waypoint starts. + * Altitude AGL at which we have enough ground clearance to allow some roll. * Until RWTO_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see RWTO_HDG). If this is lower - * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * * @min 0.0 * @max 100.0 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 144a8ed048..da91fda9f7 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1046,7 +1046,7 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_wheel == true) { + if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } 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 08deeb60a9..ed71cf2955 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 @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1371,6 +1371,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } // update navigation + // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); @@ -1406,8 +1407,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); - _att_sp.fw_control_wheel = _runway_takeoff.controlWheel(); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ From 178ec7f4fc41fb136592212d1b645c822a0c1bad Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 11:05:56 +0200 Subject: [PATCH 105/624] stay out of climbout once height has been reached, don't mix navigator roll with fixed heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 77d2047882..599cb721d0 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -87,13 +87,6 @@ void RunwayTakeoff::init(float yaw) void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) { - if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { - _climbout = true; - } - - else { - _climbout = false; - } switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: @@ -105,6 +98,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -121,6 +115,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLIMBOUT: if (alt_agl > _climbout_diff.get()) { + _climbout = false; _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -154,11 +149,17 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout + // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + if (_runway_takeoff_heading.get() == 0) { + // otherwise stay at 0 roll + return 0.0f; + + } else if (_runway_takeoff_heading.get() == 1) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } } return navigatorRoll; From 0769ec53454de195ed804c8c3b01d4d210b1d4c5 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 14:30:01 +0200 Subject: [PATCH 106/624] added max pitch parameter for climbout phase --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 14 +++++++++++++- src/lib/runway_takeoff/RunwayTakeoff.h | 2 ++ src/lib/runway_takeoff/runway_takeoff_params.c | 13 ++++++++++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 599cb721d0..7b2ab97e8e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -65,6 +65,7 @@ RunwayTakeoff::RunwayTakeoff() : _runway_takeoff_nav_alt(this, "NAV_ALT"), _runway_takeoff_throttle(this, "MAX_THR"), _runway_takeoff_pitch_sp(this, "PSP"), + _runway_takeoff_max_pitch(this, "MAX_PITCH"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -83,6 +84,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); + _climbout = true; } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -98,7 +100,6 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { - _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -218,6 +219,17 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +float RunwayTakeoff::getMaxPitch(float max) +{ + if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { + return _runway_takeoff_max_pitch.get(); + } + + else { + return max; + } +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 265ff24cb3..86b8c01b6e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -85,6 +85,7 @@ public: bool resetIntegrators(); float getMinAirspeedScaling() { return _min_airspeed_scaling; }; float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); void reset(); @@ -107,6 +108,7 @@ private: control::BlockParamFloat _runway_takeoff_nav_alt; control::BlockParamFloat _runway_takeoff_throttle; control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _runway_takeoff_max_pitch; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index d3ce5d238d..137732c074 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); /** - * Pitch setpoint during runway takeoff. + * Pitch setpoint during taxi / before takeoff airspeed is reached. * A taildragger with stearable wheel might need to pitch up * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. @@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); 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 ed71cf2955..711e888416 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 @@ -1382,7 +1382,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mavlink_fd); // update tecs - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, From 3eb0ce84df03c8abfbc1634a3183e4bc5eb6c5c3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 15:42:43 +0200 Subject: [PATCH 107/624] set disarmed/min/max pwm for throttle channel in default fw configs --- ROMFS/px4fmu_common/init.d/2101_fw_AERT | 4 +++- ROMFS/px4fmu_common/init.d/2104_fw_AETR | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 3631a1e36c..c5bb1477c5 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AERT -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 4 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index 3113ede87a..f533ce66fe 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AETR -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 3 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX From ff57c809b87c1f0e234071c442e00f2ebae6d13a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 17:45:09 +0200 Subject: [PATCH 108/624] updated default wheel params after first test --- src/modules/fw_att_control/fw_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d5fdcd5e01..f4cfa7fd8d 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); /** * Wheel steering rate integrator gain @@ -242,7 +242,7 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); * @max 50.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); /** * Wheel steering rate integrator limit From d015fbd6782e942bb8f0a3afe0aa7772093ed021 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 2 Oct 2015 11:50:18 +0200 Subject: [PATCH 109/624] added startup config for Maja and new generic mixer that uses channel 5 for wheel steering --- ROMFS/px4fmu_common/init.d/2101_fw_AERT | 4 +- ROMFS/px4fmu_common/init.d/2105_maja | 48 ++++++++++ ROMFS/px4fmu_common/mixers/AERTW.main.mix | 95 +++++++++++++++++++ .../fw_pos_control_l1_main.cpp | 1 + 4 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/2105_maja create mode 100644 ROMFS/px4fmu_common/mixers/AERTW.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index c5bb1477c5..d13514d03a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -6,8 +6,8 @@ # # @output MAIN1 aileron # @output MAIN2 elevator -# @output MAIN4 rudder -# @output MAIN3 throttle +# @output MAIN3 rudder +# @output MAIN4 throttle # @output MAIN5 flaps # # @output AUX1 feed-through of RC AUX1 channel diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja new file mode 100644 index 0000000000..af27eca5d9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -0,0 +1,48 @@ +#!nsh +# +# @name Bormatec Maja +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN3 rudder +# @output MAIN4 throttle +# @output MAIN5 wheel +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AERTW + +# use PWM parameters for throttle channel +set PWM_OUT 4 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix new file mode 100644 index 0000000000..af548826dc --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -0,0 +1,95 @@ +Aileron/rudder/elevator/throttle/wheel mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. +The configuration assumes the aileron servo(s) are connected to PX4FMU servo +output 0, the elevator to output 1, the rudder to output 2, the throttle +to output 3 and the wheel to output 4. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + + +Gimbal / flaps / payload mixer for last three channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 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 711e888416..b9f24a6eb2 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 @@ -49,6 +49,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Andreas Antener */ #include From 9c70eb0b63e1079f1fd0fcf8714cbd2fd138baed Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 2 Oct 2015 16:25:14 +0200 Subject: [PATCH 110/624] reduce wheel control speed scaling --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 12d2668f87..d67a46bb1c 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -85,7 +85,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da speed = ctl_data.ground_speed; } - float scaler = 1.0f / speed; + /* only scale a certain amount with speed else the corrections get to small */ + float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error From f5f61e42af0530142d41a8186f9fa71e6b87be6a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 7 Oct 2015 10:12:35 +0200 Subject: [PATCH 111/624] replaced magic values with parameters, renamed internal representations --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 44 +++++++++---------- src/lib/runway_takeoff/RunwayTakeoff.h | 18 ++++---- .../runway_takeoff/runway_takeoff_params.c | 33 ++++++++++++++ 3 files changed, 63 insertions(+), 32 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 7b2ab97e8e..c04ad780f2 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,14 +58,14 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _min_airspeed_scaling(1.3f), - _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), - _runway_takeoff_heading(this, "HDG"), - _runway_takeoff_nav_alt(this, "NAV_ALT"), - _runway_takeoff_throttle(this, "MAX_THR"), - _runway_takeoff_pitch_sp(this, "PSP"), - _runway_takeoff_max_pitch(this, "MAX_PITCH"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -99,7 +99,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -107,7 +107,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > _runway_takeoff_nav_alt.get()) { + if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -137,7 +137,7 @@ bool RunwayTakeoff::controlYaw() float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { - return math::radians(_runway_takeoff_pitch_sp.get()); + return math::radians(_runway_pitch_sp.get()); } return tecsPitch; @@ -152,14 +152,14 @@ float RunwayTakeoff::getRoll(float navigatorRoll) // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // otherwise stay at 0 roll return 0.0f; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll).get()); } } @@ -169,11 +169,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // fix heading in the direction the airframe points return _init_yaw; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { // or head into the direction of the takeoff waypoint // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) return navigatorYaw; @@ -188,14 +188,14 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * - _runway_takeoff_throttle.get(); - return throttle < _runway_takeoff_throttle.get() ? + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? throttle : - _runway_takeoff_throttle.get(); + _takeoff_throttle.get(); } case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - return _runway_takeoff_throttle.get(); + return _takeoff_throttle.get(); default: return tecsThrottle; @@ -221,8 +221,8 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { - return _runway_takeoff_max_pitch.get(); + if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); } else { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 86b8c01b6e..8e3cd35528 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -83,7 +83,7 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); @@ -98,17 +98,15 @@ private: float _init_yaw; bool _climbout; - /** magic values **/ - float _min_airspeed_scaling; - float _max_takeoff_roll; - /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; - control::BlockParamInt _runway_takeoff_heading; - control::BlockParamFloat _runway_takeoff_nav_alt; - control::BlockParamFloat _runway_takeoff_throttle; - control::BlockParamFloat _runway_takeoff_pitch_sp; - control::BlockParamFloat _runway_takeoff_max_pitch; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 137732c074..05110dddc2 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -109,3 +109,36 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); From e0cdf65fb446bc40ff032ea88b70ebd3f6a6d1ab Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 8 Oct 2015 22:23:18 +0200 Subject: [PATCH 112/624] use navigator to hold heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 53 +++++++++++-------- src/lib/runway_takeoff/RunwayTakeoff.h | 10 +++- .../runway_takeoff/runway_takeoff_params.c | 11 ---- .../fw_pos_control_l1_main.cpp | 26 +++++++-- 4 files changed, 63 insertions(+), 37 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index c04ad780f2..16cbb04e45 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,6 +47,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -58,6 +59,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), + _start_sp{}, + _target_sp{}, _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -150,17 +153,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout if waypoint heading is targeted + // allow some roll during climbout else if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // otherwise stay at 0 roll - return 0.0f; - - } else if (_heading_mode.get() == 1) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll.get()), - math::radians(_max_takeoff_roll).get()); - } + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); } return navigatorRoll; @@ -168,18 +165,6 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { - if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // fix heading in the direction the airframe points - return _init_yaw; - - } else if (_heading_mode.get() == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - return navigatorYaw; - } - } - return navigatorYaw; } @@ -230,6 +215,30 @@ float RunwayTakeoff::getMaxPitch(float max) } } +math::Vector<2> RunwayTakeoff::getPrevWP() +{ + math::Vector<2> prev_wp; + prev_wp(0) = (float)_start_sp.lat; + prev_wp(1) = (float)_start_sp.lon; + return prev_wp; +} + +math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { + // navigating towards calculated direction if heading mode 0 and as long as we're in climbout + math::Vector<2> curr_wp; + curr_wp(0) = (float)_target_sp.lat; + curr_wp(1) = (float)_target_sp.lon; + return curr_wp; + + } else { + // navigating towards next mission waypoint + return sp_curr_wp; + } + +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 8e3cd35528..2913b2ad64 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,6 +50,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -75,6 +76,10 @@ public: bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + position_setpoint_s *getStartSP() { return &_start_sp; }; + position_setpoint_s *getTargetSP() { return &_target_sp; }; + float getInitYaw() { return _init_yaw; }; bool controlYaw(); bool climbout() { return _climbout; }; @@ -83,9 +88,10 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); + math::Vector<2> getPrevWP(); + math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); void reset(); @@ -97,6 +103,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; + struct position_setpoint_s _start_sp; + struct position_setpoint_s _target_sp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 05110dddc2..7c64889e76 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -110,17 +110,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); -/** - * Max pitch during takeoff. - * Fixed-wing settings are used if set to 0. Note that there is also a minimum - * pitch of 10 degrees during takeoff, so this must be larger if set. - * - * @min 0.0 - * @max 60.0 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); - /** * Max roll during climbout. * Roll is limited during climbout to ensure enough lift and prevents aggressive 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 b9f24a6eb2..176d4ed11b 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 @@ -1368,12 +1368,32 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; + + // draw a line from takeoff location into the direction the UAV is heading + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + true); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // update navigation - // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + // update takeoff path if we're reaching the end of it + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + false); + } + + /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet + * or the calculated one through initial heading */ + _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper From 3b865624f1b302c256bf628de9013043d2df0a18 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 18 Sep 2015 08:42:08 +0200 Subject: [PATCH 113/624] added library for terrain_estimation Conflicts: makefiles/nuttx/config_aerocore_default.mk makefiles/nuttx/config_px4fmu-v1_default.mk makefiles/nuttx/config_px4fmu-v2_default.mk makefiles/nuttx/config_px4fmu-v2_multiplatform.mk makefiles/posix/config_posix_sitl.mk --- src/lib/terrain_estimation/module.mk | 36 ++++ .../terrain_estimation/terrain_estimator.cpp | 188 ++++++++++++++++++ .../terrain_estimation/terrain_estimator.h | 96 +++++++++ .../AttitudePositionEstimatorEKF.h | 3 + .../ekf_att_pos_estimator_main.cpp | 16 +- 5 files changed, 336 insertions(+), 3 deletions(-) create mode 100644 src/lib/terrain_estimation/module.mk create mode 100644 src/lib/terrain_estimation/terrain_estimator.cpp create mode 100644 src/lib/terrain_estimation/terrain_estimator.h diff --git a/src/lib/terrain_estimation/module.mk b/src/lib/terrain_estimation/module.mk new file mode 100644 index 0000000000..3a2047012a --- /dev/null +++ b/src/lib/terrain_estimation/module.mk @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 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 +# 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. +# +############################################################################ + +SRCS = terrain_estimator.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp new file mode 100644 index 0000000000..f522e42047 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp + * A terrain estimation kalman filter. + */ + +#include "terrain_estimator.h" + +TerrainEstimator::TerrainEstimator() : + _distance_last(0.0f), + _terrain_valid(false), + _time_last_distance(0), + _time_last_gps(0) +{ + _x.zero(); + _u_z = 0.0f; + _P.identity(); +} + +bool TerrainEstimator::is_distance_valid(float distance) { + if (distance > 40.0f || distance < 0.00001f) { + return false; + } else { + return true; + } +} + +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) +{ + if (attitude->R_valid) { + math::Matrix<3, 3> R_att(attitude->R); + math::Vector<3> a(&sensor->accelerometer_m_s2[0]); + math::Vector<3> u; + u = R_att * a; + _u_z = u(2) + 9.81f; // compensate for gravity + + } else { + _u_z = 0.0f; + } + + // dynamics matrix + math::Matrix A; + A.zero(); + A(0,1) = 1; + A(1,2) = 1; + + // input matrix + math::Matrix B; + B.zero(); + B(1,0) = 1; + + // input noise variance + float R = 0.135f; + + // process noise convariance + math::Matrix Q; + Q(0,0) = 0; + Q(1,1) = 0; + + // do prediction + math::Vector dx = (A * _x) * dt; + dx(1) += B(1,0) * _u_z * dt; + + // propagate state and covariance matrix + _x += dx; + _P += (A * _P + _P * A.transposed() + + B * R * B.transposed() + Q) * dt; +} + +void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) +{ + if (distance->timestamp > _time_last_distance) { + + float d = distance->current_distance; + + math::Matrix<1, n_x> C; + C(0, 0) = -1; // measured altitude, + + float R = 0.009f; + + math::Vector<1> y; + y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + + // residual + math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + S_I(0,0) += R; + S_I = S_I.inversed(); + math::Vector<1> r = y - C * _x; + + math::Matrix K = _P * C.transposed() * S_I; + + // some sort of outlayer rejection + if (fabsf(distance->current_distance - _distance_last) < 1.0f) { + _x += K * r; + _P -= K * C * _P; + } + + // if the current and the last range measurement are bad then we consider the terrain + // estimate to be invalid + if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { + _terrain_valid = false; + } else { + _terrain_valid = true; + } + + _time_last_distance = distance->timestamp; + _distance_last = distance->current_distance; + } + + if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + math::Matrix<1, n_x> C; + C(0,1) = 1; + + float R = 0.056f; + + math::Vector<1> y; + y(0) = gps->vel_d_m_s; + + // residual + math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + S_I(0,0) += R; + S_I = S_I.inversed(); + math::Vector<1> r = y - C * _x; + + math::Matrix K = _P * C.transposed() * S_I; + _x += K * r; + _P -= K * C * _P; + + _time_last_gps = gps->timestamp_position; + } + + // reinitialise filter if we find bad data + bool reinit = false; + for (int i = 0; i < n_x; i++) { + if (!PX4_ISFINITE(_x(i))) { + reinit = true; + } + } + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!PX4_ISFINITE(_P(i,j))) { + reinit = true; + } + } + } + + if (reinit) { + _x.zero(); + _P.zero(); + _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + } + +} diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h new file mode 100644 index 0000000000..981e621236 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.h + */ + +#include +#include +#include +#include +#include + + +/* +* This class can be used to estimate distance to the ground using a laser range finder. +* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose. +* The predict(...) function will do a state prediciton based on accelerometer inputs. It also +* considers accelerometer bias. +* The measurement_update(...) function does a measurement update based on range finder and gps +* velocity measurements. Both functions should always be called together when there is new +* acceleration data available. +* The is_valid() function provides information wether the estimate is valid. +*/ + +class __EXPORT TerrainEstimator +{ +public: + TerrainEstimator(); + ~TerrainEstimator(); + + bool is_valid() {return _terrain_valid;} + float get_distance_to_ground() {return -_x(0);} + float get_velocity() {return _x(1);}; + + void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance); + void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude); + +private: + enum {n_x=3}; + + float _distance_last; + bool _terrain_valid; + + // kalman filter variables + math::Vector _x; // state: ground distance, velocity, accel bias in z direction + float _u_z; // acceleration in earth z direction + math::Matrix<3, 3> _P; // covariance matrix + + // timestamps + uint64_t _time_last_distance; + uint64_t _time_last_gps; + + struct { + float var_acc_z; + float var_p_z; + float var_p_vz; + float var_lidar; + float var_gps_vz; + } _params; + + bool is_distance_valid(float distance); + +}; \ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 1429979d6d..fc11e0f505 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -69,6 +69,7 @@ #include #include +#include #include #include #include "estimator_22states.h" @@ -278,6 +279,8 @@ private: AttPosEKF *_ekf; + TerrainEstimator *_terrain_estimator; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _LP_att_P; math::LowPassFilter2p _LP_att_Q; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f4fcf78e25..2193b3e945 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameters{}, _parameter_handles{}, _ekf(nullptr), + _terrain_estimator(nullptr), _LP_att_P(250.0f, 20.0f), _LP_att_Q(250.0f, 20.0f), @@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : { _voter_mag.set_timeout(200000); + _terrain_estimator = new TerrainEstimator(); + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); @@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main() // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + // Run separate terrain estimator + _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); + _terrain_estimator->measurement_update(&_gps, &_distance, &_att); + // Publish attitude estimations publishAttitude(); @@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_terrain_estimator->is_valid()) { + _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); + _global_pos.terrain_alt_valid = true; + } else { + _global_pos.terrain_alt_valid = false; + } _global_pos.yaw = _local_pos.yaw; From b16e6249e4189e077ce9d82973897a7698842181 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 11 Oct 2015 16:52:09 +0200 Subject: [PATCH 114/624] more correct groundspeed scaling for wheel controller --- src/lib/ecl/attitude_fw/ecl_controller.h | 3 ++- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 19 +++++++------------ .../fw_att_control/fw_att_control_main.cpp | 13 ++++++++++--- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index 0817204ccc..fed7d1d8fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,7 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; - float ground_speed; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index d67a46bb1c..76419ef6f7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -60,7 +60,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.ground_speed))) { + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -79,21 +80,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float min_speed = 1.0f; - /* assume minimum speed to prevent oscillations */ - float speed = min_speed; - if (ctl_data.ground_speed > speed) { - speed = ctl_data.ground_speed; - } - - /* only scale a certain amount with speed else the corrections get to small */ - float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { - float id = _rate_error * dt * scaler; + float id = _rate_error * dt * ctl_data.groundspeed_scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -115,7 +108,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ 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 da91fda9f7..271dc86079 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -837,9 +837,16 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_sp = 0.0f; @@ -995,8 +1002,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); From 62122201138e369459fada1c2aa957a5465c142b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:46:00 +0200 Subject: [PATCH 115/624] replaced aileron mixer with flaperon mixer --- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index af548826dc..a28dd9ec6e 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -10,22 +10,23 @@ to output 3 and the wheel to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust). -Aileron mixer -------------- -Two scalers total (output, roll). +Flaperon mixer (ailerons + flaps) +--------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; depending on the actual configuration it may be necessary to reverse the scaling factors (to reverse the servo movement) and adjust the offset, scaling and endpoints to suit. -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 Elevator mixer ------------ From 0043c40b463c1e05703fcdadcbd5aba6cc535af6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:46:51 +0200 Subject: [PATCH 116/624] added more indexing variables --- msg/actuator_controls.msg | 3 +++ 1 file changed, 3 insertions(+) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 66e12325d2..a6ebda6aae 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 uint8 INDEX_THROTTLE = 3 uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled From 18d9c061ba5b78b2c96ac78bd07b1e0ec5cfa06c Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:47:26 +0200 Subject: [PATCH 117/624] added flag for applying flaps --- msg/vehicle_attitude_setpoint.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 3bbb5efa7b..43ea237b47 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -23,3 +23,5 @@ bool pitch_reset_integral # Reset pitch integral part (navigation logic change bool yaw_reset_integral # Reset yaw integral part (navigation logic change) bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) + +bool apply_flaps From 7fc97ed147222668d996815e0feab887f9abf7ed Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:47:50 +0200 Subject: [PATCH 118/624] implemented use of flaps for auto landings --- .../fw_att_control/fw_att_control_main.cpp | 30 +++++++++++++++++-- .../fw_att_control/fw_att_control_params.c | 18 +++++++++++ .../fw_pos_control_l1_main.cpp | 6 +++- 3 files changed, 50 insertions(+), 4 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 271dc86079..579096268e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -202,6 +202,9 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -246,6 +249,9 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t flaps_scale; + param_t flaperon_scale; + param_t vtol_type; } _parameter_handles; /**< handles for interesting parameters */ @@ -414,6 +420,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERONS_SCL"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ @@ -498,6 +507,9 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); + param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ @@ -810,8 +822,20 @@ FixedwingAttitudeControl::task_main() float flaps_control = 0.0f; /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual.flaps)) { - flaps_control = _manual.flaps; + if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flaps_control = _manual.flaps * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; + } + + /* default flaperon to center */ + float flaperon = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { + flaperon = _manual.aux2 * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } /* decide if in stabilized or full manual control */ @@ -1119,7 +1143,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; - _actuators.control[6] = _manual.aux2; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index f4cfa7fd8d..41ca2edf44 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -434,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); 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 176d4ed11b..0727c718e0 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 @@ -1076,7 +1076,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder - + _att_sp.apply_flaps = false; // by default we don't use flaps float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1207,6 +1207,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); From 42d03cb07695d832f06abeda448212714fa35dfe Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:59:35 +0200 Subject: [PATCH 119/624] activate wheel controller as soon as plane flares --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 0727c718e0..8d8409445d 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 @@ -1291,6 +1291,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; + /* enable direct yaw control using rudder/wheel */ + _att_sp.yaw_body = target_bearing; + _att_sp.fw_control_yaw = true; + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { From f3e0d91f24ed30b32647a10b30b82f1bda9b29f5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 07:47:39 +0200 Subject: [PATCH 120/624] added airspeed scale parameter for takeoff and landing --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++++-- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 12 ++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) 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 8d8409445d..121eae8af5 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 @@ -281,6 +281,7 @@ private: float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; int land_use_terrain_estimate; + float land_airspeed_scale; } _parameters; /**< local copies of interesting parameters */ @@ -329,6 +330,7 @@ private: param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t land_airspeed_scale; } _parameter_handles; /**< handles for interesting parameters */ @@ -572,6 +574,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -675,6 +678,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -1263,8 +1267,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this could make a great param float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..14b73038a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); + +/** + * Takeoff and landing airspeed scale factor + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed for takeoff and landing approach. + * + * @min 1.0 + * @max 1.5 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f); From b30091be00edc5e9994cf12bc08d644d4c0f8d3b Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 11:06:10 +0200 Subject: [PATCH 121/624] minor fixes --- ROMFS/px4fmu_common/init.d/2105_maja | 2 +- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 8 ++++---- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index af27eca5d9..2cbc11c275 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -42,7 +42,7 @@ fi set MIXER AERTW # use PWM parameters for throttle channel -set PWM_OUT 4 +set PWM_OUT 5 set PWM_DISARMED p:PWM_DISARMED set PWM_MIN p:PWM_MIN set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index a28dd9ec6e..49665c5110 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -83,14 +83,14 @@ Gimbal / flaps / payload mixer for last three channels, using the payload control group ----------------------------------------------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 2 0 10000 10000 0 -10000 10000 -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - M: 1 O: 10000 10000 0 -10000 10000 S: 2 2 10000 10000 0 -10000 10000 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 579096268e..7600aca22b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -421,7 +421,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); - _parameter_handles.flaperon_scale = param_find("FW_FLAPERONS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); _parameter_handles.vtol_type = param_find("VT_TYPE"); @@ -823,7 +823,7 @@ FixedwingAttitudeControl::task_main() /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { - flaps_control = _manual.flaps * _parameters.flaps_scale; + flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; } else if (_vcontrol_mode.flag_control_auto_enabled) { flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } @@ -833,7 +833,7 @@ FixedwingAttitudeControl::task_main() /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { - flaperon = _manual.aux2 * _parameters.flaperon_scale; + flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; } else if (_vcontrol_mode.flag_control_auto_enabled) { flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } From 36df3a04996e0d6e69d02e8ae59768ce25aab416 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 12:55:44 +0200 Subject: [PATCH 122/624] fixed minor things from review --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 52 +++++++++++++++++++++--- src/lib/runway_takeoff/RunwayTakeoff.h | 2 +- 2 files changed, 47 insertions(+), 7 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 16cbb04e45..90e3601e69 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,7 +47,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -61,6 +60,7 @@ RunwayTakeoff::RunwayTakeoff() : _climbout(false), _start_sp{}, _target_sp{}, + _throttle_ramp_time(2 * 1e6), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -87,7 +87,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); - _climbout = true; + _climbout = true; // this is true until climbout is finished } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -95,7 +95,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: - if (hrt_elapsed_time(&_initialized_time) > 1e6) { + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } @@ -131,12 +131,21 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } +/* + * Returns true as long as we're below navigation altitude + */ bool RunwayTakeoff::controlYaw() { // keep controlling yaw directly until we start navigation return _state < RunwayTakeoffState::CLIMBOUT; } +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { @@ -146,6 +155,9 @@ float RunwayTakeoff::getPitch(float tecsPitch) return tecsPitch; } +/* + * Returns the roll setpoint to use. + */ float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 @@ -163,16 +175,25 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return navigatorRoll; } +/* + * Returns the yaw setpoint to use. + */ float RunwayTakeoff::getYaw(float navigatorYaw) { return navigatorYaw; } +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ float RunwayTakeoff::getThrottle(float tecsThrottle) { switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { - float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * _takeoff_throttle.get(); return throttle < _takeoff_throttle.get() ? throttle : @@ -193,9 +214,16 @@ bool RunwayTakeoff::resetIntegrators() return _state < RunwayTakeoffState::TAKEOFF; } +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) { - if (_climbout) { + if (_state < RunwayTakeoffState::FLY) { return math::max(sp_min, climbout_min); } @@ -204,9 +232,15 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { return _max_takeoff_pitch.get(); } @@ -215,6 +249,9 @@ float RunwayTakeoff::getMaxPitch(float max) } } +/* + * Returns the "previous" (start) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getPrevWP() { math::Vector<2> prev_wp; @@ -223,6 +260,9 @@ math::Vector<2> RunwayTakeoff::getPrevWP() return prev_wp; } +/* + * Returns the "current" (target) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) { if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2913b2ad64..cf0ab455d8 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,7 +50,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -105,6 +104,7 @@ private: bool _climbout; struct position_setpoint_s _start_sp; struct position_setpoint_s _target_sp; + unsigned _throttle_ramp_time; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; From 1ae722159332da0fa0abf1cbd939ea0670e6b2fe Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 13:56:16 +0200 Subject: [PATCH 123/624] make flaps and flaperons continuous --- .../fw_att_control/fw_att_control_main.cpp | 44 +++++++++++++++++-- 1 file changed, 41 insertions(+), 3 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 7600aca22b..3e7a9f9aa8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -161,6 +161,10 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ + float _flaps_cmd_last; + float _flaperons_cmd_last; + + struct { float tconst; float p_p; @@ -363,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false), - _debug(false) + _debug(false), + _flaps_cmd_last(0), + _flaperons_cmd_last(0) { /* safely initialize structs */ _ctrl_state = {}; @@ -821,6 +827,8 @@ FixedwingAttitudeControl::task_main() /* default flaps to center */ float flaps_control = 0.0f; + static float delta_flaps = 0; + /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; @@ -828,9 +836,25 @@ FixedwingAttitudeControl::task_main() flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } + // move the actual control value continuous with time + static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { + t_flaps_changed = hrt_absolute_time(); + delta_flaps = flaps_control - _flaps_cmd_last; + _flaps_cmd_last = flaps_control; + } + + static float flaps_applied = 0.0f; + + if (fabsf(flaps_applied - flaps_control) > 0.01f) { + flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000; + } + /* default flaperon to center */ float flaperon = 0.0f; + static float delta_flaperon = 0.0f; + /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; @@ -838,6 +862,20 @@ FixedwingAttitudeControl::task_main() flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } + // move the actual control value continuous with time + static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { + t_flaperons_changed = hrt_absolute_time(); + delta_flaperon = flaperon - _flaperons_cmd_last; + _flaperons_cmd_last = flaperon; + } + + static float flaperon_applied = 0.0f; + + if (fabsf(flaperon_applied - flaperon) > 0.01f) { + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ @@ -1141,9 +1179,9 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } - _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied; _actuators.control[5] = _manual.aux1; - _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ From 11c6ee2b5a9115b0b0e6727bd6f2df574ad447f3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 17:14:38 +0200 Subject: [PATCH 124/624] make terrain estimate invalid after range sensor timeout --- src/lib/terrain_estimation/terrain_estimator.cpp | 9 ++++++++- src/lib/terrain_estimation/terrain_estimator.h | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index f522e42047..16ab8de4a0 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -38,6 +38,8 @@ #include "terrain_estimator.h" +#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead + TerrainEstimator::TerrainEstimator() : _distance_last(0.0f), _terrain_valid(false), @@ -100,9 +102,14 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transposed() + Q) * dt; } -void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude) { + // terrain estimate is invalid if we have range sensor timeout + if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { + _terrain_valid = false; + } + if (distance->timestamp > _time_last_distance) { float d = distance->current_distance; diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 981e621236..fc10cb8d63 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,7 +65,7 @@ public: void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, const struct distance_sensor_s *distance); - void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2193b3e945..e4c8eeccde 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -702,7 +702,7 @@ void AttitudePositionEstimatorEKF::task_main() // Run separate terrain estimator _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); - _terrain_estimator->measurement_update(&_gps, &_distance, &_att); + _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att); // Publish attitude estimations publishAttitude(); From 5949b6615d52fe694f3f2a8824d19838158fd572 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 18:08:35 +0200 Subject: [PATCH 125/624] don't reset the yaw integrator on takeoff --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 121eae8af5..02a61a20e7 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 @@ -1443,9 +1443,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + // reset integrals except yaw (which also counts for the wheel controller) _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); - _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ From 4e22d65325cc34423e0259402de3a720d71e78f1 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 22:01:09 +0200 Subject: [PATCH 126/624] don't use virtual line anymore for takeoff but use correct starting point to navigate, updated default parameters for wheel controller --- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 4 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 58 +++++++++---------- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++-- .../fw_att_control/fw_att_control_params.c | 4 +- .../fw_pos_control_l1_main.cpp | 32 +++------- 5 files changed, 45 insertions(+), 65 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 76419ef6f7..7f8a9ca6c7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -111,8 +111,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + integrator_constrained; - /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", - (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 90e3601e69..46df5cd744 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,9 +58,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _start_sp{}, - _target_sp{}, _throttle_ramp_time(2 * 1e6), + _start_wp(), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -81,16 +80,19 @@ RunwayTakeoff::~RunwayTakeoff() } -void RunwayTakeoff::init(float yaw) +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) { _init_yaw = yaw; _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; } -void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { @@ -112,6 +114,16 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -127,7 +139,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; default: - return; + break; } } @@ -177,10 +189,18 @@ float RunwayTakeoff::getRoll(float navigatorRoll) /* * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. */ float RunwayTakeoff::getYaw(float navigatorYaw) { - return navigatorYaw; + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } } /* @@ -252,31 +272,9 @@ float RunwayTakeoff::getMaxPitch(float max) /* * Returns the "previous" (start) WP for navigation. */ -math::Vector<2> RunwayTakeoff::getPrevWP() +math::Vector<2> RunwayTakeoff::getStartWP() { - math::Vector<2> prev_wp; - prev_wp(0) = (float)_start_sp.lat; - prev_wp(1) = (float)_start_sp.lon; - return prev_wp; -} - -/* - * Returns the "current" (target) WP for navigation. - */ -math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) -{ - if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { - // navigating towards calculated direction if heading mode 0 and as long as we're in climbout - math::Vector<2> curr_wp; - curr_wp(0) = (float)_target_sp.lat; - curr_wp(1) = (float)_target_sp.lon; - return curr_wp; - - } else { - // navigating towards next mission waypoint - return sp_curr_wp; - } - + return _start_wp; } void RunwayTakeoff::reset() diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index cf0ab455d8..0e1c5ed14b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -68,16 +68,14 @@ public: RunwayTakeoff(); ~RunwayTakeoff(); - void init(float yaw); - void update(float airspeed, float alt_agl, int mavlink_fd); + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); RunwayTakeoffState getState() { return _state; }; bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; - position_setpoint_s *getStartSP() { return &_start_sp; }; - position_setpoint_s *getTargetSP() { return &_target_sp; }; float getInitYaw() { return _init_yaw; }; bool controlYaw(); @@ -89,8 +87,7 @@ public: bool resetIntegrators(); float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); - math::Vector<2> getPrevWP(); - math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); + math::Vector<2> getStartWP(); void reset(); @@ -102,9 +99,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; - struct position_setpoint_s _start_sp; - struct position_setpoint_s _target_sp; unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 41ca2edf44..ce2db9ad5c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); /** * Wheel steering rate integrator gain @@ -254,7 +254,7 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); /** * Maximum wheel steering rate 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 02a61a20e7..45032d4735 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 @@ -1375,45 +1375,31 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw); + _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - // draw a line from takeoff location into the direction the UAV is heading - get_waypoint_heading_distance( - _runway_takeoff.getInitYaw(), - HDG_HOLD_DIST_NEXT, - *_runway_takeoff.getStartSP(), - *_runway_takeoff.getTargetSP(), - true); - mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // update takeoff path if we're reaching the end of it - if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance( - _runway_takeoff.getInitYaw(), - HDG_HOLD_DIST_NEXT, - *_runway_takeoff.getStartSP(), - *_runway_takeoff.getTargetSP(), - false); - } - - /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet - * or the calculated one through initial heading */ - _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper _runway_takeoff.update( _airspeed.true_airspeed_m_s, _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, _mavlink_fd); + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); + // update tecs float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); From 73b1c186989f2c11426badb7a19e6417688efab1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 13:49:22 +0200 Subject: [PATCH 127/624] do not stick to terrain estimate if it's not valid --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 45032d4735..b608ea7f9e 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 @@ -963,7 +963,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { + if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { if(!land_useterrain) { mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; From 0bd23dd7c5511446451a491424ea58d296b9ca4b Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 13:56:53 +0200 Subject: [PATCH 128/624] only go into heading hold mode after flaring --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 b608ea7f9e..122ce0fdda 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 @@ -1227,8 +1227,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + if (land_noreturn_vertical) { /* heading hold, along the line connecting this and the last waypoint */ From 2a2e2a27d321146aa06ae7588c5d2f3cad1375d2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 14:55:32 +0200 Subject: [PATCH 129/624] update comments --- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index 49665c5110..2ab5bdb611 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -1,16 +1,16 @@ -Aileron/rudder/elevator/throttle/wheel mixer for PX4FMU +Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU ======================================================= This file defines mixers suitable for controlling a fixed wing aircraft with aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. The configuration assumes the aileron servo(s) are connected to PX4FMU servo -output 0, the elevator to output 1, the rudder to output 2, the throttle -to output 3 and the wheel to output 4. +output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle +to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Flaperon mixer (ailerons + flaps) +Aileron mixer (roll + flaperon) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -79,7 +79,7 @@ O: 10000 10000 0 -10000 10000 S: 0 2 10000 10000 0 -10000 10000 -Gimbal / flaps / payload mixer for last three channels, +Flaps / gimbal / payload mixer for last three channels, using the payload control group ----------------------------------------------------- From 8fa22c2efa7494956d32e8cd27961adc2c9de0c6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 14:57:34 +0200 Subject: [PATCH 130/624] renamed mixer file --- ROMFS/px4fmu_common/mixers/{AERTW.main.mix => AERTWF.main.mix} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/mixers/{AERTW.main.mix => AERTWF.main.mix} (100%) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTWF.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/AERTW.main.mix rename to ROMFS/px4fmu_common/mixers/AERTWF.main.mix From 5f400946855dc3cac6e1faf133a16018bed85588 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 16:10:33 +0200 Subject: [PATCH 131/624] fixed bug which lead to direct yaw control in stabilized mode --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 3e7a9f9aa8..8772ceb835 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -800,6 +800,10 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* lock integrator until control is started */ bool lock_integrator; From 3d3398e33026bb5c12157d1490805a768313dd0e Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 27 Oct 2015 16:39:33 +0100 Subject: [PATCH 132/624] added code handling aborting landings --- .../fw_pos_control_l1_main.cpp | 73 ++++++++++++++++++- src/modules/navigator/navigator_main.cpp | 11 ++- 2 files changed, 78 insertions(+), 6 deletions(-) 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 122ce0fdda..61d29494db 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 @@ -105,6 +105,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode @@ -196,6 +197,11 @@ private: bool land_onslope; bool land_useterrain; + // terrain estimation states + float _t_alt_prev_valid; //**< last terrain estimate which was valid */ + hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ + hrt_abstime _time_started_landing; //*< time at which landing started */ + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -530,6 +536,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _t_alt_prev_valid(0), + _time_last_t_alt(0), + _time_started_landing(0), _was_in_air(false), _time_went_in_air(0), _runway_takeoff(), @@ -1198,13 +1207,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (in_takeoff_situation()) { + float alt_sp; + if (_nav_capabilities.abort_landing == true) { + // if we entered loiter due to an aborted landing, demand + // altitude setpoint well above landing waypoint + alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { + // use altitude given by wapoint + alt_sp = _pos_sp_triplet.current.alt; + } + + if (in_takeoff_situation() || + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); @@ -1215,6 +1235,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // if they have been enabled using the corresponding parameter _att_sp.apply_flaps = true; + // save time at which we started landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + } + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1273,7 +1298,40 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ - float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + float terrain_alt; + if (_parameters.land_use_terrain_estimate) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 5 * 1000 * 1000) { + terrain_alt = _pos_sp_triplet.current.alt; + } else { + // still no valid terrain, abort landing + terrain_alt = _pos_sp_triplet.current.alt; + _nav_capabilities.abort_landing = true; + } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _nav_capabilities.abort_landing = true; + } + } else { + // no terrain estimation, just use landing waypoint altitude + terrain_alt = _pos_sp_triplet.current.alt; + } + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? @@ -1720,9 +1778,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /** MANUAL FLIGHT **/ - /* reset hold altitude */ + // reset hold altitude _hold_alt = _global_pos.alt; + // reset terrain estimation relevant values + _time_started_landing = 0; + _time_last_t_alt = 0; + + // reset lading abort state + _nav_capabilities.abort_landing = false; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e44b8d0be5..b056e3fd99 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -440,8 +440,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; + if (_nav_caps.abort_landing) { + // pos controller aborted landing, requests loiter + // above landing waypoint + _navigation_mode = &_loiter; + _pos_sp_triplet_published_invalid_once = false; + } else { + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; From 0f03ae12d79f062a65de76ab6690def0fd1ac6f8 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 07:03:45 +0100 Subject: [PATCH 133/624] added field for aborted fw landings --- msg/navigation_capabilities.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg index 235b5df03b..6d12aaaed9 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/navigation_capabilities.msg @@ -3,3 +3,4 @@ float32 turn_distance # the optimal distance to a waypoint to switch to the nex float32 landing_horizontal_slope_displacement float32 landing_slope_angle_rad float32 landing_flare_length +bool abort_landing From 896dff40cfbee4e93bb91d4c988bcceab32d9d27 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 08:02:51 +0100 Subject: [PATCH 134/624] added geo functions to create new waypoints from given setting --- src/lib/geo/geo.c | 25 +++++++++++++++++++++++++ src/lib/geo/geo.h | 6 ++++++ 2 files changed, 31 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a4b1599a9d..07d765c3bc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,6 +298,31 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +{ + float heading; + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + } + else if (dist >= FLT_EPSILON) { + heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { + heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + 180.0f * M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH); + + *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); +} __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f77a8b58a4..f9250fb5a3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -236,6 +236,12 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +// TODO put description for both functions and improve naming +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); + /** * Returns the bearing to the next waypoint in radians. * From 7f0c3a9b71ada3b422bbd8c339ba7696cd39beb3 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 08:16:30 +0100 Subject: [PATCH 135/624] use virtual setpoint for landing line tracking --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) 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 61d29494db..e08af118e9 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 @@ -1252,6 +1252,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + math::Vector<2> curr_wp_shifted; + double lat; + double lon; + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + curr_wp_shifted(0) = (float)lat; + curr_wp_shifted(1) = (float)lon; // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds if (land_noreturn_vertical) { @@ -1276,7 +1286,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); From 6f4c8d45ff5624f341a8ce017faca86bae47607a Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 10:37:30 +0100 Subject: [PATCH 136/624] during flare control pitch setpoint based on distance to ground --- .../fw_pos_control_l1_main.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) 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 e08af118e9..7a98859f81 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 @@ -197,10 +197,11 @@ private: bool land_onslope; bool land_useterrain; - // terrain estimation states + // landing relevant states float _t_alt_prev_valid; //**< last terrain estimate which was valid */ hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ hrt_abstime _time_started_landing; //*< time at which landing started */ + float height_flare; //*< estimated height to ground at which flare started */ bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -539,6 +540,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _t_alt_prev_valid(0), _time_last_t_alt(0), _time_started_landing(0), + height_flare(0.0f), _was_in_air(false), _time_went_in_air(0), _runway_takeoff(), @@ -1396,10 +1398,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + height_flare = _global_pos.alt - terrain_alt; mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare; + } else { + _att_sp.pitch_body = _att_sp.pitch_body; + } } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1852,7 +1862,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled()) + _runway_takeoff.runwayTakeoffEnabled() || + land_noreturn_vertical) )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 5b1c7321e732a20e39dc52bc0ce174c23ea858ad Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 14:08:42 +0100 Subject: [PATCH 137/624] reverted navigation on landing back to old heading hold --- src/lib/geo/geo.c | 4 ++-- .../fw_pos_control_l1_main.cpp | 20 +++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 07d765c3bc..0d09e8526b 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -310,7 +310,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); - heading = _wrap_2pi(heading + 180.0f * M_PI_F); + heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } } @@ -318,7 +318,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); - double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); 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 7a98859f81..080b96c5ce 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 @@ -1264,9 +1264,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; + // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds - if (land_noreturn_vertical) { + //if (land_noreturn_vertical) { + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1288,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -1405,7 +1407,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_noreturn_vertical = true; } else { if (_global_pos.vel_d > 0.1f) { - _att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare; + _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); } else { _att_sp.pitch_body = _att_sp.pitch_body; } @@ -1859,12 +1862,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled() || - land_noreturn_vertical) - )) { + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From bc0fb69189ecf20c13c88fd81e86923ed1d3f345 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 14:52:15 +0100 Subject: [PATCH 138/624] change mixer for maja --- ROMFS/px4fmu_common/init.d/2105_maja | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index 2cbc11c275..10c5608eae 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -39,7 +39,7 @@ then param set PWM_DISARMED 1000 fi -set MIXER AERTW +set MIXER AERTWF # use PWM parameters for throttle channel set PWM_OUT 5 From d97ead81aa249ba82f518e6f7c59bd50e5f0d5d3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 17:06:38 +0100 Subject: [PATCH 139/624] set talt timeout to 10sec --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 080b96c5ce..0b07ceda39 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 @@ -1323,7 +1323,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // we have started landing phase but don't have valid terrain // wait for some time, maybe we will soon get a valid estimate // until then just use the altitude of the landing waypoint - if (hrt_elapsed_time(&_time_started_landing) < 5 * 1000 * 1000) { + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { terrain_alt = _pos_sp_triplet.current.alt; } else { // still no valid terrain, abort landing From 5e4df86091f587ffcbb9fdb66fe87d00aaf2520b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 2 Nov 2015 17:40:32 +0100 Subject: [PATCH 140/624] added albatross config and mixer --- ROMFS/px4fmu_common/init.d/2105_maja | 12 +-- ROMFS/px4fmu_common/init.d/2106_albatross | 51 +++++++++++ .../{AERTWF.main.mix => AAERTWF.main.mix} | 0 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 84 +++++++++++++++++++ 4 files changed, 142 insertions(+), 5 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/2106_albatross rename ROMFS/px4fmu_common/mixers/{AERTWF.main.mix => AAERTWF.main.mix} (100%) create mode 100644 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index 10c5608eae..1f68428f3b 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -5,10 +5,12 @@ # @type Standard Plane # # @output MAIN1 aileron -# @output MAIN2 elevator -# @output MAIN3 rudder -# @output MAIN4 throttle -# @output MAIN5 wheel +# @output MAIN2 aileron +# @output MAIN3 elevator +# @output MAIN4 rudder +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps # # @output AUX1 feed-through of RC AUX1 channel # @output AUX2 feed-through of RC AUX2 channel @@ -39,7 +41,7 @@ then param set PWM_DISARMED 1000 fi -set MIXER AERTWF +set MIXER AAERTWF # use PWM parameters for throttle channel set PWM_OUT 5 diff --git a/ROMFS/px4fmu_common/init.d/2106_albatross b/ROMFS/px4fmu_common/init.d/2106_albatross new file mode 100644 index 0000000000..79e560fb5f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2106_albatross @@ -0,0 +1,51 @@ +#!nsh +# +# @name Applied Aeronautics Albatross +# +# @type Standard Plane +# +# @output MAIN1 aileron right +# @output MAIN2 aileron left +# @output MAIN3 v-tail right +# @output MAIN4 v-tail left +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps right +# @output MAIN8 flaps left +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAVVTWFF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/AERTWF.main.mix rename to ROMFS/px4fmu_common/mixers/AAERTWF.main.mix diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix new file mode 100644 index 0000000000..52146b84f0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -0,0 +1,84 @@ +Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps +using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU servo +output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4, the wheel to output 5 and the flaps to output 6 and 7. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 + +Flaps mixer +------------ +Flap servos are physically reversed. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 5000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 -10000 -5000 0 -10000 10000 + From 3a46487fa4f58889dae818075e6ed6dd73f6106c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 3 Nov 2015 15:35:28 +0100 Subject: [PATCH 141/624] fixed flaps offset to have correct neutral position --- ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix index 52146b84f0..e6520862d7 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -76,9 +76,9 @@ Flap servos are physically reversed. M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 5000 0 -10000 10000 +S: 0 4 0 5000 -10000 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 -10000 -5000 0 -10000 10000 +S: 0 4 0 -5000 10000 -10000 10000 From 4abff89be0bd022c87e1be0a533c8c914478704e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 6 Nov 2015 22:48:44 +0100 Subject: [PATCH 142/624] updated terrain estimator and runway takeoff libs to cmake build system --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 2 ++ cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 ++ cmake/configs/posix_eagle_default.cmake | 2 ++ cmake/configs/posix_sitl_simple.cmake | 2 ++ cmake/configs/qurt_eagle_hil.cmake | 2 ++ cmake/configs/qurt_eagle_release.cmake | 2 ++ cmake/configs/qurt_eagle_travis.cmake | 2 ++ src/lib/ecl/CMakeLists.txt | 1 + .../{module.mk => CMakeLists.txt} | 22 ++++++++++--------- .../{module.mk => CMakeLists.txt} | 14 ++++++++---- 10 files changed, 37 insertions(+), 14 deletions(-) rename src/lib/runway_takeoff/{module.mk => CMakeLists.txt} (86%) rename src/lib/terrain_estimation/{module.mk => CMakeLists.txt} (90%) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index ed48bd4f34..902447c282 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -111,6 +111,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 0c6458cad1..c3a3586f66 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -118,6 +118,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index fc97e70918..76d9711187 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -38,6 +38,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 581b82e827..a7291a04f3 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -56,6 +56,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff ) set(config_extra_builtin_cmds diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 4222fff249..87dd29152e 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -47,6 +47,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 945e9da199..62a20263e5 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -70,6 +70,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index b9fda6ec48..75cb5f2394 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -52,6 +52,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/ecl + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index 0fd5b5f5e9..9f8e02ba2e 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( attitude_fw/ecl_pitch_controller.cpp attitude_fw/ecl_roll_controller.cpp attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_wheel_controller.cpp l1/ecl_l1_pos_controller.cpp validation/data_validator.cpp validation/data_validator_group.cpp diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/CMakeLists.txt similarity index 86% rename from src/lib/runway_takeoff/module.mk rename to src/lib/runway_takeoff/CMakeLists.txt index 95b9aea53e..7938279a0e 100644 --- a/src/lib/runway_takeoff/module.mk +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. +# Copyright (c) 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 @@ -30,12 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# RunwayTakeoff Library -# - -SRCS = RunwayTakeoff.cpp \ - runway_takeoff_params.c - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__runway_takeoff + COMPILE_FLAGS + -Os + SRCS + RunwayTakeoff.cpp + runway_takeoff_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/terrain_estimation/module.mk b/src/lib/terrain_estimation/CMakeLists.txt similarity index 90% rename from src/lib/terrain_estimation/module.mk rename to src/lib/terrain_estimation/CMakeLists.txt index 3a2047012a..0c1177b96b 100644 --- a/src/lib/terrain_estimation/module.mk +++ b/src/lib/terrain_estimation/CMakeLists.txt @@ -30,7 +30,13 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -SRCS = terrain_estimator.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__terrain_estimation + COMPILE_FLAGS + -Os + SRCS + terrain_estimator.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From 2719789b2e773f4d4dfc347649d1bb71bc690b57 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:15 +0100 Subject: [PATCH 143/624] use matrix lib to enable building for posix --- .../terrain_estimation/terrain_estimator.cpp | 54 +++++++++---------- .../terrain_estimation/terrain_estimator.h | 5 +- 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 16ab8de4a0..a92bbb136e 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -46,9 +46,9 @@ TerrainEstimator::TerrainEstimator() : _time_last_distance(0), _time_last_gps(0) { - _x.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); _u_z = 0.0f; - _P.identity(); + _P.setIdentity(); } bool TerrainEstimator::is_distance_valid(float distance) { @@ -63,9 +63,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu const struct distance_sensor_s *distance) { if (attitude->R_valid) { - math::Matrix<3, 3> R_att(attitude->R); - math::Vector<3> a(&sensor->accelerometer_m_s2[0]); - math::Vector<3> u; + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; u = R_att * a; _u_z = u(2) + 9.81f; // compensate for gravity @@ -74,32 +74,32 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu } // dynamics matrix - math::Matrix A; - A.zero(); + matrix::Matrix A; + A.setZero(); A(0,1) = 1; A(1,2) = 1; // input matrix - math::Matrix B; - B.zero(); + matrix::Matrix B; + B.setZero(); B(1,0) = 1; // input noise variance float R = 0.135f; // process noise convariance - math::Matrix Q; + matrix::Matrix Q; Q(0,0) = 0; Q(1,1) = 0; // do prediction - math::Vector dx = (A * _x) * dt; + matrix::Vector dx = (A * _x) * dt; dx(1) += B(1,0) * _u_z * dt; // propagate state and covariance matrix _x += dx; - _P += (A * _P + _P * A.transposed() + - B * R * B.transposed() + Q) * dt; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; } void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, @@ -114,21 +114,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl float d = distance->current_distance; - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0, 0) = -1; // measured altitude, float R = 0.009f; - math::Vector<1> y; + matrix::Vector y; y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; // some sort of outlayer rejection if (fabsf(distance->current_distance - _distance_last) < 1.0f) { @@ -149,21 +149,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0,1) = 1; float R = 0.056f; - math::Vector<1> y; + matrix::Vector y; y(0) = gps->vel_d_m_s; // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; @@ -187,8 +187,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (reinit) { - _x.zero(); - _P.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index fc10cb8d63..697a053b88 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -36,6 +36,7 @@ */ #include +#include "matrix/Matrix.hpp" #include #include #include @@ -75,9 +76,9 @@ private: bool _terrain_valid; // kalman filter variables - math::Vector _x; // state: ground distance, velocity, accel bias in z direction + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction float _u_z; // acceleration in earth z direction - math::Matrix<3, 3> _P; // covariance matrix + matrix::Matrix _P; // covariance matrix // timestamps uint64_t _time_last_distance; From 120fd9d5225e1e09a19dc51c0f0ccc27b4a98766 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:56 +0100 Subject: [PATCH 144/624] use control state topic for attitude and airspeed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 0b07ceda39..24cf1f084f 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 @@ -1457,7 +1457,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); + math::Quaternion q(&_ctrl_state.q[0]); + math::Vector<3> euler = q.to_euler(); + _runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ @@ -1470,7 +1472,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // update runway takeoff helper _runway_takeoff.update( - _airspeed.true_airspeed_m_s, + _ctrl_state.airspeed, _global_pos.alt - terrain_alt, _global_pos.lat, _global_pos.lon, From e48cf53ce83ac7f1e30b3e662e6eae5e568688c3 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:43:07 +0100 Subject: [PATCH 145/624] fix code style --- src/lib/geo/geo.c | 15 ++++--- src/lib/geo/geo.h | 6 ++- src/lib/runway_takeoff/RunwayTakeoff.cpp | 2 +- .../terrain_estimation/terrain_estimator.cpp | 40 +++++++++++-------- .../terrain_estimation/terrain_estimator.h | 7 ++-- 5 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 0d09e8526b..023337ff04 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,16 +298,19 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) { float heading; + if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; - } - else if (dist >= FLT_EPSILON) { + + } else if (dist >= FLT_EPSILON) { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); @@ -315,13 +318,15 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } } -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), + cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f9250fb5a3..8dc0836e34 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -238,9 +238,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou // TODO put description for both functions and improve naming -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *end_lat, double *end_lon); /** * Returns the bearing to the next waypoint in radians. diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 46df5cd744..7089889858 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -92,7 +92,7 @@ void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) } void RunwayTakeoff::update(float airspeed, float alt_agl, - double current_lat, double current_lon, int mavlink_fd) + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index a92bbb136e..64a747f532 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -51,16 +51,19 @@ TerrainEstimator::TerrainEstimator() : _P.setIdentity(); } -bool TerrainEstimator::is_distance_valid(float distance) { +bool TerrainEstimator::is_distance_valid(float distance) +{ if (distance > 40.0f || distance < 0.00001f) { return false; + } else { return true; } } -void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance) +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) { if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); @@ -76,25 +79,25 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu // dynamics matrix matrix::Matrix A; A.setZero(); - A(0,1) = 1; - A(1,2) = 1; + A(0, 1) = 1; + A(1, 2) = 1; // input matrix - matrix::Matrix B; + matrix::Matrix B; B.setZero(); - B(1,0) = 1; + B(1, 0) = 1; // input noise variance float R = 0.135f; // process noise convariance matrix::Matrix Q; - Q(0,0) = 0; - Q(1,1) = 0; + Q(0, 0) = 0; + Q(1, 1) = 0; // do prediction matrix::Vector dx = (A * _x) * dt; - dx(1) += B(1,0) * _u_z * dt; + dx(1) += B(1, 0) * _u_z * dt; // propagate state and covariance matrix _x += dx; @@ -102,8 +105,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transpose() + Q) * dt; } -void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, - const struct vehicle_attitude_s *attitude) +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) { // terrain estimate is invalid if we have range sensor timeout if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { @@ -124,7 +128,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv (S_I); matrix::Vector r = y - C * _x; @@ -140,6 +144,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // estimate to be invalid if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { _terrain_valid = false; + } else { _terrain_valid = true; } @@ -150,7 +155,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { matrix::Matrix C; - C(0,1) = 1; + C(0, 1) = 1; float R = 0.056f; @@ -159,7 +164,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv(S_I); matrix::Vector r = y - C * _x; @@ -172,6 +177,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // reinitialise filter if we find bad data bool reinit = false; + for (int i = 0; i < n_x; i++) { if (!PX4_ISFINITE(_x(i))) { reinit = true; @@ -180,7 +186,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl for (int i = 0; i < n_x; i++) { for (int j = 0; j < n_x; j++) { - if (!PX4_ISFINITE(_P(i,j))) { + if (!PX4_ISFINITE(_P(i, j))) { reinit = true; } } @@ -189,7 +195,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (reinit) { memset(&_x._data[0], 0, sizeof(_x._data)); _P.setZero(); - _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; } } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 697a053b88..25723f21d8 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,12 +65,13 @@ public: float get_velocity() {return _x(1);}; void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance); - void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: - enum {n_x=3}; + enum {n_x = 3}; float _distance_last; bool _terrain_valid; From c989c21269e70399389af5a48a4222145f9512d6 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 10 Nov 2015 13:33:01 +0100 Subject: [PATCH 146/624] fixed geo functions --- src/lib/geo/geo.c | 15 +++++++++++---- src/lib/geo/geo.h | 25 +++++++++++++++++++++++-- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 023337ff04..a8eee50435 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -319,14 +319,21 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *lat_end, double *lon_end) + double *lat_target, double *lon_target) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); - *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), - cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + double lat_start_rad = lat_start / (double)180.0 * M_PI; + double lon_start_rad = lon_start / (double)180.0 * M_PI; + + *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( + double)bearing)); + *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), + cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); + + *lat_target *= (double)180.0 / M_PI; + *lon_target *= (double)180.0 / M_PI; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 8dc0836e34..0aa6cf03a8 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -237,12 +237,33 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// TODO put description for both functions and improve naming +/** + * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance + * from waypoint A + * + * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°) + * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) + * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) + * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) + * @param dist distance of target waypoint from waypoint A (can be negative) + * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) + */ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +/** + * Creates a waypoint from given waypoint, distance and bearing + * + * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) + * @param bearing + * @param distance + * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) + */ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *end_lat, double *end_lon); + double *lat_target, double *lon_target); /** * Returns the bearing to the next waypoint in radians. From 154fa07a4647534f2673e86870ae9b5d292f98d3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 16 Nov 2015 12:02:56 +0100 Subject: [PATCH 147/624] fixes after review --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++-------- src/lib/geo/geo.h | 7 ++++--- src/lib/runway_takeoff/runway_takeoff_params.c | 4 ++++ src/lib/terrain_estimation/terrain_estimator.h | 6 ++++-- .../ekf_att_pos_estimator_main.cpp | 2 +- 6 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 7f8a9ca6c7..7e34ff2a61 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -33,7 +33,7 @@ /** * @file ecl_wheel_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * Implementation of a simple PID wheel controller for heading tracking. * * Authors and acknowledgements in header. */ diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a8eee50435..af49bf1504 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -301,18 +301,16 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) { - float heading; - if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; } else if (dist >= FLT_EPSILON) { - heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { - heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } @@ -324,16 +322,16 @@ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_st bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); - double lat_start_rad = lat_start / (double)180.0 * M_PI; - double lon_start_rad = lon_start / (double)180.0 * M_PI; + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( double)bearing)); *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); - *lat_target *= (double)180.0 / M_PI; - *lon_target *= (double)180.0 / M_PI; + *lat_target *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0aa6cf03a8..11451fcf19 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -245,7 +245,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) - * @param dist distance of target waypoint from waypoint A (can be negative) + * @param dist distance of target waypoint from waypoint A in meters (can be negative) * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) */ @@ -254,11 +254,12 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou /** * Creates a waypoint from given waypoint, distance and bearing + * see http://www.movable-type.co.uk/scripts/latlong.html * * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) - * @param bearing - * @param distance + * @param bearing in rad + * @param distance in meters * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) */ diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 7c64889e76..f5a1017f96 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -71,6 +71,7 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0); * rudder is used to keep the heading (see RWTO_HDG). This should be below * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * + * @unit m * @min 0.0 * @max 100.0 * @group Runway Takeoff @@ -93,6 +94,7 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. * + * @unit deg * @min 0.0 * @max 20.0 * @group Runway Takeoff @@ -104,6 +106,7 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * Fixed-wing settings are used if set to 0. Note that there is also a minimum * pitch of 10 degrees during takeoff, so this must be larger if set. * + * @unit deg * @min 0.0 * @max 60.0 * @group Runway Takeoff @@ -115,6 +118,7 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); * Roll is limited during climbout to ensure enough lift and prevents aggressive * navigation before we're on a safe height. * + * @unit deg * @min 0.0 * @max 60.0 * @group Runway Takeoff diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 25723f21d8..ed9e9741f3 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -51,14 +51,14 @@ * The measurement_update(...) function does a measurement update based on range finder and gps * velocity measurements. Both functions should always be called together when there is new * acceleration data available. -* The is_valid() function provides information wether the estimate is valid. +* The is_valid() function provides information whether the estimate is valid. */ class __EXPORT TerrainEstimator { public: TerrainEstimator(); - ~TerrainEstimator(); + ~TerrainEstimator() {}; bool is_valid() {return _terrain_valid;} float get_distance_to_ground() {return -_x(0);} @@ -85,6 +85,7 @@ private: uint64_t _time_last_distance; uint64_t _time_last_gps; + /* struct { float var_acc_z; float var_p_z; @@ -92,6 +93,7 @@ private: float var_lidar; float var_gps_vz; } _params; + */ bool is_distance_valid(float distance); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e4c8eeccde..49f08f3cab 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -270,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } - + delete _terrain_estimator; delete _ekf; estimator::g_estimator = nullptr; From 0544d06ad0b6f398ea5d457fac5adce67bfb6795 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Nov 2015 16:13:42 -0800 Subject: [PATCH 148/624] Changed dspal submodule to come from ATLFlight The dspal headers were moved to a github organization. Signed-off-by: Mark Charlebois --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 60cdcb1105..b84b78548e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -15,7 +15,7 @@ url = https://github.com/ros/gencpp.git [submodule "src/lib/dspal"] path = src/lib/dspal - url = https://github.com/mcharleb/dspal.git + url = https://github.com/ATLFlight/dspal.git [submodule "Tools/jMAVSim"] path = Tools/jMAVSim url = https://github.com/PX4/jMAVSim.git From 238db8c6d7b75bee86a73f305fa80dc2802b4b5b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Nov 2015 19:30:51 -0800 Subject: [PATCH 149/624] Moved IOCTL definitions to DevIOCTL.h in DriverFramework Removed sched.h from platform/qurt since it is now in DSPAL. Signed-off-by: Mark Charlebois --- cmake/qurt/px4_impl_qurt.cmake | 4 ++++ src/drivers/drv_device.h | 25 +------------------ src/firmware/qurt/CMakeLists.txt | 1 + src/lib/DriverFramework | 2 +- src/lib/dspal | 2 +- src/platforms/qurt/include/sched.h | 12 ---------- src/platforms/qurt/stubs/stubs_posix.c | 33 ++++++++++++++++++++++++++ 7 files changed, 41 insertions(+), 38 deletions(-) delete mode 100644 src/platforms/qurt/include/sched.h diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 312c818ed5..87c71b1ecd 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -165,6 +165,7 @@ function(px4_os_add_flags) ${DSPAL_ROOT}/sys/sys ${DSPAL_ROOT}/mpu_spi/inc ${DSPAL_ROOT}/uart_esc/inc + src/lib/DriverFramework/framework/include src/platforms/qurt/include src/platforms/posix/include ) @@ -172,6 +173,7 @@ function(px4_os_add_flags) set(added_definitions -D__PX4_QURT -D__PX4_POSIX + -D__DF_QURT -include ${PX4_INCLUDE_DIR}visibility.h ) @@ -185,6 +187,8 @@ function(px4_os_add_flags) set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "") + set(DF_TARGET "qurt") + # output foreach(var ${inout_vars}) string(TOLOWER ${var} lower_var) diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index af0f4982b3..411c30f51f 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -46,30 +46,7 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" -#ifdef __PX4_NUTTX -/* - * ioctl() definitions - */ - -#define _DEVICEIOCBASE (0x100) -#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n)) - -/** ask device to stop publishing */ -#define DEVIOCSPUBBLOCK _DEVICEIOC(0) - -/** check publication block status */ -#define DEVIOCGPUBBLOCK _DEVICEIOC(1) - -/** - * Return device ID, to enable matching of configuration parameters - * (such as compass offsets) to specific sensors - */ -#define DEVIOCGDEVICEID _DEVICEIOC(2) - -#else -#include "DevObj.hpp" - -#endif +#include "DevIOCTL.h" #ifdef __PX4_POSIX diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt index b6d743186f..bb55e8dc22 100644 --- a/src/firmware/qurt/CMakeLists.txt +++ b/src/firmware/qurt/CMakeLists.txt @@ -24,6 +24,7 @@ target_link_libraries(mainapp -Wl,--whole-archive ${module_libraries} ${target_libraries} + df_driver_framework m -Wl,--no-whole-archive -Wl,${TOOLSLIB}/pic/libstdc++.a) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 12373364a3..94abaa2067 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 12373364a3e78a20abad6cd63b36e355d712d9e4 +Subproject commit 94abaa2067438c8b3c300c1bec05688cda5ff2e8 diff --git a/src/lib/dspal b/src/lib/dspal index 93b8d36619..94188fb83a 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit 93b8d366191fb24486e0484ca4f020c85ed6cfee +Subproject commit 94188fb83ab6b2da59eb9a8c8596ab49b67c42f6 diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h deleted file mode 100644 index b67db23b1d..0000000000 --- a/src/platforms/qurt/include/sched.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#define SCHED_FIFO 1 -#define SCHED_RR 2 - -struct sched_param { - int sched_priority; -}; - -int sched_get_priority_max(int policy); -int sched_get_priority_min(int policy); - diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/src/platforms/qurt/stubs/stubs_posix.c index 0b9faab882..64555694d4 100644 --- a/src/platforms/qurt/stubs/stubs_posix.c +++ b/src/platforms/qurt/stubs/stubs_posix.c @@ -97,3 +97,36 @@ int clock_gettime(clockid_t clk_id, struct timespec *tp) { return 1; } + +int pthread_mutex_lock(pthread_mutex_t *mutex) +{ + return 1; +} + +int pthread_mutex_unlock(pthread_mutex_t *mutex) +{ + return 1; +} + +int pthread_cond_signal(pthread_cond_t *cond) +{ + return 1; +} +int pthread_mutex_destroy(pthread_mutex_t *mutex) +{ + return 1; +} +int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr) +{ + return 1; +} + +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) +{ + return 1; +} + +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime) +{ + return 1; +} From d2cacb9bc65ef1bdf10177ac11ddef609f3ce551 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Nov 2015 23:17:52 -0800 Subject: [PATCH 150/624] Added changes to sensors.cpp for DriverFramework This only works for DF based builds (not NuttX). NuttX is not yet ported to DF. Signed-off-by: Mark Charlebois --- src/modules/sensors/sensors.cpp | 130 +++++++++++++++----------------- 1 file changed, 59 insertions(+), 71 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a1..1d66e45fa1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -97,6 +97,10 @@ #include #include +#include "DevMgr.hpp" + +using namespace DriverFramework; + /** * Analog layout: * FMU: @@ -898,23 +902,20 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int barofd; - barofd = px4_open(BARO0_DEVICE_PATH, 0); + DevHandle h_baro; + DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); - if (barofd < 0) { - warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); + if (!h_baro.isValid()) { + warnx("ERROR: no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); return ERROR; } else { - int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (baroret) { warnx("qnh could not be set"); - px4_close(barofd); return ERROR; } - - px4_close(barofd); } return OK; @@ -923,23 +924,20 @@ Sensors::parameters_update() int Sensors::accel_init() { - int fd; - - fd = px4_open(ACCEL0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); + DevHandle h_accel; + DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel); + + if (!h_accel.isValid()) { + warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); return ERROR; } else { /* set the accel internal sampling rate to default rate */ - px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); + h_accel.ioctl(ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); /* set the driver to poll at default rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - px4_close(fd); + h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); } return OK; @@ -948,23 +946,20 @@ Sensors::accel_init() int Sensors::gyro_init() { - int fd; + DevHandle h_gyro; + DevMgr::getHandle(GYRO0_DEVICE_PATH, h_gyro); - fd = px4_open(GYRO0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); + if (!h_gyro.isValid()) { + warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError()); return ERROR; } /* set the gyro internal sampling rate to default rate */ - px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + h_gyro.ioctl(GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); /* set the driver to poll at default rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - px4_close(fd); + h_gyro.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); return OK; } @@ -972,32 +967,32 @@ Sensors::gyro_init() int Sensors::mag_init() { - int fd; int ret; - fd = px4_open(MAG0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); + DevHandle h_mag; + DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag); + + if (!h_mag.isValid()) { + warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError()); return ERROR; } /* try different mag sampling rates */ - ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150); + ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); + h_mag.ioctl(SENSORIOCSPOLLRATE, 150); } else { - ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 100); + ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 100); /* if the slower sampling rate still fails, something is wrong */ if (ret == OK) { /* set the driver to poll also at the slower rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 100); + h_mag.ioctl(SENSORIOCSPOLLRATE, 100); } else { warnx("FATAL: mag sampling rate could not be set"); @@ -1005,27 +1000,22 @@ Sensors::mag_init() } } - px4_close(fd); - return OK; } int Sensors::baro_init() { - int fd; + DevHandle h_baro; + DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); - fd = px4_open(BARO0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); + if (!h_baro.isValid()) { + warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); return ERROR; } /* set the driver to poll at 150Hz */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); - - px4_close(fd); + h_baro.ioctl(SENSORIOCSPOLLRATE, 150); return OK; } @@ -1034,10 +1024,11 @@ int Sensors::adc_init() { - _fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + DevHandle h_adc; + DevMgr::getHandle(ADC0_DEVICE_PATH, h_adc); - if (_fd_adc < 0) { - warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); + if (!h_adc.isValid()) { + warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, h_adc.getError()); return ERROR; } @@ -1277,9 +1268,10 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); + DevHandle h; + DevMgr::getHandle(str, h); - if (fd < 0) { + if (!h.isValid()) { continue; } @@ -1295,12 +1287,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - px4_close(fd); + DevMgr::releaseHandle(h); continue; } /* if the calibration is for this device, apply it */ - if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct gyro_scale gscale = {}; (void)sprintf(str, "CAL_GYRO%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -1320,7 +1312,7 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + res = h.ioctl(GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); @@ -1337,8 +1329,6 @@ Sensors::parameter_update_poll(bool forced) if (config_ok) { gyro_count++; } - - px4_close(fd); } /* run through all accel sensors */ @@ -1347,9 +1337,10 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); + DevHandle h; + DevMgr::getHandle(str, h); - if (fd < 0) { + if (!h.isValid()) { continue; } @@ -1365,12 +1356,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - px4_close(fd); + DevMgr::releaseHandle(h); continue; } /* if the calibration is for this device, apply it */ - if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct accel_scale gscale = {}; (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -1390,7 +1381,7 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); @@ -1407,8 +1398,6 @@ Sensors::parameter_update_poll(bool forced) if (config_ok) { accel_count++; } - - px4_close(fd); } /* run through all mag sensors */ @@ -1423,9 +1412,10 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); + DevHandle h; + DevMgr::getHandle(str, h); - if (fd < 0) { + if (!h.isValid()) { /* the driver is not running, abort */ continue; } @@ -1444,12 +1434,12 @@ Sensors::parameter_update_poll(bool forced) (void)param_find(str); if (failed) { - px4_close(fd); + DevMgr::releaseHandle(h); continue; } /* if the calibration is for this device, apply it */ - if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct mag_scale gscale = {}; (void)sprintf(str, "CAL_MAG%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -1466,7 +1456,7 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ROT", i); - if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { + if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) { /* mag is internal */ _mag_rotation[s] = _board_rotation; /* reset param to -1 to indicate internal mag */ @@ -1520,7 +1510,7 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + res = h.ioctl(MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); @@ -1537,8 +1527,6 @@ Sensors::parameter_update_poll(bool forced) if (config_ok) { mag_count++; } - - px4_close(fd); } int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); From 2cf6d9f60576e911045edc6584d15caa75e5219a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 10:15:20 +0100 Subject: [PATCH 151/624] Fix relative path of inject XML file. By Yi Yi --- Tools/px_process_params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 3a11fc5c50..fcd5b994e9 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -130,7 +130,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml) + out = xmlout.XMLOutput(param_groups, args.board, os.path.join(args.src_path, args.inject_xml)) out.Save(args.xml) # Output to DokuWiki tables From c721d565a97c7f14a17211c715092192bc8e53e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 11:21:08 +0100 Subject: [PATCH 152/624] Upgrade XCode version to what is working locally for Travis CI --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index e6b5239344..3745cc867a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,7 +9,7 @@ matrix: - os: linux sudo: false - os: osx - osx_image: beta-xcode6.3 + osx_image: xcode7 sudo: true cache: From 3c1c2ba34e600cad3d463847322a9123ffda9eb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 11:57:01 +0100 Subject: [PATCH 153/624] Travis CI: Install CMake --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 3745cc867a..2aafa122e1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -60,11 +60,9 @@ before_install: elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 && brew update - && brew install astyle - && brew install gcc-arm-none-eabi + && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ; From 041d6e704d8a0ee815590ffd532aefa4c544b8cf Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 12:30:21 +0100 Subject: [PATCH 154/624] support for simplistic tailsitter --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d362e661b4..d704f43029 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe +Subproject commit d704f430297fffea766a42bda16436e3bd00d6ea From d86ca63c78c6b7fb0aa9b85ba33dbae83e38b907 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:27:23 +0100 Subject: [PATCH 155/624] added gazebo startup script for tailsitter --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 posix-configs/SITL/init/rcS_gazebo_tailsitter diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter new file mode 100644 index 0000000000..2bf7a78368 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -0,0 +1,64 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 20 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set VT_TYPE 0 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a From 6200cf6d869d1eb452519834f24557cfa346f4b3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:27:44 +0100 Subject: [PATCH 156/624] reduce gyro update rate to 400 Hz --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0ac023285c..c7f78dbca6 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -95,9 +95,9 @@ // Product Name Product Revision #define GYROSIMES_REV_C4 0x14 -#define GYROSIM_ACCEL_DEFAULT_RATE 1000 +#define GYROSIM_ACCEL_DEFAULT_RATE 400 -#define GYROSIM_GYRO_DEFAULT_RATE 1000 +#define GYROSIM_GYRO_DEFAULT_RATE 400 #define GYROSIM_ONE_G 9.80665f From 2f0b24feab7cac9c98ff80ff673868608c536cfd Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:28:47 +0100 Subject: [PATCH 157/624] cmake: replace vtol model with tailsitter model --- Makefile | 2 +- Tools/sitl_run.sh | 4 ++-- src/firmware/posix/CMakeLists.txt | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 096a058653..b0f49eda3e 100644 --- a/Makefile +++ b/Makefile @@ -184,7 +184,7 @@ clean: # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ - jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 8767f9f841..7dee02868a 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -88,9 +88,9 @@ then ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program} elif [ "$debugger" == "valgrind" ] then - valgrind ./mainapp ../../../../${rc_script}_${program} + valgrind ./mainapp ../../../../${rc_script}_${program}_${model} else - ./mainapp ../../../../${rc_script}_${program} + ./mainapp ../../../../${rc_script}_${program}_${model} fi if [ "$program" == "jmavsim" ] diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index fb7490ffe2..efc81d6335 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -34,7 +34,7 @@ add_dependencies(run_config mainapp) foreach(viewer none jmavsim gazebo) foreach(debugger none gdb lldb ddd valgrind) - foreach(model none iris vtol) + foreach(model none iris tailsitter) if (debugger STREQUAL "none") if (model STREQUAL "none") set(_targ_name "${viewer}") From 354e623318a0826398849eb7217b98c863bac6d9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:29:23 +0100 Subject: [PATCH 158/624] support for vtol simulation --- Tools/sitl_gazebo | 2 +- src/modules/simulator/simulator_mavlink.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d704f43029..d7b4fcfabd 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d704f430297fffea766a42bda16436e3bd00d6ea +Subproject commit d7b4fcfabde7d770cf0bf50287836d943eaf6714 diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ca45cdf2b8..2a6efc7fa7 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -80,7 +80,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) // for now we only support quadrotors unsigned n = 4; - if (_vehicle_status.is_rotary_wing) { + if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) { for (unsigned i = 0; i < 8; i++) { if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { @@ -113,7 +113,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1]; + actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; From cf338246fc971b78390cd659475432973cb857cd Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:56:46 +0100 Subject: [PATCH 159/624] renamed SITL startup script for iris --- posix-configs/SITL/init/{rcS_gazebo => rcS_gazebo_iris} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename posix-configs/SITL/init/{rcS_gazebo => rcS_gazebo_iris} (100%) diff --git a/posix-configs/SITL/init/rcS_gazebo b/posix-configs/SITL/init/rcS_gazebo_iris similarity index 100% rename from posix-configs/SITL/init/rcS_gazebo rename to posix-configs/SITL/init/rcS_gazebo_iris From 16aa852c53f8ba5f5e4d0a67377adee4d362230f Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 16:01:53 +0100 Subject: [PATCH 160/624] merged master into sitl_gazebo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d7b4fcfabd..7a2e18e786 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d7b4fcfabde7d770cf0bf50287836d943eaf6714 +Subproject commit 7a2e18e7869e7d7c47ac7a97d956a64751429140 From 0fa0654f5a5cab33b1bb6788d9b57827a66f9bc5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 16:08:53 +0100 Subject: [PATCH 161/624] sitl_gazebo cleanup --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 7a2e18e786..6072426bbc 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 7a2e18e7869e7d7c47ac7a97d956a64751429140 +Subproject commit 6072426bbca9407f2f26698d3e20870c1744b4fb From b8c40ecb6bc3522b65144c1f6894a66bc0045c51 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 18 Nov 2015 11:58:21 -0800 Subject: [PATCH 162/624] Enabled DriverFramework drivers for SITL build The code here works only for SITL at the present time. Signed-off-by: Mark Charlebois --- cmake/configs/posix_sitl_simple.cmake | 4 +- posix-configs/SITL/init/rcS_jmavsim | 2 +- src/drivers/drv_led.h | 1 + src/drivers/drv_rgbled.h | 2 + src/lib/DriverFramework | 2 +- src/modules/commander/PreflightCheck.cpp | 52 +- src/modules/commander/commander_helper.cpp | 67 +- src/modules/sensors/sensors.cpp | 4 +- .../posix/drivers/accelsim/accelsim.cpp | 3 + .../posix/drivers/ledsim/CMakeLists.txt | 42 + src/platforms/posix/drivers/ledsim/led.cpp | 141 +++ .../posix/drivers/rgbledsim/CMakeLists.txt | 43 + .../posix/drivers/rgbledsim/rgbled.cpp | 801 ++++++++++++++++++ .../posix/drivers/tonealrmsim/tone_alarm.cpp | 12 +- 14 files changed, 1099 insertions(+), 77 deletions(-) create mode 100644 src/platforms/posix/drivers/ledsim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/ledsim/led.cpp create mode 100644 src/platforms/posix/drivers/rgbledsim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/rgbledsim/rgbled.cpp diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 581b82e827..4f8514b010 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -3,11 +3,9 @@ include(posix/px4_impl_posix) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) set(config_module_list - drivers/led drivers/device drivers/boards/sitl drivers/pwm_out_sim - drivers/rgbled platforms/common platforms/posix/px4_layer platforms/posix/work_queue @@ -18,6 +16,8 @@ set(config_module_list platforms/posix/drivers/airspeedsim platforms/posix/drivers/barosim platforms/posix/drivers/gyrosim + platforms/posix/drivers/rgbledsim + platforms/posix/drivers/ledsim systemcmds/param systemcmds/mixer systemcmds/ver diff --git a/posix-configs/SITL/init/rcS_jmavsim b/posix-configs/SITL/init/rcS_jmavsim index 878eda3a6e..a893f0dc52 100644 --- a/posix-configs/SITL/init/rcS_jmavsim +++ b/posix-configs/SITL/init/rcS_jmavsim @@ -27,7 +27,7 @@ param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 param set COM_RC_IN_MODE 2 -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index dbcfde91fa..0ebf228a2e 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -43,6 +43,7 @@ #include #include +#define LED_BASE_DEVICE_PATH "/dev/led" #define LED0_DEVICE_PATH "/dev/led0" #define _LED_BASE 0x2800 diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 4e3de36abd..2194f8ed97 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -42,6 +42,8 @@ #include #include +#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" + /* more devices will be 1, 2, etc */ #define RGBLED0_DEVICE_PATH "/dev/rgbled0" diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 94abaa2067..2d8d3a0cfa 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 94abaa2067438c8b3c300c1bec05688cda5ff2e8 +Subproject commit 2d8d3a0cfaffdd833e63c6394eb1c8febb4f301a diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2a8a0695cf..c1cbfad910 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -69,21 +69,23 @@ #include "PreflightCheck.h" +#include "DevMgr.hpp" + +using namespace DriverFramework; + namespace Commander { -static int check_calibration(int fd, const char* param_template, int &devid); - -int check_calibration(int fd, const char* param_template, int &devid) +static int check_calibration(DevHandle &h, const char* param_template, int &devid) { bool calibration_found; /* new style: ask device for calibration state */ - int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + int ret = h.ioctl(SENSORIOCCALTEST, 0); calibration_found = (ret == OK); - devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + devid = h.ioctl(DEVIOCGDEVICEID, 0); char s[20]; int instance = 0; @@ -120,9 +122,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in char s[30]; sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); @@ -131,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in return false; } - int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); + int ret = check_calibration(h, "CAL_MAG%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -140,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in goto out; } - ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); + ret = h.ioctl(MAGIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -150,7 +153,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in } out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } @@ -160,9 +163,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, char s[30]; sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, O_RDONLY); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); @@ -171,7 +175,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, return false; } - int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); + int ret = check_calibration(h, "CAL_ACC%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -180,7 +184,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } - ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); + ret = h.ioctl(ACCELIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -215,7 +219,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, #endif out: +#ifdef __PX4_NUTTX px4_close(fd); +#endif return success; } @@ -225,9 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev char s[30]; sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); @@ -236,7 +243,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return false; } - int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); + int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -245,7 +252,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev goto out; } - ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); + ret = h.ioctl(GYROIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -255,7 +262,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev } out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } @@ -265,9 +272,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev char s[30]; sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); + DevHandle h; + DevMgr::getHandle(s, h); - if (fd < 0) { + if (!h.isValid()) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); @@ -290,7 +298,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev //out: - px4_close(fd); + DevMgr::releaseHandle(h); return success; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f411e1c128..34552c3d33 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -63,6 +63,9 @@ #include #include "commander_helper.h" +#include "DevMgr.hpp" + +using namespace DriverFramework; /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -136,7 +139,7 @@ int buzzer_init() buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { - warnx("Buzzer: px4_open fail\n"); + PX4_WARN("Buzzer: px4_open fail\n"); return ERROR; } @@ -264,30 +267,30 @@ int blink_msg_state() } } -static int leds = -1; -static int rgbleds = -1; +static DevHandle h_leds; +static DevHandle h_rgbleds; int led_init() { blink_msg_end = 0; /* first open normal LEDs */ - leds = px4_open(LED0_DEVICE_PATH, 0); + DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); - if (leds < 0) { - warnx("LED: px4_open fail\n"); + if (!h_leds.isValid()) { + PX4_WARN("LED: getHandle fail\n"); return ERROR; } /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ - (void)px4_ioctl(leds, LED_ON, LED_BLUE); + (void)h_leds.ioctl(LED_ON, LED_BLUE); /* switch blue off */ led_off(LED_BLUE); /* we consider the amber led mandatory */ - if (px4_ioctl(leds, LED_ON, LED_AMBER)) { - warnx("Amber LED: px4_ioctl fail\n"); + if (h_leds.ioctl(LED_ON, LED_AMBER)) { + PX4_WARN("Amber LED: ioctl fail\n"); return ERROR; } @@ -295,10 +298,11 @@ int led_init() led_off(LED_AMBER); /* then try RGB LEDs, this can fail on FMUv1*/ - rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0); + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h_rgbleds); - if (rgbleds < 0) { - warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); + if (!h_rgbleds.isValid()) { + PX4_WARN("No RGB LED found at " RGBLED0_DEVICE_PATH); } return 0; @@ -306,64 +310,41 @@ int led_init() void led_deinit() { - if (leds >= 0) { - px4_close(leds); - } - - if (rgbleds >= 0) { - px4_close(rgbleds); - } + DevMgr::releaseHandle(h_leds); + DevMgr::releaseHandle(h_rgbleds); } int led_toggle(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_TOGGLE, led); + return h_leds.ioctl(LED_TOGGLE, led); } int led_on(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_ON, led); + return h_leds.ioctl(LED_ON, led); } int led_off(int led) { - if (leds < 0) { - return leds; - } - return px4_ioctl(leds, LED_OFF, led); + return h_leds.ioctl(LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + h_rgbleds.ioctl(RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + h_rgbleds.ioctl(RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds < 0) { - return; - } - px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern); } unsigned battery_get_n_cells() { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1d66e45fa1..a82ffb8684 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -926,7 +926,7 @@ Sensors::accel_init() { DevHandle h_accel; DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel); - + if (!h_accel.isValid()) { warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); return ERROR; @@ -971,7 +971,7 @@ Sensors::mag_init() DevHandle h_mag; DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag); - + if (!h_mag.isValid()) { warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError()); return ERROR; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 1ba06148cb..99a54fc85f 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -349,16 +349,19 @@ ACCELSIM::~ACCELSIM() int ACCELSIM::init() { + PX4_INFO("ACCELSIM::init"); int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; + PX4_INFO("ACCELSIM::init before VirtDevObj::init()"); /* do SIM init first */ if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } + PX4_INFO("ACCELSIM::init after VirtDevObj::init()"); /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); diff --git a/src/platforms/posix/drivers/ledsim/CMakeLists.txt b/src/platforms/posix/drivers/ledsim/CMakeLists.txt new file mode 100644 index 0000000000..0f20b87988 --- /dev/null +++ b/src/platforms/posix/drivers/ledsim/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 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 +# 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. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__ledsim + COMPILE_FLAGS + -Os + SRCS + led.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/ledsim/led.cpp b/src/platforms/posix/drivers/ledsim/led.cpp new file mode 100644 index 0000000000..f1d1021e65 --- /dev/null +++ b/src/platforms/posix/drivers/ledsim/led.cpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.cpp + * + * LED driver. + */ + +#include +#include +#include + +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + +#define DEVICE_DEBUG PX4_INFO + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +class LED : public VirtDevObj +{ +public: + LED(); + virtual ~LED(); + + virtual int init(); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); + +protected: + virtual void _measure() {} +}; + +LED::LED() : + VirtDevObj("led", "/dev/ledsim", LED_BASE_DEVICE_PATH, 0) +{ + // force immediate init/device registration + init(); +} + +LED::~LED() +{ +} + +int +LED::init() +{ + DEVICE_DEBUG("LED::init"); + int ret = VirtDevObj::init(); + if (ret == 0) { + led_init(); + } + + return ret; +} + +int +LED::devIOCTL(unsigned long cmd, unsigned long arg) +{ + int result = OK; + + switch (cmd) { + case LED_ON: + led_on(arg); + break; + + case LED_OFF: + led_off(arg); + break; + + case LED_TOGGLE: + led_toggle(arg); + break; + + + default: + result = VirtDevObj::devIOCTL(cmd, arg); + } + + return result; +} + +namespace +{ +LED *gLED; +} + +void +drv_led_start(void) +{ + if (gLED == nullptr) { + gLED = new LED; + + if (gLED != nullptr) { + gLED->init(); + } + } +} diff --git a/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt b/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt new file mode 100644 index 0000000000..edb2392a3a --- /dev/null +++ b/src/platforms/posix/drivers/rgbledsim/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 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 +# 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. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__rgbledsim + MAIN rgbledsim + COMPILE_FLAGS + -Os + SRCS + rgbled.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp new file mode 100644 index 0000000000..bf839fbb52 --- /dev/null +++ b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp @@ -0,0 +1,801 @@ +/**************************************************************************** + * + * 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 + * 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 rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + +#define DEVICE_LOG PX4_INFO +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 + +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class RGBLEDSIM : public VirtDevObj +{ +public: + RGBLEDSIM(int bus, int rgbled); + virtual ~RGBLEDSIM(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); + + // Don't use the default periodic callback + virtual void _measure() {} + +private: + work_s _work; + + rgbled_mode_t _mode; + rgbled_pattern_t _pattern; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + float _brightness; + float _max_brightness; + + bool _running; + int _led_interval; + bool _should_run; + int _counter; + int _param_sub; + + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); + void update_params(); + + /** + * Perform an I2C transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + +}; + +/* for now, we only support one RGBLEDSIM */ +namespace +{ +RGBLEDSIM *g_rgbled = nullptr; +} + +void rgbled_usage(); + +extern "C" __EXPORT int rgbledsim_main(int argc, char *argv[]); + +RGBLEDSIM::RGBLEDSIM(int bus, int rgbled) : + VirtDevObj("rgbled", "/dev/rgbledsim", RGBLED_BASE_DEVICE_PATH, 0), + _mode(RGBLED_MODE_OFF), + _r(0), + _g(0), + _b(0), + _brightness(1.0f), + _max_brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0), + _param_sub(-1) +{ + memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); +} + +RGBLEDSIM::~RGBLEDSIM() +{ +} + +int +RGBLEDSIM::init() +{ + int ret; + ret = VirtDevObj::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + send_led_rgb(); + + return OK; +} + +int +RGBLEDSIM::probe() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLEDSIM is on the bus. + */ + + if ((ret = get(on, powersave, r, g, b)) != OK || + (ret = send_led_enable(false) != OK) || + (ret = send_led_enable(false) != OK)) { + return ret; + } + + return ret; +} + +int +RGBLEDSIM::info() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + ret = get(on, powersave, r, g, b); + + if (ret == OK) { + /* we don't care about power-save mode */ + DEVICE_LOG("state: %s", on ? "ON" : "OFF"); + DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + + } else { + PX4_WARN("failed to read led"); + } + + return ret; +} + +int +RGBLEDSIM::devIOCTL(unsigned long cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case RGBLED_SET_RGB: + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); + return OK; + + case RGBLED_SET_COLOR: + /* set the specified color name */ + set_color((rgbled_color_t)arg); + send_led_rgb(); + return OK; + + case RGBLED_SET_MODE: + /* set the specified mode */ + set_mode((rgbled_mode_t)arg); + return OK; + + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t *)arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = VirtDevObj::devIOCTL(cmd, arg); + break; + } + + return ret; +} + + +void +RGBLEDSIM::led_trampoline(void *arg) +{ + RGBLEDSIM *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLEDSIM::led() +{ + if (!_should_run) { + _running = false; + return; + } + + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + if (_param_sub >= 0) { + bool updated = false; + orb_check(_param_sub, &updated); + + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + + switch (_mode) { + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) { + _counter = 0; + } + + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) { + _counter = 0; + } + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); + send_led_rgb(); + break; + + case RGBLED_MODE_PATTERN: + + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) { + _counter = 0; + } + + set_color(_pattern.color[_counter]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; + } + + _counter++; +} + +/** + * Parse color constant and set _r _g _b values + */ +void +RGBLEDSIM::set_color(rgbled_color_t color) +{ + switch (color) { + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 200; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 80; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + PX4_WARN("color unknown"); + break; + } +} + +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ +void +RGBLEDSIM::set_mode(rgbled_mode_t mode) +{ + if (mode != _mode) { + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + send_led_enable(false); + break; + + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; + + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BREATHE: + _should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; + + case RGBLED_MODE_PATTERN: + _should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; + + default: + PX4_WARN("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLEDSIM::led_trampoline, this, 1); + } + + } +} + +/** + * Set pattern for PATTERN mode, but don't change current mode + */ +void +RGBLEDSIM::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +RGBLEDSIM::send_led_enable(bool enable) +{ + uint8_t settings_byte = 0; + + if (enable) { + settings_byte |= SETTING_ENABLE; + } + + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLEDSIM::send_led_rgb() +{ + /* To scale from 0..255 -> 0..15 shift right by 4 bits */ + const uint8_t msg[6] = { + SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness * _max_brightness + 0.5f) + }; + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLEDSIM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2]; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + powersave = !(result[0] & SETTING_NOT_POWERSAVE); + /* XXX check, looks wrong */ + r = (result[0] & 0x0f) << 4; + g = (result[1] & 0xf0); + b = (result[1] & 0x0f) << 4; + } + + return ret; +} + +void +RGBLEDSIM::update_params() +{ + int32_t maxbrt = 15; + param_get(param_find("LED_RGB_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 15 ? 15 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + // A minimum of 2 "on" steps is required for breathe effect + if (maxbrt == 1) { + maxbrt = 2; + } + + _max_brightness = maxbrt / 15.0f; +} + +void +rgbled_usage() +{ + PX4_WARN("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); + PX4_WARN("options:"); + PX4_WARN(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + PX4_WARN(" -a addr (0x%x)", ADDR); +} + +int +rgbledsim_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(myoptarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(myoptarg, NULL, 0); + break; + + default: + rgbled_usage(); + return 1; + } + } + + if (myoptind >= argc) { + rgbled_usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + int ret; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) { + PX4_WARN("already started"); + return 1; + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLEDSIM(PX4_I2C_BUS_EXPANSION, rgbledadr); + + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + + if (g_rgbled == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLEDSIM(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) { + PX4_WARN("new failed"); + return 1; + } + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + } + + return 0; + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + PX4_WARN("not started"); + rgbled_usage(); + return 1; + } + + if (!strcmp(verb, "test")) { + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); + + if (!h.isValid()) { + PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern + }; + + ret = h.ioctl(RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + + DevMgr::releaseHandle(h); + return ret; + } + + if (!strcmp(verb, "info")) { + g_rgbled->info(); + return 0; + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); + + if (!h.isValid()) { + PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + DevMgr::releaseHandle(h); + + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + return 0; + } + + return ret; + } + + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + PX4_WARN("Usage: rgbled rgb "); + return 1; + } + + DevHandle h; + DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); + + if (!h.isValid()) { + PX4_WARN("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + rgbled_rgbset_t v; + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); + ret = h.ioctl(RGBLED_SET_RGB, (unsigned long)&v); + ret = h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + DevMgr::releaseHandle(h); + return ret; + } + + rgbled_usage(); + return 1; +} + +int RGBLEDSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + return 0; +} diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 6adffb9bf9..a30c07303d 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -741,17 +741,17 @@ ToneAlarm *g_dev; int play_tune(unsigned tune) { - int fd, ret; + int ret; - fd = px4_open(TONEALARM0_DEVICE_PATH, 0); + DevHandle h; + DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h); - if (fd < 0) { - PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + if (!h.isValid()) { + PX4_WARN("Error: failed to open %s (%d)", TONEALARM0_DEVICE_PATH, h.getError()); return 1; } - ret = px4_ioctl(fd, TONE_SET_ALARM, tune); - px4_close(fd); + ret = h.ioctl(TONE_SET_ALARM, tune); if (ret != 0) { PX4_WARN("TONE_SET_ALARM"); From af155b8e5938227c3e169d618882d2976d161167 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 18 Nov 2015 13:02:39 -0800 Subject: [PATCH 163/624] Fixed SITL build The SITL build seems to run correctly now Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- src/modules/commander/commander_helper.cpp | 13 ++--- .../posix/drivers/accelsim/accelsim.cpp | 3 -- src/platforms/posix/drivers/gpssim/gpssim.cpp | 52 +++++++++++-------- 4 files changed, 38 insertions(+), 32 deletions(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 2d8d3a0cfa..737d2cbf2c 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 2d8d3a0cfaffdd833e63c6394eb1c8febb4f301a +Subproject commit 737d2cbf2c94210b964cf00d55903f1b9c52b2f6 diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 34552c3d33..a7ca5b231f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -96,7 +96,7 @@ bool is_vtol(const struct vehicle_status_s * current_status) { current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR); } -static int buzzer = -1; +static DevHandle h_buzzer; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end @@ -136,9 +136,10 @@ int buzzer_init() tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; - buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); + DevHandle h_buzzer; + DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h_buzzer); - if (buzzer < 0) { + if (!h_buzzer.isValid()) { PX4_WARN("Buzzer: px4_open fail\n"); return ERROR; } @@ -148,12 +149,12 @@ int buzzer_init() void buzzer_deinit() { - px4_close(buzzer); + DevMgr::releaseHandle(h_buzzer); } void set_tune_override(int tune) { - px4_ioctl(buzzer, TONE_SET_ALARM, tune); + h_buzzer.ioctl(TONE_SET_ALARM, tune); } void set_tune(int tune) @@ -164,7 +165,7 @@ void set_tune(int tune) if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { - px4_ioctl(buzzer, TONE_SET_ALARM, tune); + h_buzzer.ioctl(TONE_SET_ALARM, tune); } tune_current = tune; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 99a54fc85f..1ba06148cb 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -349,19 +349,16 @@ ACCELSIM::~ACCELSIM() int ACCELSIM::init() { - PX4_INFO("ACCELSIM::init"); int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; - PX4_INFO("ACCELSIM::init before VirtDevObj::init()"); /* do SIM init first */ if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } - PX4_INFO("ACCELSIM::init after VirtDevObj::init()"); /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 889d793486..20e929ce88 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -62,6 +62,11 @@ #include +#include "DevMgr.hpp" +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + #define GPS_DRIVER_MODE_UBX_SIM #define GPSSIM_DEVICE_PATH "/dev/gpssim" @@ -82,7 +87,7 @@ public: }; -class GPSSIM : public device::VDev +class GPSSIM : public VirtDevObj { public: GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info); @@ -90,13 +95,16 @@ public: virtual int init(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual int devIOCTL(unsigned long cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ void print_info(); +protected: + virtual void _measure() {} + private: bool _task_should_exit; ///< flag to make the main worker task exit @@ -115,6 +123,7 @@ private: orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output + SyncObj _sync; /** * Try to configure the GPS, handle outgoing communication to the GPS @@ -160,7 +169,7 @@ GPSSIM *g_dev = nullptr; GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : - VDev("gps", GPSSIM_DEVICE_PATH), + VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 0), _task_should_exit(false), //_healthy(false), //_mode_changed(false), @@ -188,8 +197,6 @@ GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : _p_report_sat_info = &_Sat_Info->_data; memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); } - - _debug_enabled = true; } GPSSIM::~GPSSIM() @@ -217,7 +224,7 @@ GPSSIM::init() int ret = ERROR; /* do regular cdev init */ - if (VDev::init() != OK) { + if (VirtDevObj::init() != OK) { goto out; } @@ -236,9 +243,9 @@ out: } int -GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GPSSIM::devIOCTL(unsigned long cmd, unsigned long arg) { - lock(); + _sync.lock(); int ret = OK; @@ -249,11 +256,11 @@ GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to parent if no one wants it */ - ret = VDev::ioctl(filp, cmd, arg); + ret = VirtDevObj::devIOCTL(cmd, arg); break; } - unlock(); + _sync.unlock(); return ret; } @@ -319,7 +326,7 @@ GPSSIM::task_main() //no time and satellite information simulated - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -339,7 +346,7 @@ GPSSIM::task_main() _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.timestamp_time = hrt_absolute_time(); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -354,7 +361,7 @@ GPSSIM::task_main() while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { /* opportunistic publishing - else invalid data would end up on the bus */ - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); if (_p_report_sat_info) { @@ -368,7 +375,8 @@ GPSSIM::task_main() } } - lock(); + // FIXME - if ioctl is called then it will deadlock + _sync.lock(); } } @@ -440,7 +448,7 @@ void info(); void start(const char *uart_path, bool fake_gps, bool enable_sat_info) { - int fd; + DevHandle h; /* create the driver */ g_dev = new GPSSIM(uart_path, fake_gps, enable_sat_info); @@ -454,10 +462,10 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY); + DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); - if (fd < 0) { - PX4_ERR("open: %s\n", GPS0_DEVICE_PATH); + if (!h.isValid()) { + PX4_ERR("getHandle failed: %s", GPS0_DEVICE_PATH); goto fail; } @@ -500,13 +508,13 @@ test() void reset() { - int fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { + DevHandle h; + DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); + if (!h.isValid()) { PX4_ERR("failed "); } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("reset failed"); } } From f10bce2a631776412ad3badfbe840c71ee942277 Mon Sep 17 00:00:00 2001 From: sanderux Date: Thu, 19 Nov 2015 04:56:33 +0100 Subject: [PATCH 164/624] Added V-Tail VTOL config and mixer --- .../init.d/13007_vtol_AAVVT_quad | 51 ++++++++++++++++ ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix | 59 +++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix diff --git a/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad b/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad new file mode 100644 index 0000000000..2d48670e71 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad @@ -0,0 +1,51 @@ +#!nsh +# +# @name Generic AAVVT v-tail plane airframe with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Sander Smeets +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 + +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AAVVT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix new file mode 100644 index 0000000000..86581585c2 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix @@ -0,0 +1,59 @@ +Aileron/v-tail/throttle VTOL mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator) and throttle using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU +AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 6 -10000 -10000 0 -10000 10000 + + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 2 -7000 -7000 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 2 -7000 -7000 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 + From 52957ab0ccbdcde4f92aefc080bd0b156644b362 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 18 Nov 2015 20:14:48 -0800 Subject: [PATCH 165/624] Don't add DriverFramework dir for NuttX I am unable to get the nuttx build dependencies set up so that it builds the export dir before it builds df_driver_framework. Signed-off-by: Mark Charlebois --- CMakeLists.txt | 5 ++--- cmake/nuttx/px4_impl_nuttx.cmake | 5 ++++- cmake/qurt/px4_impl_qurt.cmake | 2 +- src/firmware/nuttx/CMakeLists.txt | 2 +- src/lib/DriverFramework | 2 +- src/modules/commander/PreflightCheck.cpp | 6 ++---- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 64c73efa67..56aacd2bbe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -335,10 +335,9 @@ foreach(module ${config_module_list}) endforeach() add_subdirectory(src/firmware/${OS}) -add_subdirectory(src/lib/DriverFramework) -if("${OS}" STREQUAL "nuttx") - add_dependencies(df_driver_framework nuttx_export_${BOARD}) +if (NOT "${OS}" STREQUAL "nuttx") + add_subdirectory(src/lib/DriverFramework) endif() if (config_io_board) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 14bd2ee3d6..756c42d635 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -423,6 +423,7 @@ function(px4_os_add_flags) ${nuttx_export_dir}/include/cxx ${nuttx_export_dir}/arch/chip ${nuttx_export_dir}/arch/common + src/lib/DriverFramework/framework/include ) set(added_link_dirs ${nuttx_export_dir}/libs @@ -440,7 +441,7 @@ function(px4_os_add_flags) -nostdlib ) - set(added_exe_linker_flags) # none currently + set(added_exe_linker_flags -lstdc++) # none currently set(cpu_flags) if (${BOARD} STREQUAL "px4fmu-v1") @@ -490,6 +491,8 @@ function(px4_os_add_flags) #message(STATUS "nuttx: set(${${var}} ${${${var}}} ${added_${lower_var}} PARENT_SCOPE)") endforeach() + set(DF_TARGET "nuttx" PARENT_SCOPE) + endfunction() #============================================================================= diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 87c71b1ecd..01025ce270 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -187,7 +187,7 @@ function(px4_os_add_flags) set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "") - set(DF_TARGET "qurt") + set(DF_TARGET "qurt" PARENT_SCOPE) # output foreach(var ${inout_vars}) diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index be6f9b1062..992987f9de 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -13,7 +13,7 @@ add_executable(firmware_nuttx set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export) set(link_libs - romfs apps nuttx m gcc + romfs apps nuttx m gcc df_driver_framework ) if(NOT ${BOARD} STREQUAL "sim") diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 737d2cbf2c..a759ad3142 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 737d2cbf2c94210b964cf00d55903f1b9c52b2f6 +Subproject commit a759ad3142b3b30d4e3a3157ec684966f7dcc83c diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index c1cbfad910..d542707ba4 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -197,7 +197,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (dynamic) { /* check measurement result range */ struct accel_report acc; - ret = px4_read(fd, &acc, sizeof(acc)); + ret = h.read(&acc, sizeof(acc)); if (ret == sizeof(acc)) { /* evaluate values */ @@ -219,9 +219,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, #endif out: -#ifdef __PX4_NUTTX - px4_close(fd); -#endif + DevMgr::releaseHandle(h); return success; } From 7111e9d4c9a5f6f91cb26d7e9d4a08fef9bda77c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 18 Nov 2015 20:18:22 -0800 Subject: [PATCH 166/624] Removed -lstdc++ for nuttx build Signed-off-by: Mark Charlebois --- cmake/nuttx/px4_impl_nuttx.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 756c42d635..47a3ea8c5b 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -441,7 +441,7 @@ function(px4_os_add_flags) -nostdlib ) - set(added_exe_linker_flags -lstdc++) # none currently + set(added_exe_linker_flags) # none currently set(cpu_flags) if (${BOARD} STREQUAL "px4fmu-v1") From 169a873457abf3aa3084762918df7476c9ac287b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 18 Nov 2015 20:33:38 -0800 Subject: [PATCH 167/624] Fix compiler complaint about bogus uninitialized variable Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/rgbledsim/rgbled.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp index bf839fbb52..27518f0116 100644 --- a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp +++ b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp @@ -588,7 +588,7 @@ RGBLEDSIM::send_led_rgb() int RGBLEDSIM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) { - uint8_t result[2]; + uint8_t result[2] = {0, 0}; int ret; ret = transfer(nullptr, 0, &result[0], 2); From 054b5420a60d41b79fddf14180608e8c8ba3bb3c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 18 Nov 2015 20:35:16 -0800 Subject: [PATCH 168/624] Code style fixes Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/gpssim/gpssim.cpp | 1 + src/platforms/posix/drivers/ledsim/led.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 20e929ce88..3241c6c30d 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -510,6 +510,7 @@ reset() { DevHandle h; DevMgr::getHandle(GPSSIM_DEVICE_PATH, h); + if (!h.isValid()) { PX4_ERR("failed "); } diff --git a/src/platforms/posix/drivers/ledsim/led.cpp b/src/platforms/posix/drivers/ledsim/led.cpp index f1d1021e65..161514cd93 100644 --- a/src/platforms/posix/drivers/ledsim/led.cpp +++ b/src/platforms/posix/drivers/ledsim/led.cpp @@ -90,6 +90,7 @@ LED::init() { DEVICE_DEBUG("LED::init"); int ret = VirtDevObj::init(); + if (ret == 0) { led_init(); } From 5103ba1c23f0b266c070b34b16fc8bf5d2c1f560 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 19 Nov 2015 09:09:51 +0100 Subject: [PATCH 169/624] implemented better shell for SITL --- src/platforms/posix/main.cpp | 80 ++++++++++++++++++++++++++++++++---- 1 file changed, 72 insertions(+), 8 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1b22627721..149ae6c185 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -35,6 +35,7 @@ * Basic shell to execute builtin "apps" * * @author Mark Charlebois + * @auther Roman Bapst */ #include @@ -43,6 +44,7 @@ #include #include #include +#include namespace px4 { @@ -56,6 +58,8 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" #include "px4_middleware.h" +#define CMD_BUFF_SIZE 100 + static bool _ExitFlag = false; extern "C" { void _SigIntHandler(int sig_num); @@ -203,22 +207,82 @@ int main(int argc, char **argv) } if (!daemon_mode) { - string mystr; + string mystr = ""; + string string_buffer[CMD_BUFF_SIZE]; + int buf_ptr_write = 0; + int buf_ptr_read = 0; print_prompt(); + // change input mode so that we can manage shell + struct termios term; + tcgetattr(0, &term); + term.c_lflag &= ~ICANON; + term.c_lflag &= ~ECHO; + tcsetattr(0, TCSANOW, &term); + setbuf(stdin, NULL); + while (!_ExitFlag) { - struct pollfd fds; - int ret; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 100); + char c = getchar(); - if (ret > 0) { - getline(cin, mystr); + switch (c) { + case 127: // backslash + if (mystr.length() > 0) { + mystr.pop_back(); + printf("%c[2K", 27); // clear line + cout << (char)13; + print_prompt(); + cout << mystr; + } + + break; + + case'\n': // user hit enter + if (buf_ptr_write == CMD_BUFF_SIZE) { + buf_ptr_write = 0; + } + + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; process_line(mystr, !daemon_mode); mystr = ""; + buf_ptr_read = buf_ptr_write; + break; + + case '\033': { // arrow keys + c = getchar(); // skip first one, does not have the info + c = getchar(); + + if (c == 'A') { + buf_ptr_read--; + + } else if (c == 'B') { + if (buf_ptr_read < buf_ptr_write) { + buf_ptr_read++; + } + + } else { + // TODO: Support editing current line + } + + if (buf_ptr_read < 0) { + buf_ptr_read = 0; + } + + string saved_cmd = string_buffer[buf_ptr_read]; + printf("%c[2K", 27); + cout << (char)13; + mystr = saved_cmd; + print_prompt(); + cout << mystr; + break; + } + + default: // any other input + cout << c; + mystr += c; + break; } } From a8ab52b444c711050f5abeb59c23cb568a367f5e Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 19 Nov 2015 09:13:44 +0100 Subject: [PATCH 170/624] topic_listener: allow listening to actuator control groups --- Tools/generate_listener.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 625eda7037..e8cd1ae114 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -17,7 +17,7 @@ for index,m in enumerate(raw_messages): temp_list_floats = [] temp_list_uint64 = [] temp_list_bool = [] - if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + if("pwm_input" not in m and "position_setpoint" not in m): temp_list = [] f = open(m,'r') for line in f.readlines(): @@ -55,9 +55,11 @@ for index,m in enumerate(raw_messages): f.close() (m_head, m_tail) = os.path.split(m) - messages.append(m_tail.split('.')[0]) + message = m_tail.split('.')[0] + if message != "actuator_controls": + messages.append(message) + message_elements.append(temp_list) #messages.append(m.split('/')[-1].split('.')[0]) - message_elements.append(temp_list) num_messages = len(messages); From 020844e9e9ca96e17f05f1e041cb2b42f5c8fe75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 15:53:08 +0100 Subject: [PATCH 171/624] Gyro: also output to console --- src/modules/commander/gyro_calibration.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 54866d8cb3..c65779c051 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); } } @@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (poll_errcount > 1000) { - mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); + mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); return calibrate_return_error; } } for (unsigned s = 0; s < max_gyros; s++) { if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { - mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) + mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } @@ -155,7 +155,7 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; gyro_worker_data_t worker_data = {}; - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_fd = mavlink_fd; @@ -179,7 +179,7 @@ int do_gyro_calibration(int mavlink_fd) (void)sprintf(str, "CAL_GYRO%u_ID", s); res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); if (res != OK) { - mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); return ERROR; } @@ -193,7 +193,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); return ERROR; } } @@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); } } } @@ -325,7 +325,7 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } } @@ -333,9 +333,9 @@ int do_gyro_calibration(int mavlink_fd) usleep(200000); if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ From 8b70bd248742e2ca291c17c98150d34980957ddb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 15:53:32 +0100 Subject: [PATCH 172/624] Commander: Increase stack for commandline calibration --- src/modules/commander/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c1ec35b5ee..c8ede875b6 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -37,7 +37,7 @@ endif() px4_add_module( MODULE modules__commander MAIN commander - STACK 1200 + STACK 3000 COMPILE_FLAGS ${MODULE_CFLAGS} -Os From c77c0f927b279fe0fa5ef8cbb39762454bfdbb37 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 15:54:57 +0100 Subject: [PATCH 173/624] Gyro cal: be more forgiving in calibration offset --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index c65779c051..2048ac03ea 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -238,7 +238,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.0055f; + const float maxoff = 0.01f; if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || From ca125bccbd5cefc6682e8d988d3927806faeae91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 16:03:36 +0100 Subject: [PATCH 174/624] Update error message --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2048ac03ea..1fcd48b088 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1); } } } From 173edcef63b09fd0d334f57298368b0f24824cb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 16:33:56 +0100 Subject: [PATCH 175/624] Commander: Fix reporting in presence of no telemetry link --- src/modules/commander/commander.cpp | 4 ++-- src/modules/commander/state_machine_helper.cpp | 6 +++--- src/modules/commander/state_machine_helper.h | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 83e79b77fc..d416358eeb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -382,9 +382,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = 0; - checkres = preflight_check(&status, mavlink_fd_local, false); + checkres = preflight_check(&status, mavlink_fd_local, false, true); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, mavlink_fd_local, true); + checkres = preflight_check(&status, mavlink_fd_local, true, true); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); px4_close(mavlink_fd_local); return 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bcdb5d3f9d..295ea3dcd2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -743,12 +743,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm) +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ - bool reportFailures = false; - reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout; + bool reportFailures = force_report || (!status->data_link_lost && + !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout); bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bdb7b13f28..19b9394e3d 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm); +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */ From f2b988dcaa9781c2a397fbcfe8d2e2540891639a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 17:48:39 +0100 Subject: [PATCH 176/624] Fix error reporting logic --- src/modules/commander/commander.cpp | 10 +++------- src/modules/commander/state_machine_helper.cpp | 17 +++++++++++------ src/modules/commander/state_machine_helper.h | 2 +- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d416358eeb..e89626300e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2195,9 +2195,9 @@ int commander_thread_main(int argc, char *argv[]) /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); - } else if (telemetry_last_dl_loss[i] == 0){ - /* do not report a new data link in order to not spam the user */ - status.data_link_found_new = true; + } else if (telemetry_last_dl_loss[i] == 0) { + /* new link */ + status.condition_system_prearm_error_reported = false; status_changed = true; } @@ -2208,10 +2208,6 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; - if(status.data_link_found_new) { - status.data_link_found_new = false; - status_changed = true; - } } } else { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 295ea3dcd2..c2f7126e85 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -123,6 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); } /* re-run the pre-flight check as long as sensors are failing */ @@ -130,6 +131,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); status->condition_system_sensors_initialized = !prearm_ret; } @@ -272,10 +274,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition) { status->condition_system_prearm_error_reported = false; } - if(status->data_link_found_new == true) - { - status->condition_system_prearm_error_reported = false; - } /* end of atomic state update */ #ifdef __PX4_NUTTX @@ -743,7 +741,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ @@ -761,7 +759,14 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; - if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + } + + /* report once, then set the flag */ + if (mavlink_fd >= 0 && reportFailures && !preflight_ok) { + status->condition_system_prearm_error_reported = true; } return !preflight_ok; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 19b9394e3d..7c209cf07f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */ From 7a5391a7233e8245d123ba966e79ba9202a1e789 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 18:28:16 +0100 Subject: [PATCH 177/624] Commander: Fix preflight check reporting and simplify logic --- src/modules/commander/PreflightCheck.cpp | 30 ++++++++++++++----- src/modules/commander/commander.cpp | 8 +++-- .../commander/state_machine_helper.cpp | 9 ++++-- 3 files changed, 33 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 8093ef56ba..54847fae28 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -124,8 +124,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } } return false; @@ -134,8 +136,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -143,8 +147,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -164,8 +170,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } } return false; @@ -174,8 +182,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -183,8 +193,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -200,7 +212,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + } /* this is frickin' fatal */ success = false; goto out; @@ -506,7 +520,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { - if(!gnssCheck(mavlink_fd, reportFailures)) { + if (!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e89626300e..02b919b524 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1034,7 +1034,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - status.condition_system_prearm_error_reported = true; + status.condition_system_prearm_error_reported = false; status.condition_system_hotplug_timeout = false; status.counter++; @@ -2197,10 +2197,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); } else if (telemetry_last_dl_loss[i] == 0) { /* new link */ - status.condition_system_prearm_error_reported = false; - status_changed = true; } + /* got link again or new */ + status.condition_system_prearm_error_reported = false; + status_changed = true; + telemetry_lost[i] = false; have_link = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c2f7126e85..478bad7595 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -271,6 +271,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + status->arming_state != vehicle_status_s::ARMING_STATE_INIT && valid_transition) { status->condition_system_prearm_error_reported = false; } @@ -745,8 +746,8 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool { /* */ - bool reportFailures = force_report || (!status->data_link_lost && - !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout); + bool reportFailures = force_report || (!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout); bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not @@ -755,7 +756,9 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool checkAirspeed = true; } - bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), + !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; From c4438bf50ac64f50da02ee8c59019cd7a659e6ef Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Nov 2015 09:35:18 -0800 Subject: [PATCH 178/624] Fix to move DriverFramework includes to common Nuttx now gets to link phase. But there are two unresolved symbols: arm-none-eabi/include/c++/4.7.4/bits/basic_string.h:1128: undefined reference to `std::string::assign(char const*, unsigned int)' arm-none-eabi/include/c++/4.7.4/bits/stl_list.h:1534: undefined reference to `std::__detail::_List_node_base::_M_hook(std::__detail::_List_node_base*)' Signed-off-by: Mark Charlebois --- CMakeLists.txt | 3 ++- cmake/common/px4_base.cmake | 1 + cmake/nuttx/px4_impl_nuttx.cmake | 1 - cmake/posix/px4_impl_posix.cmake | 1 - cmake/qurt/px4_impl_qurt.cmake | 1 - src/lib/DriverFramework | 2 +- 6 files changed, 4 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 56aacd2bbe..607fafb9ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -336,8 +336,9 @@ endforeach() add_subdirectory(src/firmware/${OS}) +add_subdirectory(src/lib/DriverFramework/framework/src) +#add_dependencies(df_driver_framework nuttx_export_${CONFIG}.stamp) if (NOT "${OS}" STREQUAL "nuttx") - add_subdirectory(src/lib/DriverFramework) endif() if (config_io_board) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 8a3a844886..fa34b1584d 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -619,6 +619,7 @@ function(px4_add_common_flags) ${CMAKE_BINARY_DIR}/src/modules/px4_messages ${CMAKE_BINARY_DIR}/src/modules ${CMAKE_SOURCE_DIR}/mavlink/include/mavlink + ${CMAKE_SOURCE_DIR}/src/lib/DriverFramework/framework/include ) list(APPEND added_include_dirs diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 47a3ea8c5b..df9a74e1c7 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -423,7 +423,6 @@ function(px4_os_add_flags) ${nuttx_export_dir}/include/cxx ${nuttx_export_dir}/arch/chip ${nuttx_export_dir}/arch/common - src/lib/DriverFramework/framework/include ) set(added_link_dirs ${nuttx_export_dir}/libs diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index ca6033448d..4c2dfbb709 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -164,7 +164,6 @@ function(px4_os_add_flags) src/lib/eigen src/platforms/posix/include mavlink/include/mavlink - src/lib/DriverFramework/framework/include ) if(UNIX AND APPLE) diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 01025ce270..5c40dfebd7 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -165,7 +165,6 @@ function(px4_os_add_flags) ${DSPAL_ROOT}/sys/sys ${DSPAL_ROOT}/mpu_spi/inc ${DSPAL_ROOT}/uart_esc/inc - src/lib/DriverFramework/framework/include src/platforms/qurt/include src/platforms/posix/include ) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index a759ad3142..10bc08d31f 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit a759ad3142b3b30d4e3a3157ec684966f7dcc83c +Subproject commit 10bc08d31fe0cddb997b3349f34a7ea38c5c7b9d From e326e190cb85d142736aaa70dae66c48841322a9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 07:26:59 -1000 Subject: [PATCH 179/624] BUGFIX: Multiple PREFLIGHT FAIL: xxx messages - multiline macros not wrapped in do/while --- src/include/mavlink/mavlink_log.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 13a6b16b34..f0141d8a17 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -106,10 +106,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_emergency(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink critical message and print to console. @@ -117,10 +117,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_critical(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink emergency message and print to console. @@ -128,10 +128,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_info(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; From 2ffccc5c9f1667aae5a2908e66e8a39df7faa15e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 07:50:45 -1000 Subject: [PATCH 180/624] BUGFIX:CLI "commander calibrate acel" resulted in hardfault --- src/modules/commander/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c8ede875b6..4239797e0a 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -37,7 +37,7 @@ endif() px4_add_module( MODULE modules__commander MAIN commander - STACK 3000 + STACK 4096 COMPILE_FLAGS ${MODULE_CFLAGS} -Os From c51f414b222c95189e0e472fa4b49a8ad7d5096c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 19:09:42 +0100 Subject: [PATCH 181/624] Vehicle status remove unused field --- msg/vehicle_status.msg | 1 - 1 file changed, 1 deletion(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c91672c0be..60ab34e90d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -141,7 +141,6 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. -bool data_link_found_new # new datalink to GCS found bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events From d5c89c53923d0eacfcc1614d07887b7db77e383b Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 11:43:11 -1000 Subject: [PATCH 182/624] BUGFIX:Take 2! CLI "commander calibrate acel" resulted in hardfault Processes: 20 total, 4 running, 16 sleeping CPU usage: 65.19% tasks, 0.56% sched, 34.24% idle Uptime: 387.573s total, 135.045s idle PID COMMAND CPU(ms) CPU(%) USED/STACK PRIO(BASE) STATE 0 Idle Task 135045 34.242 0/ 0 0 ( 0) READY 1 hpwork 7495 1.693 956/ 1592 192 (192) w:sig 2 lpwork 1576 0.376 572/ 1592 50 ( 50) w:sig 3 init 1655 0.000 1404/ 2496 100 (100) w:sem 170 top 9 0.000 1204/ 1696 100 (100) RUN 87 dataman 1 0.000 652/ 1192 90 ( 90) w:sem 105 sensors 31810 8.090 1644/ 1992 250 (250) w:sem 107 gps 1217 0.000 708/ 1192 220 (220) w:sem 109 commander 49016 17.027 3412/ 3596 215 (215) w:sig +100 184 head room 114 mavlink_if0 2607 0.564 2092/ 2392 100 (100) READY 115 mavlink_rcv_if0 27 0.000 804/ 2096 175 (175) w:sem 122 sdlog2 849 0.188 2068/ 2992 70 ( 70) READY 125 commander_low_prio 4678 0.000 2740/ 2872 50 ( 50) w:sem +200 132 head room 142 attitude_estimator_q 65555 16.933 1956/ 2096 250 (250) w:sem 144 position_estimator_inav 23877 6.208 4588/ 4992 250 (250) w:sem 148 mc_att_control 31210 7.902 1132/ 1496 250 (250) w:sem 150 mc_pos_control 2901 0.658 1044/ 1496 250 (250) w:sem 155 navigator 2219 0.470 828/ 1496 105 (105) w:sem 165 mavlink_if1 21195 5.079 2156/ 2392 100 (100) w:sig 167 mavlink_rcv_if1 116 0.000 1236/ 2096 175 (175) w:sem --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 02b919b524..49e6459b03 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -301,7 +301,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3500, + 3600, commander_thread_main, (char * const *)&argv[0]); @@ -1304,7 +1304,7 @@ int commander_thread_main(int argc, char *argv[]) /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2600); + pthread_attr_setstacksize(&commander_low_prio_attr, 2880); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); From 34410e7ce4f384fa49206a712183f096766b9713 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Nov 2015 19:03:10 -0800 Subject: [PATCH 183/624] Changes for updated DriverFramework Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 6 +++--- src/lib/DriverFramework | 2 +- src/modules/commander/state_machine_helper.cpp | 10 +++++----- src/platforms/posix/drivers/barosim/baro.cpp | 4 ++-- src/platforms/posix/drivers/rgbledsim/rgbled.cpp | 1 + 5 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 470b8388b3..927dd570d1 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -550,16 +550,16 @@ void VDev::showDevices() pthread_mutex_unlock(&devmutex); PX4_INFO("DF Devices:"); - std::string dev_path, instance_path; + const char *dev_path; unsigned int index = 0; i = 0; do { // Each look increments index and returns -1 if end reached - i = DevMgr::getNextDevicePath(index, dev_path, instance_path); + i = DevMgr::getNextDeviceName(index, &dev_path); if (i == 0) { - PX4_INFO(" %s (%s)", dev_path.c_str(), instance_path.c_str()); + PX4_INFO(" %s", dev_path); } } while (i == 0); } diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 10bc08d31f..e137690747 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 10bc08d31fe0cddb997b3349f34a7ea38c5c7b9d +Subproject commit e137690747130776b24ace01099d3ba2329cd36a diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a135e9dfca..1b6d671c97 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -496,25 +496,25 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta // Handle DF devices - std::string df_dev_path, df_dev_instance_path; + const char *df_dev_path; unsigned int index = 0; for(;;) { - if (DevMgr::getNextDevicePath(index, df_dev_path, df_dev_instance_path) < 0) { + if (DevMgr::getNextDeviceName(index, &df_dev_path) < 0) { break; } DevHandle h; - DevMgr::getHandle(df_dev_path.c_str(), h); + DevMgr::getHandle(df_dev_path, h); if (!h.isValid()) { - warn("failed opening device %s", df_dev_path.c_str()); + warn("failed opening device %s", df_dev_path); continue; } int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1); DevMgr::releaseHandle(h); - printf("Disabling %s: %s\n", df_dev_path.c_str(), (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", df_dev_path, (block_ret == OK) ? "OK" : "ERROR"); } ret = TRANSITION_CHANGED; diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 578846a58e..6e66959ffc 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -920,7 +920,7 @@ test() /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { UpdateList in_set, out_set; - in_set.push_back(&h); + in_set.pushBack(&h); /* wait for data to be ready */ ret = DevMgr::waitForUpdate(in_set, out_set, 1000); @@ -1029,7 +1029,7 @@ calibrate(unsigned altitude) int ret; ssize_t sz; UpdateList in_set, out_set; - in_set.push_back(&h); + in_set.pushBack(&h); /* wait for data to be ready */ ret = DevMgr::waitForUpdate(in_set, out_set, 1000); diff --git a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp index 27518f0116..64d8580250 100644 --- a/src/platforms/posix/drivers/rgbledsim/rgbled.cpp +++ b/src/platforms/posix/drivers/rgbledsim/rgbled.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include From 59b1e6b559aac0f7248551e3f8a200c233fcefbb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Nov 2015 20:15:14 -0800 Subject: [PATCH 184/624] Added qurt stub for pthread_cont_init Updated to latest DriverFramework Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- src/platforms/qurt/stubs/stubs_posix.c | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index e137690747..5df8560897 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit e137690747130776b24ace01099d3ba2329cd36a +Subproject commit 5df8560897b02314f4e3069ed3bfd4c525f38059 diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/src/platforms/qurt/stubs/stubs_posix.c index 64555694d4..3dd4720cb3 100644 --- a/src/platforms/qurt/stubs/stubs_posix.c +++ b/src/platforms/qurt/stubs/stubs_posix.c @@ -130,3 +130,8 @@ int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const s { return 1; } + +int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr) +{ + return 1; +} From ec250ea9d4b4f52f35b2350cb99826b14553d9be Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Nov 2015 20:40:45 -0800 Subject: [PATCH 185/624] Updated to latest DriverFramework Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 5df8560897..fd274dce0c 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 5df8560897b02314f4e3069ed3bfd4c525f38059 +Subproject commit fd274dce0cc30db2fe21ef59c4eba6c3b0804aa0 From 71320ac0fef7b5d9282a6b245adc14df2ce67725 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 09:00:44 +0100 Subject: [PATCH 186/624] Commander: Add decimal hints --- src/modules/commander/commander_params.c | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1c4108017f..4b06ecbdf4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); @@ -69,6 +70,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); @@ -83,6 +85,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); @@ -93,6 +96,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -103,6 +107,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -115,6 +120,8 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * @group Battery Calibration * @unit V * @min 0.0 + * @max 1.5 + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -137,6 +144,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * * @group Battery Calibration * @unit mA + * @decimal 0 */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); @@ -184,6 +192,7 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @group Commander * @min 0.0 * @max 1.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); @@ -196,6 +205,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * @min 0.0 * @max 30.0 * @unit ampere + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -209,6 +219,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @unit second * @min 0.0 * @max 60.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); @@ -221,6 +232,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * @unit second * @min 0 * @max 35 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); @@ -233,6 +245,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); * @unit meter * @min 2 * @max 15 + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); @@ -245,6 +258,7 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); * @unit meter * @min 5 * @max 25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f); From c9c5f59bce2e5feea2879851a50fde8bfac8cd5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 09:05:38 +0100 Subject: [PATCH 187/624] Update driver framework to include Mac build fixes --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index fd274dce0c..243a32301b 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit fd274dce0cc30db2fe21ef59c4eba6c3b0804aa0 +Subproject commit 243a32301b9c467205a59a16366cc28a9a244e71 From 4606bc927ea1a0351846d062f4c1f213ca590a00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 09:23:39 +0100 Subject: [PATCH 188/624] Fix SITL / jMAVSim with correct start script --- posix-configs/SITL/init/{rcS_jmavsim => rcS_jmavsim_iris} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename posix-configs/SITL/init/{rcS_jmavsim => rcS_jmavsim_iris} (99%) diff --git a/posix-configs/SITL/init/rcS_jmavsim b/posix-configs/SITL/init/rcS_jmavsim_iris similarity index 99% rename from posix-configs/SITL/init/rcS_jmavsim rename to posix-configs/SITL/init/rcS_jmavsim_iris index 878eda3a6e..a893f0dc52 100644 --- a/posix-configs/SITL/init/rcS_jmavsim +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -27,7 +27,7 @@ param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 param set COM_RC_IN_MODE 2 -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start From 3c8a52a72b196522b96c17ed4382d2223ec0e3c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 09:23:57 +0100 Subject: [PATCH 189/624] POSIX shell: Fix enter handling --- src/platforms/posix/main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 149ae6c185..0bad53108f 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -137,6 +137,10 @@ static void usage() static void process_line(string &line, bool exit_on_fail) { + if (line.length() == 0) { + printf("\n"); + } + vector appargs(8); stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> From 1f33451690d80978b2a46d69fe3cdd8fa6261c63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 09:46:05 +0100 Subject: [PATCH 190/624] POSIX: Fix code style --- src/platforms/posix/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 0bad53108f..5734839460 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -139,7 +139,7 @@ static void process_line(string &line, bool exit_on_fail) { if (line.length() == 0) { printf("\n"); - } + } vector appargs(8); From 7d59213a01ba9998b65de26013e4e8a75c5fae48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:14:36 +0100 Subject: [PATCH 191/624] Add takeoff commands / flags --- msg/vehicle_status.msg | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 60ab34e90d..deddd35e30 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -9,7 +9,8 @@ uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 uint8 MAIN_STATE_RATTITUDE = 9 -uint8 MAIN_STATE_MAX = 10 +uint8 MAIN_STATE_AUTO_TAKEOFF = 10 +uint8 MAIN_STATE_MAX = 11 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -43,7 +44,8 @@ uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode -uint8 NAVIGATION_STATE_MAX = 17 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_MAX = 18 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 From e96fd83565cf712e1085f99b99c1d83cfeccd6b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:14:51 +0100 Subject: [PATCH 192/624] Navigator: Support takeoff command --- src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/navigator.h | 5 +- src/modules/navigator/navigator_main.cpp | 21 ++++++ src/modules/navigator/takeoff.cpp | 93 ++++++++++++++++++++++++ src/modules/navigator/takeoff.h | 67 +++++++++++++++++ 5 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/takeoff.cpp create mode 100644 src/modules/navigator/takeoff.h diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index d8399ccafd..6a5c4c15d9 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mission.cpp loiter.cpp rtl.cpp + takeoff.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 6ecf73a606..6facdd9081 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -61,6 +61,7 @@ #include "navigator_mode.h" #include "mission.h" #include "loiter.h" +#include "takeoff.h" #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" @@ -71,7 +72,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 7 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public control::SuperBlock { @@ -183,6 +184,7 @@ private: int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ int _param_update_sub; /**< param update subscription */ + int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; @@ -218,6 +220,7 @@ private: NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ + Takeoff _takeoff; /**< class for handling takeoff commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b056e3fd99..e65b434f4b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include @@ -111,6 +112,7 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _offboard_mission_sub(-1), _param_update_sub(-1), + _vehicle_command_sub(-1), _pos_sp_triplet_pub(nullptr), _mission_result_pub(nullptr), _geofence_result_pub(nullptr), @@ -135,6 +137,7 @@ Navigator::Navigator() : _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), + _takeoff(this, "TKF"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), @@ -157,6 +160,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_engineFailure; _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; + _navigation_mode_array[7] = &_takeoff; updateParams(); } @@ -286,6 +290,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); @@ -398,6 +403,18 @@ Navigator::task_main() } } + bool updated; + orb_check(_vehicle_command_sub, &updated); + + if (updated) { + vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { + warnx("navigator: got takeoff coordinates"); + } + } + /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && @@ -466,6 +483,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_takeoff; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp new file mode 100644 index 0000000000..76f58c11b9 --- /dev/null +++ b/src/modules/navigator/takeoff.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 Takeoff.cpp + * + * Helper class to Takeoff + * + * @author Lorenz Meier +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Takeoff.h" +#include "navigator.h" + +Takeoff::Takeoff(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) +{ + /* load initial params */ + updateParams(); +} + +Takeoff::~Takeoff() +{ +} + +void +Takeoff::on_inactive() +{ +} + +void +Takeoff::on_activation() +{ + /* set current mission item to Takeoff */ + set_loiter_item(&_mission_item, _param_min_alt.get()); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Takeoff::on_active() +{ +} diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h new file mode 100644 index 0000000000..2ac50b8748 --- /dev/null +++ b/src/modules/navigator/takeoff.h @@ -0,0 +1,67 @@ +/*************************************************************************** + * + * Copyright (c) 2014 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 Takeoff.h + * + * Helper class to take off + * + * @author Lorenz Meier + */ + +#ifndef NAVIGATOR_TAKEOFF_H +#define NAVIGATOR_TAKEOFF_H + +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Takeoff : public MissionBlock +{ +public: + Takeoff(Navigator *navigator, const char *name); + + ~Takeoff(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; +}; + +#endif From a242c0ff6d7d3c492946a1d41c5160a920af6893 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:15:17 +0100 Subject: [PATCH 193/624] Commander: Add takeoff command handler. Do not check RC config in SITL RC mode --- src/modules/commander/commander.cpp | 51 +++++++++++++++++-- .../commander/state_machine_helper.cpp | 24 ++++++++- 2 files changed, 70 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 49e6459b03..6251e66168 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -410,6 +410,37 @@ int commander_main(int argc, char *argv[]) return 0; } + if (!strcmp(argv[1], "takeoff")) { + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + warnx("note: not updating home position on commandline arming!"); + + /* see if we got a home position */ + if (status.condition_home_position_valid) { + vehicle_command_s cmd = {}; + + cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; + // cmd.param1 = 0.25f; /* minimum pitch */ + // /* param 2-3 unused */ + // cmd.param4 = home_position.yaw; + // cmd.param5 = home_position.lat; + // cmd.param6 = home_position.lon; + // cmd.param7 = home_position.alt; + + // XXX inspect use of publication handle + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + } else { + warnx("rejecting takeoff, no position lock yet"); + } + + } else { + warnx("arming failed"); + } + px4_close(mavlink_fd_local); + return 0; + } + usage("unrecognized command"); return 1; } @@ -809,6 +840,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; + case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { + /* ok, home set, use it to take off */ + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF)) { + warnx("taking off!"); + } else { + warnx("takeoff denied"); + } + + } + break; + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -974,6 +1016,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; @@ -1367,7 +1410,7 @@ int commander_thread_main(int argc, char *argv[]) if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } /* re-check RC calibration */ @@ -1471,11 +1514,11 @@ int commander_thread_main(int argc, char *argv[]) if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true); } else { /* check sensors also */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -3248,7 +3291,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e7dc322206..ba8720f9ee 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -351,6 +351,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case vehicle_status_s::MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; @@ -744,6 +745,27 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + } + break; + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { @@ -785,7 +807,7 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool } bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), + checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); if (!status->cb_usb && status->usb_connected && prearm) { From 703979c8927cbaac3fa057e7d7450968f5133ca3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:33:22 +0100 Subject: [PATCH 194/624] jMAVSim update: Have GPS after 1 second --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 96029523d1..a313e80266 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 +Subproject commit a313e802663332e405dee24e732a82c79884dc3a From 9c80375a784f76b0d576080ba16857921ee76f1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:36:57 +0100 Subject: [PATCH 195/624] Navigator: Fix header file name typo --- src/modules/navigator/takeoff.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 76f58c11b9..13ba35c2b9 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -50,7 +50,7 @@ #include #include -#include "Takeoff.h" +#include "takeoff.h" #include "navigator.h" Takeoff::Takeoff(Navigator *navigator, const char *name) : From a27a6acb54756600b0504f25efdadc521b12101d Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Fri, 20 Nov 2015 22:29:40 +1100 Subject: [PATCH 196/624] Don't call param_opcode unless there are dirty nodes [resolves PX4/Firmware#3160] --- src/modules/uavcan/uavcan_servers.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ca5bcede66..961002c2e2 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -523,8 +523,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) switch (command_id) { case 1: { // Param save request - _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; - param_opcode(get_next_dirty_node_id(1)); + int node_id; + node_id = get_next_dirty_node_id(1); + if (node_id < 128) { + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + param_opcode(node_id); + } break; } case 2: { From ff5db6b1be3c0fdd27eced5f29e51b5c58b8fbb4 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 13:10:46 +0100 Subject: [PATCH 197/624] fixed alt hold bug --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c36ba5bfc6..1b261bef81 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -744,20 +744,24 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { - if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) + if (!_alt_hold_engaged) { - _alt_hold_engaged = true; + if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { + _alt_hold_engaged = true; + } else { + _alt_hold_engaged = false; + } } } else { _alt_hold_engaged = false; - _pos_sp(2) = _pos(2); } /* set requested velocity setpoint */ if (!_alt_hold_engaged) { _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ _vel_sp(2) = req_vel_sp_scaled(2); + _pos_sp(2) = _pos(2); } } } From 5303a63da0ab3bf54d7a76f229bf757c47ebbead Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 13:16:42 +0100 Subject: [PATCH 198/624] mc_pos_control: code formatting --- .../mc_pos_control/mc_pos_control_main.cpp | 138 ++++++++++-------- 1 file changed, 76 insertions(+), 62 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 1b261bef81..a26a65d468 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -279,8 +279,8 @@ private: */ void control_offboard(float dt); - bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res); /** * Set position setpoint for AUTO @@ -321,7 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _control_task(-1), _mavlink_fd(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), @@ -332,7 +332,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _pos_sp_triplet_sub(-1), _global_vel_sp_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), @@ -517,7 +517,7 @@ MulticopterPositionControl::parameters_update(bool force) _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - param_get(_params_handles.mc_att_yaw_p,&v); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -626,9 +626,9 @@ MulticopterPositionControl::reset_pos_sp() _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); + - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); + - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -703,7 +703,8 @@ MulticopterPositionControl::control_manual(float dt) /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity + math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult( + _params.vel_max); // in NED and scaled to actual velocity /* * assisted velocity mode: user controls velocity, but if velocity is small enough, position @@ -711,20 +712,20 @@ MulticopterPositionControl::control_manual(float dt) */ /* horizontal axes */ - if (_control_mode.flag_control_position_enabled) - { + if (_control_mode.flag_control_position_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) - { + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { - if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { + if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy + && fabsf(_vel(1)) < _params.hold_max_xy)) { _pos_hold_engaged = true; + } else { _pos_hold_engaged = false; } } - } - else { + + } else { _pos_hold_engaged = false; } @@ -739,21 +740,19 @@ MulticopterPositionControl::control_manual(float dt) } /* vertical axis */ - if (_control_mode.flag_control_altitude_enabled) - { + if (_control_mode.flag_control_altitude_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) - { - if (!_alt_hold_engaged) - { + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { + if (!_alt_hold_engaged) { if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { _alt_hold_engaged = true; + } else { _alt_hold_engaged = false; } } - } - else { + + } else { _alt_hold_engaged = false; } @@ -781,6 +780,7 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -795,6 +795,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_pos_sp_triplet.current.yaw_valid) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } @@ -802,6 +803,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { /* Control altitude */ _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -811,6 +813,7 @@ MulticopterPositionControl::control_offboard(float dt) _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } + } else { reset_pos_sp(); reset_alt_sp(); @@ -818,8 +821,8 @@ MulticopterPositionControl::control_offboard(float dt) } bool -MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res) { /* project center of sphere on line */ /* normalized AB */ @@ -854,13 +857,14 @@ void MulticopterPositionControl::control_auto(float dt) //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); + if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || - !PX4_ISFINITE(_pos_sp_triplet.current.lon) || - !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { + !PX4_ISFINITE(_pos_sp_triplet.current.lon) || + !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } } @@ -880,21 +884,21 @@ void MulticopterPositionControl::control_auto(float dt) curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); if (PX4_ISFINITE(curr_sp(0)) && - PX4_ISFINITE(curr_sp(1)) && - PX4_ISFINITE(curr_sp(2))) { + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { current_setpoint_valid = true; } } if (_pos_sp_triplet.previous.valid) { map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if (PX4_ISFINITE(prev_sp(0)) && - PX4_ISFINITE(prev_sp(1)) && - PX4_ISFINITE(prev_sp(2))) { + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { previous_setpoint_valid = true; } } @@ -924,14 +928,15 @@ void MulticopterPositionControl::control_auto(float dt) math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than unit radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ if (_pos_sp_triplet.next.valid) { math::Vector<3> next_sp; map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp.data[0], &next_sp.data[1]); + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); if ((next_sp - curr_sp).length() > MIN_DIST) { @@ -949,6 +954,7 @@ void MulticopterPositionControl::control_auto(float dt) if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than unit radius, limit it */ if (curr_next_s_len > 1.0f) { cos_a_curr_next /= curr_next_s_len; @@ -956,8 +962,8 @@ void MulticopterPositionControl::control_auto(float dt) /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * - (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); pos_sp_s += pos_ff; } } @@ -965,6 +971,7 @@ void MulticopterPositionControl::control_auto(float dt) } else { bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { /* unit sphere crosses trajectory */ @@ -992,6 +999,7 @@ void MulticopterPositionControl::control_auto(float dt) /* difference between current and desired position setpoints, 1 = max speed */ math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } @@ -1149,7 +1157,8 @@ MulticopterPositionControl::task_main() control_auto(dt); } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1182,18 +1191,20 @@ MulticopterPositionControl::task_main() } /* make sure velocity setpoint is saturated in xy*/ - float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + - _vel_sp(1)*_vel_sp(1)); - if (vel_norm_xy > _params.vel_max(0)) { + float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + + _vel_sp(1) * _vel_sp(1)); + + if (vel_norm_xy > _params.vel_max(0)) { /* note assumes vel_max(0) == vel_max(1) */ - _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; - _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy; } /* make sure velocity setpoint is saturated in z*/ - float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { - _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + _vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; } if (!_control_mode.flag_control_position_enabled) { @@ -1214,7 +1225,8 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1304,7 +1316,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; @@ -1514,6 +1526,7 @@ MulticopterPositionControl::task_main() /* publish local position setpoint */ if (_local_pos_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } @@ -1528,7 +1541,7 @@ MulticopterPositionControl::task_main() } /* generate attitude setpoint from manual controls */ - if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -1537,8 +1550,7 @@ MulticopterPositionControl::task_main() } /* do not move yaw while arming */ - else if (_manual.z > 0.1f) - { + else if (_manual.z > 0.1f) { const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; @@ -1569,8 +1581,8 @@ MulticopterPositionControl::task_main() } /* construct attitude setpoint rotation matrix */ - math::Matrix<3,3> R_sp; - R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); /* copy quaternion setpoint to attitude setpoint topic */ @@ -1578,8 +1590,8 @@ MulticopterPositionControl::task_main() q_sp.from_dcm(R_sp); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - } - else { + + } else { reset_yaw_sp = true; } @@ -1591,17 +1603,19 @@ MulticopterPositionControl::task_main() * in this case the attitude setpoint is published by the mavlink app */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled + && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(_mavlink_fd, "[mpc] stopped"); @@ -1616,11 +1630,11 @@ MulticopterPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); From 442c0d47ef191deedf3c269fc9b872341ab661df Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Fri, 20 Nov 2015 23:32:54 +1100 Subject: [PATCH 199/624] Fix off-by-one error in parameter count [resolves PX4/Firmware#3162] --- src/modules/uavcan/uavcan_servers.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ca5bcede66..19316a8b8e 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -575,7 +575,8 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult Date: Fri, 20 Nov 2015 14:58:29 +0100 Subject: [PATCH 200/624] ekf: use airspeed directly, the other thing was just wrong --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 49f08f3cab..44c7b3e434 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -894,8 +894,10 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[3] = _ekf->states[3]; /* Airspeed (Groundspeed - Windspeed) */ - _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); - + //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL + // and in outdoor tests + _ctrl_state.airspeed = _airspeed.true_airspeed_m_s; /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; From 6bab225feddff9a097d8c83c7c6c11a425271c89 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 20 Nov 2015 04:17:24 -1000 Subject: [PATCH 201/624] Added the integration like on mpu6000 --- src/drivers/mpu9250/mpu9250.cpp | 45 ++++++++++++++++++++++++++++----- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index bff57865c8..002b0dbf68 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -70,6 +70,7 @@ #include #include +#include #include #include #include @@ -173,12 +174,16 @@ #define MPU_WHOAMI_9250 0x71 -#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 #define MPU9250_ACCEL_DEFAULT_RATE 1000 +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU9250_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE #define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 +#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 + #define MPU9250_ONE_G 9.80665f #ifdef PX4_SPI_BUS_EXT @@ -280,6 +285,9 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // this is used to support runtime checking of key @@ -535,6 +543,8 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_next(0), _last_temperature(0), @@ -667,7 +677,7 @@ MPU9250::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -679,7 +689,7 @@ MPU9250::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -1669,6 +1679,14 @@ MPU9250::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1696,6 +1714,14 @@ MPU9250::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1706,10 +1732,15 @@ MPU9250::measure() _gyro_reports->force(&grb); /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + if (accel_notify) { + poll_notify(POLLIN); + } - if (!(_pub_blocked)) { + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); @@ -1717,7 +1748,7 @@ MPU9250::measure() orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (!(_pub_blocked)) { + if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } From 3337eb8f1a1935452b4d7579ad3987a19d6bcd6a Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 16:09:49 +0100 Subject: [PATCH 202/624] unsubscribe from topic to prevent overflow in device handler list --- Tools/generate_listener.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index e8cd1ae114..f097cb741b 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -200,6 +200,7 @@ for index,m in enumerate(messages[1:]): print("\t} else {") print("\t\t printf(\" Topic did not match any known topics\\n\");") print("\t}") +print("\t\torb_unsubscribe(sub);") print("\t return 0;") print("}") From 275761f3d516d1cbcd7e9f33a6141681364a775b Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 20 Nov 2015 23:32:53 +0100 Subject: [PATCH 203/624] properly assign struct member --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 373e7505b6..d27f81291e 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -474,8 +474,9 @@ PWMSim::task_main() } /* do mixing */ - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); + outputs.noutputs = num_outputs; outputs.timestamp = hrt_absolute_time(); /* disable unused ports by setting their output to NaN */ From e41c3cf87676e64a1cadb42840998791a3bd1981 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 21 Nov 2015 01:08:58 -1000 Subject: [PATCH 204/624] Missing Close es and poll rate reset --- src/drivers/mpu9250/mpu9250.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 002b0dbf68..4308149bb3 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -2043,6 +2043,13 @@ test(bool external_bus) warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + /* reset to default polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "reset to default polling"); + } + + close(fd); + close(fd_gyro); /* XXX add poll-rate tests here too */ From cbbdb8af9b8e4d7d19022e2157d4b5336d64b8aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 14:23:15 +0100 Subject: [PATCH 205/624] Gyro sim: add missing call initializer --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 9fb3ae4c6b..f9e9272049 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -311,6 +311,7 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), + _call{}, _accel_reports(nullptr), _accel_scale{}, _accel_range_scale(0.0f), From 2b8cb692ca4c6791b842d700e7703c2fae028488 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 14:25:04 +0100 Subject: [PATCH 206/624] Update DriverFramework to fix locking and missing field initializers --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 243a32301b..297adfd4f8 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 243a32301b9c467205a59a16366cc28a9a244e71 +Subproject commit 297adfd4f8b912c7a6cb25d131530af29371e9d2 From 7c7ef7f767ea79508696d68082ac4f4b01ec749b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:10:43 +0100 Subject: [PATCH 207/624] Fix system boot commandline handling --- src/platforms/posix/main.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 274db530ab..11828dad23 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -75,6 +75,7 @@ extern "C" { { cout.flush(); cout << endl << "floating point exception" << endl; + PX4_BACKTRACE(); cout.flush(); } } @@ -102,14 +103,17 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) } arg[i] = (char *)0; + + if (exit_on_fail) { + cout << endl; + } + int retval = apps[command](i, (char **)arg); if (exit_on_fail && retval) { exit(retval); } - usleep(65000); - } else if (command.compare("help") == 0) { list_builtins(); @@ -117,11 +121,13 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) // Do nothing } else { - cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; + cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } - print_prompt(); + if (exit_on_fail) { + print_prompt(); + } } static void usage() From a4a9ff70464ef71e8e40d36a62d5dceff915c76e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:43:07 +0100 Subject: [PATCH 208/624] Fix debug targets for sitl run --- Tools/sitl_run.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7dee02868a..85d9034cb0 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -48,7 +48,7 @@ then ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & SIM_PID=`echo $!` -elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ] +elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ] then if [ -x "$(command -v gazebo)" ] then @@ -79,13 +79,13 @@ touch rootfs/eeprom/parameters # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../${rc_script}_${program} + lldb -- mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../${rc_script}_${program} + gdb --args mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "ddd" ] then - ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program} + ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "valgrind" ] then valgrind ./mainapp ../../../../${rc_script}_${program}_${model} From 240b13e003f9906e81d0f1a3c8c6ef4acc09a023 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:43:24 +0100 Subject: [PATCH 209/624] DriverFramework: Update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 297adfd4f8..4e08464199 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 297adfd4f8b912c7a6cb25d131530af29371e9d2 +Subproject commit 4e0846419914a95fd1e9fe8a086de6fecb217a08 From 4b56587188d9b990809dd15f045e8103038adb17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:45:06 +0100 Subject: [PATCH 210/624] Gyrosim: fix rate and device id, also output of info command --- .../posix/drivers/gyrosim/gyrosim.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index f9e9272049..d11261df20 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -166,7 +166,6 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; - unsigned _sample_rate; perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; @@ -308,7 +307,7 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1000), + VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), _call{}, @@ -323,7 +322,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _sample_rate(1000), _accel_reads(perf_alloc(PC_COUNT, "gyrosim_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "gyrosim_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")), @@ -335,6 +333,7 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _last_temperature(0) { + m_id.dev_id_s.bus = 1; m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ @@ -542,9 +541,10 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) // register dumps look correct write_reg(MPUREG_SMPLRT_DIV, div - 1); - _sample_rate = 1000 / div; - PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - setSampleInterval(1000000 / _sample_rate); + unsigned sample_rate = 1000 / div; + PX4_INFO("GYROSIM: Changed sample rate to %uHz", sample_rate); + setSampleInterval(1000000 / sample_rate); + _gyro->setSampleInterval(1000000 / sample_rate); } ssize_t @@ -816,7 +816,7 @@ GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) return _accel_reports->size(); case ACCELIOCGSAMPLERATE: - return _sample_rate; + return 1e6 / m_sample_interval_usecs; case ACCELIOCSSAMPLERATE: _set_sample_rate(arg); @@ -887,7 +887,7 @@ GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg) return _gyro_reports->size(); case GYROIOCGSAMPLERATE: - return _sample_rate; + return 1e6 / m_sample_interval_usecs; case GYROIOCSSAMPLERATE: _set_sample_rate(arg); @@ -1179,6 +1179,7 @@ int GYROSIM_gyro::init() { int ret = VirtDevObj::init(); + printf("init ret: %d\n", ret); return ret ? ret : start(); } @@ -1434,6 +1435,8 @@ info() PX4_INFO("state @ %p", *g_dev_ptr); (*g_dev_ptr)->print_info(); + unsigned dummy = 0; + PX4_INFO("device_id: %u", (unsigned int)(*g_dev_ptr)->devIOCTL(DEVIOCGDEVICEID, dummy)); return 0; } From 3b38004725dc60da3ac3088a61d2d0a0b961d6bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:45:40 +0100 Subject: [PATCH 211/624] Accelsim: fix device ID --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 1ba06148cb..169b82abcf 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -266,7 +266,7 @@ private: ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : - VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1000), + VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _mag(new ACCELSIM_mag(this)), _accel_call{}, _mag_call{}, @@ -300,6 +300,7 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _constant_accel_count(0), _last_temperature(0) { + m_id.dev_id_s.bus = 1; m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; /* Prime _mag with parents devid. */ @@ -1020,6 +1021,8 @@ ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : _mag_orb_class_instance(-1), _mag_class_instance(-1) { + m_id.dev_id_s.bus = 1; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; } ACCELSIM_mag::~ACCELSIM_mag() @@ -1035,9 +1038,12 @@ ACCELSIM_mag::devRead(void *buffer, size_t buflen) int ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) { + int ret; switch (cmd) { case DEVIOCGDEVICEID: - return (int)VirtDevObj::devIOCTL(cmd, arg); + ret = (int)VirtDevObj::devIOCTL(cmd, arg); + //PX4_WARN("DEVICE ID: %d", ret); + return ret; break; default: @@ -1147,6 +1153,9 @@ info() PX4_DEBUG("state @ %p", g_dev); + unsigned dummy = 0; + PX4_WARN("device_id: %u", (unsigned int)g_dev->devIOCTL(DEVIOCGDEVICEID, dummy)); + return 0; } From bef79955fc97dca291e7069ba9f62623958d3eb4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:46:00 +0100 Subject: [PATCH 212/624] POSIX startup: Fix device IDs --- posix-configs/SITL/init/rcS_jmavsim_iris | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index a893f0dc52..490f93bb51 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID -1105002488 +param set CAL_ACC0_ID -1105919992 +param set CAL_ACC1_ID -1105985528 +param set CAL_MAG0_ID -1107099640 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 From 6acb8850d4b1a219999c3f7c8f085395ea8ee8f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:46:18 +0100 Subject: [PATCH 213/624] Baro sim: Initialize to 100 Hz --- src/platforms/posix/drivers/barosim/baro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 6e66959ffc..15c0882aea 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -199,7 +199,7 @@ protected: extern "C" __EXPORT int barosim_main(int argc, char *argv[]); BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) : - VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 0), + VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), From a521066a0d52c618e5e91f62e66c6ca337e6b2d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 17:47:18 +0100 Subject: [PATCH 214/624] Fixed accel sim code style --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 169b82abcf..47da41c627 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1039,6 +1039,7 @@ int ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) { int ret; + switch (cmd) { case DEVIOCGDEVICEID: ret = (int)VirtDevObj::devIOCTL(cmd, arg); From e5dc02783309f4d65234e4809873fbb1ffda5e91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 22:20:13 +0100 Subject: [PATCH 215/624] Commander: Fixed takeoff command, made feedback less verbose --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6251e66168..e5a94c0dd4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -418,6 +418,8 @@ int commander_main(int argc, char *argv[]) /* see if we got a home position */ if (status.condition_home_position_valid) { vehicle_command_s cmd = {}; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; // cmd.param1 = 0.25f; /* minimum pitch */ @@ -2086,7 +2088,6 @@ int commander_thread_main(int argc, char *argv[]) } else { /* the mission is valid */ tune_mission_ok(true); - warnx("mission ok"); } /* prevent further feedback until the mission changes */ @@ -2428,7 +2429,6 @@ int commander_thread_main(int argc, char *argv[]) // TODO handle mode changes by commands if (main_state_changed || nav_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]); main_state_changed = false; } From 7579d7e90113fa2a432ea9d631e74e59b11b320b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 22:39:38 +0100 Subject: [PATCH 216/624] Report takeoff state correctly --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index be3ab850f4..a9e39c04d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -155,6 +155,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED From 6ebc26811fbce994c9056827acfd09d8f2f972cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Nov 2015 23:07:18 +0100 Subject: [PATCH 217/624] Make MAVLink less chatty --- src/modules/navigator/mission_feasibility_checker.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 880d4bd98e..a1c166bbbf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -92,10 +92,6 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } - if (!failed) { - mavlink_log_info(_mavlink_fd, "Mission checked and ready."); - } - return !failed; } From de22fd4b4bdae5eca806851094de6f05f5f270dc Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 21 Nov 2015 23:13:27 +0100 Subject: [PATCH 218/624] SITL shell: do not save command if it replicates the previous one --- src/platforms/posix/main.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 5734839460..6152ff766d 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -247,8 +247,19 @@ int main(int argc, char **argv) buf_ptr_write = 0; } - string_buffer[buf_ptr_write] = mystr; - buf_ptr_write++; + if (buf_ptr_write > 0) { + if (mystr != string_buffer[buf_ptr_write - 1]) { + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; + } + + } else { + if (mystr != string_buffer[CMD_BUFF_SIZE - 1]) { + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; + } + } + process_line(mystr, !daemon_mode); mystr = ""; buf_ptr_read = buf_ptr_write; @@ -258,8 +269,10 @@ int main(int argc, char **argv) c = getchar(); // skip first one, does not have the info c = getchar(); + // arrow up if (c == 'A') { buf_ptr_read--; + // arrow down } else if (c == 'B') { if (buf_ptr_read < buf_ptr_write) { From 4e6bb353924728c7df5f9d0ae357622f33f2bb91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:12:22 +0100 Subject: [PATCH 219/624] sensors: Remove unneeded orb_set_interval() call --- src/modules/sensors/sensors.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a82ffb8684..3fbbdc80ba 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1291,6 +1291,9 @@ Sensors::parameter_update_poll(bool forced) continue; } + int id = h.ioctl(DEVIOCGDEVICEID, 0); + PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct gyro_scale gscale = {}; @@ -1360,6 +1363,9 @@ Sensors::parameter_update_poll(bool forced) continue; } + int id = h.ioctl(DEVIOCGDEVICEID, 0); + PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct accel_scale gscale = {}; @@ -1438,6 +1444,9 @@ Sensors::parameter_update_poll(bool forced) continue; } + int id = h.ioctl(DEVIOCGDEVICEID, 0); + PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { struct mag_scale gscale = {}; @@ -2102,9 +2111,6 @@ Sensors::task_main() _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vcontrol_mode_sub, 200); - /* * do advertisements */ From b157bee75c7eba6dede5a2fe200e03b5f5655874 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:19:10 +0100 Subject: [PATCH 220/624] Update Driver framework version --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 4e08464199..25b5597081 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 4e0846419914a95fd1e9fe8a086de6fecb217a08 +Subproject commit 25b559708140ecd56466adb7e30b29d002766e0b From 07df1be8418538e9f5af12fd6f7be46328db4772 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:25:01 +0100 Subject: [PATCH 221/624] Simulator buffer: Ensure we do not become limited on message size --- src/modules/simulator/simulator_mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2a6efc7fa7..e712e65be0 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -65,7 +65,7 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; static int openUart(const char *uart_name, int baud); static int _fd; -static unsigned char _buf[200]; +static unsigned char _buf[1024]; sockaddr_in _srcaddr; static socklen_t _addrlen = sizeof(_srcaddr); From b4e64f49140fce363895d25a8c54852fbde469ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:26:07 +0100 Subject: [PATCH 222/624] Update version of Matrix library --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 7656385ea1..9cd6ac3dd9 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 7656385ea1d3f0a374a8146430bc63cf02e66d6b +Subproject commit 9cd6ac3dd9a255a1a2ef299da413c38a9040b8c7 From 2f75e4adc32157071a97174919c25fb873af5bb5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:34:22 +0100 Subject: [PATCH 223/624] Camera trigger: Use smaller default stack for shell handler --- src/drivers/camera_trigger/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt index 4f3669876e..18f4bbde36 100644 --- a/src/drivers/camera_trigger/CMakeLists.txt +++ b/src/drivers/camera_trigger/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__camera_trigger MAIN camera_trigger + STACK 1200 COMPILE_FLAGS -Os SRCS From 65efee0d41b311eb818fc44a1b76e00b4b00523b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:43:58 +0100 Subject: [PATCH 224/624] Commander: Be less verbose --- src/modules/commander/commander.cpp | 58 +++++++++++++++-------------- 1 file changed, 30 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5a94c0dd4..fd543f9931 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -989,6 +989,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_disarm_land = param_find("COM_DISARM_LAND"); param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW"); + // These are too verbose, but we will retain them a little longer + // until we are sure we really don't need them. + // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; // main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; @@ -1000,34 +1003,34 @@ int commander_thread_main(int argc, char *argv[]) // main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; // main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; - const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; - arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; - arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; - arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; - arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + // const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; + // arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; + // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; + // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; + // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + // arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; + // arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; + // const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + // nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -2404,7 +2407,6 @@ int commander_thread_main(int argc, char *argv[]) /* print new state */ if (arming_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); arming_state_changed = false; } From a9d26af9a5e9bdc31cbd76ffe2edb436d6bf1890 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 13:58:12 +0100 Subject: [PATCH 225/624] Travis CI: Do not copy UAVCAN binary --- .travis.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 2aafa122e1..2839e52437 100644 --- a/.travis.yml +++ b/.travis.yml @@ -105,8 +105,6 @@ script: - cd .. - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/. - - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/. - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test From 8f8154cb1ca6ee9ffd23d1655fefe4441cfe0272 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 11:34:42 +0100 Subject: [PATCH 226/624] Sanitize address --- cmake/common/px4_base.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index fa34b1584d..1e681d98a0 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -530,14 +530,14 @@ function(px4_add_common_flags) ) endif() - set(max_optimization -Os) + set(max_optimization -O1) set(optimization_flags -fno-strict-aliasing - -fomit-frame-pointer -funsafe-math-optimizations -ffunction-sections -fdata-sections + -g -fsanitize=address -fno-omit-frame-pointer ) if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") From 103bf81a0204ed975e90d5a28b42616be9565e28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:01:03 +0200 Subject: [PATCH 227/624] PWM SIM: Remove orb interval calls --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index d27f81291e..835d1b1d67 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -381,7 +381,6 @@ PWMSim::task_main() _current_update_rate = 0; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_armed_sub, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; From 6be674cb74391225cbb8e5e886850a9f5a8b6f1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:01:50 +0200 Subject: [PATCH 228/624] Navigator: Move to orb_check() API use --- src/modules/navigator/navigator_main.cpp | 49 ++++++++++-------------- 1 file changed, 20 insertions(+), 29 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e65b434f4b..df2d6f5c07 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -263,6 +263,8 @@ Navigator::task_main() _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); + bool have_geofence_position_data = false; + /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ @@ -302,42 +304,24 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position and sensor updates to 50 Hz */ - orb_set_interval(_global_pos_sub, 20); - orb_set_interval(_sensor_combined_sub, 20); - hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - px4_pollfd_struct_t fds[8]; + px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _home_pos_sub; - fds[1].events = POLLIN; - fds[2].fd = _capabilities_sub; - fds[2].events = POLLIN; - fds[3].fd = _vstatus_sub; - fds[3].events = POLLIN; - fds[4].fd = _control_mode_sub; - fds[4].events = POLLIN; - fds[5].fd = _param_update_sub; - fds[5].events = POLLIN; - fds[6].fd = _sensor_combined_sub; - fds[6].events = POLLIN; - fds[7].fd = _gps_pos_sub; - fds[7].events = POLLIN; while (!_task_should_exit) { /* wait for up to 200ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ - PX4_WARN("timed out"); + PX4_WARN("navigator timed out"); continue; } else if (pret < 0) { @@ -354,10 +338,11 @@ Navigator::task_main() _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } - static bool have_geofence_position_data = false; + bool updated; /* gps updated */ - if (fds[7].revents & POLLIN) { + orb_check(_gps_pos_sub, &updated); + if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; @@ -365,33 +350,39 @@ Navigator::task_main() } /* sensors combined updated */ - if (fds[6].revents & POLLIN) { + orb_check(_sensor_combined_sub, &updated); + if (updated) { sensor_combined_update(); } /* parameters updated */ - if (fds[5].revents & POLLIN) { + orb_check(_param_update_sub, &updated); + if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ - if (fds[4].revents & POLLIN) { + orb_check(_control_mode_sub, &updated); + if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[3].revents & POLLIN) { + orb_check(_vstatus_sub, &updated); + if (updated) { vehicle_status_update(); } /* navigation capabilities updated */ - if (fds[2].revents & POLLIN) { + orb_check(_capabilities_sub, &updated); + if (updated) { navigation_capabilities_update(); } /* home position updated */ - if (fds[1].revents & POLLIN) { + orb_check(_home_pos_sub, &updated); + if (updated) { home_position_update(); } From 17653113d53c7c70451daca4efa62ff102c6b39a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:02:37 +0200 Subject: [PATCH 229/624] FW att control: Run all topics at full rate --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 ------ 1 file changed, 6 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 8772ceb835..ddc0d662bd 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -662,12 +662,6 @@ FixedwingAttitudeControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vcontrol_mode_sub, 200); - /* do not limit the attitude updates in order to minimize latency. - * actuator outputs are still limited by the individual drivers - * properly to not saturate IO or physical limitations */ - parameters_update(); /* get an initial update for all sensor and status data */ From 719881b4a473d8be66a00be17bfc740637891a08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:03:00 +0200 Subject: [PATCH 230/624] EKF: Run vehicle status updates at full rate --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 44c7b3e434..291acd5bad 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -526,9 +526,6 @@ void AttitudePositionEstimatorEKF::task_main() _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); _armedSub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); - _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* sets also parameters in the EKF object */ From 111287f6fbf83b7c6b5bcf334f5e7b20ef6ef4d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:03:36 +0200 Subject: [PATCH 231/624] Attitude EKF: Do not limit sensor speed --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e408fb3c99..273d159f2e 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -284,8 +284,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ - orb_set_interval(sub_raw, 3); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); From d094a8f7bfe895f6fc50401af71ad1c1b29492dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:04:02 +0200 Subject: [PATCH 232/624] uORB sub: Do not call orb_set_interval with zero --- src/modules/uORB/Subscription.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index a85e8da02d..0c9733712c 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -84,7 +84,9 @@ SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, if (_handle < 0) { warnx("sub failed"); } - orb_set_interval(getHandle(), interval); + if (interval > 0) { + orb_set_interval(getHandle(), interval); + } } bool SubscriptionBase::updated() From 81039a2940061ef7a6d58d2ecdeda3d8c596fd60 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:25:27 +0200 Subject: [PATCH 233/624] Commander: Fix home pos print --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd543f9931..34bd46b7d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -922,8 +922,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.yaw = attitude.yaw; - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_and_console_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (homePub != nullptr) { From ff77884c2cd46c850b7fa22d5add45eb1fb277db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 20:27:14 +0200 Subject: [PATCH 234/624] MAVLink app: Zero-initialize whole struct --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a9e39c04d4..20c04554e7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -977,7 +977,7 @@ protected: struct vehicle_gps_position_s gps; if (_gps_sub->update(&_gps_time, &gps)) { - mavlink_gps_raw_int_t msg; + mavlink_gps_raw_int_t msg = {}; msg.time_usec = gps.timestamp_position; msg.fix_type = gps.fix_type; From 59e3b0d95cc75d1d914d67389abbe563d5909591 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 21:12:41 +0200 Subject: [PATCH 235/624] Renamed sitl simple to default --- .../{posix_sitl_simple.cmake => posix_sitl_default.cmake} | 0 cmake/configs/posix_sitl_lpe.cmake | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename cmake/configs/{posix_sitl_simple.cmake => posix_sitl_default.cmake} (100%) diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_default.cmake similarity index 100% rename from cmake/configs/posix_sitl_simple.cmake rename to cmake/configs/posix_sitl_default.cmake diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake index 0598044844..c264466f9d 100644 --- a/cmake/configs/posix_sitl_lpe.cmake +++ b/cmake/configs/posix_sitl_lpe.cmake @@ -1,4 +1,4 @@ -include(cmake/configs/posix_sitl_simple.cmake) +include(cmake/configs/posix_sitl_default.cmake) list(APPEND config_module_list modules/local_position_estimator From dcb8705059f1934cdb5463639f47e05474f703e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 21:13:21 +0200 Subject: [PATCH 236/624] Adjust to new CMAKE targets --- Makefile | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/Makefile b/Makefile index b0f49eda3e..e0b17fe41f 100644 --- a/Makefile +++ b/Makefile @@ -139,13 +139,13 @@ px4fmu-v2_lpe: nuttx_sim_simple: $(call cmake-build,$@) -posix_sitl_simple: +posix_sitl_default: $(call cmake-build,$@) posix_sitl_lpe: $(call cmake-build,$@) -ros_sitl_simple: +ros_sitl_default: $(call cmake-build,$@) qurt_eagle_travis: @@ -157,19 +157,15 @@ qurt_eagle_release: posix_eagle_release: $(call cmake-build,$@) -posix: posix_sitl_simple - -posix_sitl_default: posix_sitl_simple - -ros: ros_sitl_simple +posix: posix_sitl_default sitl_deprecation: @echo "Deprecated. Use 'make posix_sitl_default jmavsim' or" @echo "'make posix_sitl_default gazebo' if Gazebo is preferred." -sitl_quad: sitl_deprecation -sitl_plane: sitl_deprecation -sitl_ros: sitl_deprecation +run_sitl_quad: sitl_deprecation +run_sitl_plane: sitl_deprecation +run_sitl_ros: sitl_deprecation # Other targets # -------------------------------------------------------------------- From 454f1637834d27fa1fd97ac9f49e224d1a6c566c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 22:22:29 +0200 Subject: [PATCH 237/624] Fixed travis CI target to new name --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 2839e52437..a66f6b2f27 100644 --- a/.travis.yml +++ b/.travis.yml @@ -97,8 +97,8 @@ env: script: - make check_format - arm-none-eabi-gcc --version - - echo 'Building POSIX Firmware..' && make posix_sitl_simple - - echo 'Running Tests..' && make posix_sitl_simple test + - echo 'Building POSIX Firmware..' && make posix_sitl_default + - echo 'Running Tests..' && make posix_sitl_default test - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make From 2749a57641a146fcdaee6f835d1a533fd1d63686 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 00:24:58 +0200 Subject: [PATCH 238/624] Add gyro instrumentation --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index d11261df20..d3d868c908 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -523,7 +523,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { - PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); + PX4_INFO("GYROSIM::_set_sample_rate %u Hz", desired_sample_rate_hz); if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || @@ -1010,7 +1010,7 @@ void GYROSIM::_measure() { -#if 0 +#if 1 static int x = 0; // Verify the samples are being taken at the expected rate From bffce0c9eb899a87aaa20183061b4a39547806ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:52:00 +0100 Subject: [PATCH 239/624] VDEV: Less verbose --- src/drivers/device/vdev.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 927dd570d1..330447086c 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -367,7 +367,6 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) case DEVIOCGDEVICEID: ret = (int)_device_id.devid; - PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); break; default: From aafb11ae922592eb3ec46e2e84a76c34d18e68fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:52:17 +0100 Subject: [PATCH 240/624] VDev: Less verbose in header --- src/drivers/device/vdev.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 419c92e456..b15fd8209f 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -181,7 +181,7 @@ protected: */ void lock() { - DEVICE_DEBUG("lock"); + //DEVICE_DEBUG("lock"); do {} while (px4_sem_wait(&_lock) != 0); } @@ -190,7 +190,7 @@ protected: */ void unlock() { - DEVICE_DEBUG("unlock"); + //DEVICE_DEBUG("unlock"); px4_sem_post(&_lock); } From 352b1692d12d6f3db604c7b372202d6bc0001d16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:52:47 +0100 Subject: [PATCH 241/624] Geo lib: Do not hardcode assumptions about process name. --- src/lib/geo/geo.c | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index af49bf1504..fe0224d508 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -86,12 +86,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (strcmp("commander", getprogname()) == 0) { - return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); - - } else { - return -1; - } + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, @@ -217,19 +212,14 @@ __EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) } __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { - if (strcmp("commander", getprogname()) == 0) { - gl_ref.alt = alt_0; + gl_ref.alt = alt_0; - if (!map_projection_global_init(lat_0, lon_0, timestamp)) { - gl_ref.init_done = true; - return 0; - - } else { - gl_ref.init_done = false; - return -1; - } + if (!map_projection_global_init(lat_0, lon_0, timestamp)) { + gl_ref.init_done = true; + return 0; } else { + gl_ref.init_done = false; return -1; } } From 40ef8c063c17600a7b8b759198c0dcb5936a5c74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:53:03 +0100 Subject: [PATCH 242/624] PWM Sim: Less verbose --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 835d1b1d67..5d81944245 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -390,8 +390,6 @@ PWMSim::task_main() _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); - DEVICE_LOG("starting"); - /* loop until killed */ while (!_task_should_exit) { From 1bfde3823c749e8e4339db6794a898536b5f03d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:53:41 +0100 Subject: [PATCH 243/624] Commander: More efficient printing --- src/modules/commander/commander.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 34bd46b7d1..5bf6e462e7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -796,8 +796,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) { - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + mavlink_and_console_log_info(mavlink_fd, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); /* announce new home position */ if (*home_pub != nullptr) { @@ -2062,12 +2061,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { - warnx("Flight termination because of navigator request"); + mavlink_and_console_log_critical(mavlink_fd, "Geofence violation: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "Flight termination active"); + mavlink_and_console_log_critical(mavlink_fd, "Flight termination active"); } } From 86eb011f7d5604293fe024d854464483025a08f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:54:25 +0100 Subject: [PATCH 244/624] MAVLink log: protect against empty field --- src/modules/systemlib/mavlink_log.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index f7388091e7..4216546c1a 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -101,6 +101,10 @@ __EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink __EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { + if (!fmt) { + return; + } + va_list ap; va_start(ap, fmt); int end = (lb->start + lb->count) % lb->size; @@ -119,6 +123,10 @@ __EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int seve __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) { + if (!fmt) { + return; + } + va_list ap; va_start(ap, fmt); char text[MAVLINK_LOG_MAXLEN + 1]; From 5432d7a24604b610970a764e8653338ca94f9bf7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 13:09:28 +0100 Subject: [PATCH 245/624] sensors: Param file cleanup --- src/modules/sensors/sensor_params.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3d4dfd62c1..9543306cda 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -41,9 +41,6 @@ * @author Thomas Gubler */ -#include -#include - /** * ID of the board this parameter set was calibrated on. * @@ -1805,7 +1802,6 @@ PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); */ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Enable relay control of relay 1 mapped to the Spektrum receiver power supply * @@ -1814,7 +1810,6 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ -#endif /** * DSM binding trigger. From a3e15e161cb887c6326156a22731125a391fe6fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 13:10:20 +0100 Subject: [PATCH 246/624] Gyro sim: do not spam output --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index d3d868c908..43bfa61288 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1010,7 +1010,7 @@ void GYROSIM::_measure() { -#if 1 +#if 0 static int x = 0; // Verify the samples are being taken at the expected rate From 3af906b9a8d5f9c730b984ea9b98c08ce0985c63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 13:19:20 +0100 Subject: [PATCH 247/624] Ensure g_hrt_work is zero-initialized --- src/platforms/posix/work_queue/hrt_thread.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 98331ca798..9ac14e4b2d 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -63,7 +63,7 @@ ****************************************************************************/ /* The state of each work queue. */ -struct wqueue_s g_hrt_work; +struct wqueue_s g_hrt_work = {}; /**************************************************************************** * Private Variables From 0a883fd11d52cf8a7c17da028547bf7e21b9b945 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 12:11:10 +0100 Subject: [PATCH 248/624] HRT thread: Do not make implicit assumptions about struct members --- src/platforms/posix/work_queue/hrt_thread.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 9ac14e4b2d..08de42a730 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -143,7 +143,7 @@ static void hrt_work_process() if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + (void)dq_rem((struct dq_entry_s *)&(work->dq), &(wqueue->q)); //PX4_INFO("Dequeued work=%p", work); /* Extract the work description from the entry (in case the work From e12766826cac6eee1ddcbe4ff6e93e1ef40ed113 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 12:59:16 +0100 Subject: [PATCH 249/624] VDev posix: Initialize HRT work --- src/drivers/device/vdev_posix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index c5e1c9a4f5..745a2acb4c 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -270,7 +270,7 @@ extern "C" { if (ret >= 0) { if (timeout > 0) { // Use a work queue task - work_s _hpwork; + work_s _hpwork = {}; hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout); px4_sem_wait(&sem); From 274eb39208abaa84ff2da6eea0ea389643e8d18d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 12:59:56 +0100 Subject: [PATCH 250/624] Tone alarm: Reset work when resetting notes --- src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index a30c07303d..4946b00613 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -215,7 +215,8 @@ ToneAlarm::ToneAlarm() : _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), - _next(nullptr) + _next(nullptr), + _note_call{} { // enable debug() calls //_debug_enabled = true; @@ -376,6 +377,7 @@ ToneAlarm::start_tune(const char *tune) _repeat = false; // otherwise command-line tunes repeat forever... // schedule a callback to start playing + _note_call = {}; hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); } @@ -385,6 +387,7 @@ ToneAlarm::next_note() // do we have an inter-note gap to wait for? if (_silence_length > 0) { stop_note(); + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); _silence_length = 0; return; @@ -482,6 +485,7 @@ ToneAlarm::next_note() case 'P': // pause for a note length stop_note(); + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)rest_duration(next_number(), next_dots()), (hrt_callout)next_trampoline, @@ -510,6 +514,7 @@ ToneAlarm::next_note() if (note == 0) { // this is a rest - pause for the current note length + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)rest_duration(_note_length, next_dots()), (hrt_callout)next_trampoline, @@ -567,6 +572,7 @@ ToneAlarm::next_note() start_note(note); // and arrange a callback when the note should stop + _note_call = {}; hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); return; From cda3c28661b388d609bac3795a6fbcc3b6558ab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:00:34 +0100 Subject: [PATCH 251/624] HRT driver: Initialize work struct --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index e4839f0ac7..9874278012 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -63,7 +63,7 @@ static void hrt_call_reschedule(void); #define HRT_INTERVAL_MAX 50000000 static px4_sem_t _hrt_lock; -static struct work_s _hrt_work; +static struct work_s _hrt_work = {}; static hrt_abstime px4_timestart = 0; static void From c0033054720e43e88a43d2961b8194cb7c62fa46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:03:55 +0100 Subject: [PATCH 252/624] HRT thread: Fix style --- src/platforms/posix/work_queue/hrt_thread.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 08de42a730..b071937dd6 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -143,7 +143,7 @@ static void hrt_work_process() if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ - (void)dq_rem((struct dq_entry_s *)&(work->dq), &(wqueue->q)); + (void)dq_rem((struct dq_entry_s *) & (work->dq), &(wqueue->q)); //PX4_INFO("Dequeued work=%p", work); /* Extract the work description from the entry (in case the work From 4c60f4d98a2c27e5e0393ba6d9cc5cfc66d86df6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:47:50 +0100 Subject: [PATCH 253/624] Q estimator: Ensure init --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 4b4fba54c8..ab87014d9a 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -299,7 +299,7 @@ void AttitudeEstimatorQ::task_main() hrt_abstime last_time = 0; - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; @@ -313,10 +313,12 @@ void AttitudeEstimatorQ::task_main() if (ret < 0) { // Poll error, sleep and try again usleep(10000); + PX4_WARN("Q POLL ERROR"); continue; } else if (ret == 0) { // Poll timeout, do nothing + PX4_WARN("Q POLL TIMEOUT"); continue; } From 4fa0123652a3f99009aea1ca1f71b1f7436131cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:48:30 +0100 Subject: [PATCH 254/624] Navigator: Only indicate timeout if we really time out --- src/modules/navigator/navigator_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index df2d6f5c07..c7d1c10d05 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -314,6 +314,8 @@ Navigator::task_main() fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; + bool global_pos_available_once = false; + while (!_task_should_exit) { /* wait for up to 200ms for data */ @@ -321,7 +323,9 @@ Navigator::task_main() if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ - PX4_WARN("navigator timed out"); + if (global_pos_available_once) { + PX4_WARN("navigator timed out"); + } continue; } else if (pret < 0) { @@ -330,6 +334,8 @@ Navigator::task_main() continue; } + global_pos_available_once = true; + perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { From c81e9714c737d2d299932f438ed5420605acddd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:48:59 +0100 Subject: [PATCH 255/624] INAV: Robustify polling --- .../position_estimator_inav_main.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d8f29f1082..56db8b4c0a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -390,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); - px4_pollfd_struct_t fds_init[1] = { - { .fd = sensor_combined_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds_init[1] = {}; + fds_init[0].fd = sensor_combined_sub; + fds_init[0].events = POLLIN; /* wait for initial baro value */ bool wait_baro = true; @@ -400,11 +400,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) thread_running = true; while (wait_baro && !thread_should_exit) { - int ret = px4_poll(fds_init, 1, 1000); + int ret = px4_poll(&fds_init[0], 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(mavlink_fd, "[inav] poll error on init"); + PX4_WARN("INAV poll error"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { @@ -430,6 +431,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } + } else { + PX4_WARN("INAV poll timeout"); } } From e3b13e13245adabe6d8eb46709fca04ea9cf96cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:49:47 +0100 Subject: [PATCH 256/624] HRT: initialize queue --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 9874278012..5aab5803c3 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -48,7 +48,7 @@ #include #include "hrt_work.h" -static struct sq_queue_s callout_queue; +static struct sq_queue_s callout_queue = {}; /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 From 734ec9094e5b630dc9712d8bfe35a1fee5fa0445 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:50:49 +0100 Subject: [PATCH 257/624] Posix tasks: Initialization and locking --- .../posix/px4_layer/px4_posix_tasks.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 41cade1d5d..67abe2044d 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -62,6 +62,7 @@ #define SHELL_TASK_ID (PX4_MAX_TASKS+1) pthread_t _shell_task_id = 0; +pthread_mutex_t task_mutex = PTHREAD_MUTEX_INITIALIZER; struct task_entry { pthread_t pid; @@ -70,7 +71,7 @@ struct task_entry { task_entry() : isused(false) {} }; -static task_entry taskmap[PX4_MAX_TASKS]; +static task_entry taskmap[PX4_MAX_TASKS] = {}; typedef struct { px4_main_t entry; @@ -109,9 +110,9 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char *p = (char *)argv; - pthread_t task; + pthread_t task = {}; pthread_attr_t attr; - struct sched_param param; + struct sched_param param = {}; // Calculate argc while (p != (char *)0) { @@ -130,6 +131,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize + len); + memset(taskdata, 0, structsize + len); offset = ((unsigned long)taskdata) + structsize; taskdata->entry = entry; @@ -195,6 +197,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } } + pthread_mutex_lock(&task_mutex); for (i = 0; i < PX4_MAX_TASKS; ++i) { if (taskmap[i].isused == false) { taskmap[i].pid = task; @@ -203,6 +206,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int break; } } + pthread_mutex_unlock(&task_mutex); if (i >= PX4_MAX_TASKS) { return -ENOSPC; @@ -224,9 +228,11 @@ int px4_task_delete(px4_task_t id) return -EINVAL; } + pthread_mutex_lock(&task_mutex); // If current thread then exit, otherwise cancel if (pthread_self() == pid) { taskmap[id].isused = false; + pthread_mutex_unlock(&task_mutex); pthread_exit(0); } else { @@ -234,6 +240,7 @@ int px4_task_delete(px4_task_t id) } taskmap[id].isused = false; + pthread_mutex_unlock(&task_mutex); return rv; } @@ -246,6 +253,7 @@ void px4_task_exit(int ret) // Get pthread ID from the opaque ID for (i = 0; i < PX4_MAX_TASKS; ++i) { if (taskmap[i].pid == pid) { + pthread_mutex_lock(&task_mutex); taskmap[i].isused = false; break; } @@ -257,6 +265,7 @@ void px4_task_exit(int ret) } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } + pthread_mutex_unlock(&task_mutex); pthread_exit((void *)(unsigned long)ret); } @@ -268,7 +277,9 @@ int px4_task_kill(px4_task_t id, int sig) PX4_DEBUG("Called px4_task_kill %d", sig); if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) { + pthread_mutex_lock(&task_mutex); pid = taskmap[id].pid; + pthread_mutex_unlock(&task_mutex); } else { return -EINVAL; @@ -326,7 +337,9 @@ const char *getprogname() for (int i = 0; i < PX4_MAX_TASKS; i++) { if (taskmap[i].isused && taskmap[i].pid == pid) { + pthread_mutex_lock(&task_mutex); return taskmap[i].name.c_str(); + pthread_mutex_unlock(&task_mutex); } } From 9c13e4c4be8da8fc3197ff384e5c924e679385fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 18:45:36 +0100 Subject: [PATCH 258/624] Fix race in task create --- .../posix/px4_layer/px4_posix_tasks.cpp | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 67abe2044d..5d19dc65f7 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -110,7 +110,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char *p = (char *)argv; - pthread_t task = {}; pthread_attr_t attr; struct sched_param param = {}; @@ -127,10 +126,9 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); - pthdata_t *taskdata; // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize + len); + pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len); memset(taskdata, 0, structsize + len); offset = ((unsigned long)taskdata) + structsize; @@ -179,16 +177,30 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } - rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + pthread_mutex_lock(&task_mutex); + + int taskid = 0; + + for (i = 0; i < PX4_MAX_TASKS; ++i) { + if (taskmap[i].isused == false) { + taskmap[i].name = name; + taskmap[i].isused = true; + taskid = i; + break; + } + } + + rv = pthread_create(&taskmap[taskid].pid, &attr, &entry_adapter, (void *) taskdata); if (rv != 0) { if (rv == EPERM) { //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create(&task, NULL, &entry_adapter, (void *) taskdata); + rv = pthread_create(&taskmap[taskid].pid, NULL, &entry_adapter, (void *) taskdata); if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + taskmap[taskid].isused = false; return (rv < 0) ? rv : -rv; } @@ -197,15 +209,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } } - pthread_mutex_lock(&task_mutex); - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused == false) { - taskmap[i].pid = task; - taskmap[i].name = name; - taskmap[i].isused = true; - break; - } - } pthread_mutex_unlock(&task_mutex); if (i >= PX4_MAX_TASKS) { From 3a2473e309aeb999c460cca80ffaf5b65b2ad046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 18:47:49 +0100 Subject: [PATCH 259/624] Revert Address sanitizer changes --- cmake/common/px4_base.cmake | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 1e681d98a0..19839892bc 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -530,14 +530,13 @@ function(px4_add_common_flags) ) endif() - set(max_optimization -O1) + set(max_optimization -Os) set(optimization_flags -fno-strict-aliasing -funsafe-math-optimizations -ffunction-sections -fdata-sections - -g -fsanitize=address -fno-omit-frame-pointer ) if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") From c97854999ae7b7218ebd71ed4b4351d7d7bc42ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Nov 2015 09:33:06 +0100 Subject: [PATCH 260/624] Re-add content --- cmake/common/px4_base.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 19839892bc..fa34b1584d 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -534,6 +534,7 @@ function(px4_add_common_flags) set(optimization_flags -fno-strict-aliasing + -fomit-frame-pointer -funsafe-math-optimizations -ffunction-sections -fdata-sections From 65cca56c29259acfc363ce31df413858254bfe14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 14:07:27 +0100 Subject: [PATCH 261/624] Code style fix --- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 5d19dc65f7..729185691b 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -232,6 +232,7 @@ int px4_task_delete(px4_task_t id) } pthread_mutex_lock(&task_mutex); + // If current thread then exit, otherwise cancel if (pthread_self() == pid) { taskmap[id].isused = false; @@ -268,6 +269,7 @@ void px4_task_exit(int ret) } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } + pthread_mutex_unlock(&task_mutex); pthread_exit((void *)(unsigned long)ret); From 17882cee78ffd8dc1178c959e1b793e53d8bc79a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 15:17:07 +0100 Subject: [PATCH 262/624] Fix Travis build error --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 5aab5803c3..9874278012 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -48,7 +48,7 @@ #include #include "hrt_work.h" -static struct sq_queue_s callout_queue = {}; +static struct sq_queue_s callout_queue; /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 From f6a6c41af5ccf19eeb33f4cd2e603bee6dcf8a34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 15:30:36 +0100 Subject: [PATCH 263/624] POSIX: More Travis compile fixes --- src/platforms/posix/work_queue/hrt_thread.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index b071937dd6..5711aa7d6d 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -63,7 +63,7 @@ ****************************************************************************/ /* The state of each work queue. */ -struct wqueue_s g_hrt_work = {}; +struct wqueue_s g_hrt_work; /**************************************************************************** * Private Variables @@ -265,6 +265,7 @@ static int work_hrtthread(int argc, char *argv[]) void hrt_work_queue_init(void) { px4_sem_init(&_hrt_work_lock, 0, 1); + memset(&g_hrt_work, 0, sizeof(g_hrt_work)); // Create high priority worker thread g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", From 2730d8ab208164d0932d59cf2a8eaf5042cf1bf3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 15:50:14 +0100 Subject: [PATCH 264/624] Fix HRT init compile error --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 9874278012..e4839f0ac7 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -63,7 +63,7 @@ static void hrt_call_reschedule(void); #define HRT_INTERVAL_MAX 50000000 static px4_sem_t _hrt_lock; -static struct work_s _hrt_work = {}; +static struct work_s _hrt_work; static hrt_abstime px4_timestart = 0; static void From 4950762d4e3a0eb1c10a521257d80f8271bc16fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 15:55:49 +0100 Subject: [PATCH 265/624] Navigator: Fix compile error --- src/modules/navigator/navigator_main.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7d1c10d05..0227a2ed07 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -392,17 +392,7 @@ Navigator::task_main() home_position_update(); } - /* global position updated */ - if (fds[0].revents & POLLIN) { - global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { - have_geofence_position_data = true; - } - } - - bool updated; orb_check(_vehicle_command_sub, &updated); - if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); @@ -412,6 +402,14 @@ Navigator::task_main() } } + /* global position updated */ + if (fds[0].revents & POLLIN) { + global_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; + } + } + /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && From 4e5b2a1db7f2a3e56e3e9aee6b007bb03b6d17d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 15:56:01 +0100 Subject: [PATCH 266/624] POSIX: Fix sensor ID --- posix-configs/SITL/init/rc.fixed_wing | 8 ++++---- posix-configs/SITL/init/rcS_gazebo_iris | 8 ++++---- posix-configs/SITL/init/rcS_gazebo_tailsitter | 8 ++++---- posix-configs/SITL/init/rcS_jmavsim_iris | 8 ++++---- posix-configs/SITL/init/rcS_lpe_gazebo | 8 ++++---- posix-configs/SITL/init/rcS_lpe_jmavsim | 8 ++++---- posix-configs/SITL/init/rc_iris_ros | 8 ++++---- 7 files changed, 28 insertions(+), 28 deletions(-) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index bdb2fb8e9d..635772d727 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -5,10 +5,10 @@ param set MAV_TYPE 1 param set SYS_AUTOSTART 3033 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index ca5415d110..d5410cd2e2 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index 2bf7a78368..f7a74b20e3 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -10,10 +10,10 @@ param set VT_TYPE 0 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 490f93bb51..1661d61c4f 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID -1105002488 -param set CAL_ACC0_ID -1105919992 -param set CAL_ACC1_ID -1105985528 -param set CAL_MAG0_ID -1107099640 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo index 6093991c13..a6c3f5cf39 100644 --- a/posix-configs/SITL/init/rcS_lpe_gazebo +++ b/posix-configs/SITL/init/rcS_lpe_gazebo @@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 1 param set CAL_ACC0_YOFF 2 diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim b/posix-configs/SITL/init/rcS_lpe_jmavsim index 9b416a0b26..7d59fe876f 100644 --- a/posix-configs/SITL/init/rcS_lpe_jmavsim +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim @@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 096f659d65..3293166bad 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -9,10 +9,10 @@ param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 dataman start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1376256 -param set CAL_ACC1_ID 1310720 -param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 param set CAL_ACC0_YOFF -0.01 From 34f4c574564286cf59561b9050c6019fd2f03454 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 16:03:58 +0100 Subject: [PATCH 267/624] commander: Handle takeoff state --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5bf6e462e7..b9e6817c14 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2929,6 +2929,7 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; From 57b9ac05de158befa4062451a0203b9a8f446167 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 00:23:13 +0100 Subject: [PATCH 268/624] Navigator: taking off now on takeoff command, but not yet holding altitude once completed. --- src/modules/navigator/mission_block.cpp | 26 +++++++++++++++++++++++++ src/modules/navigator/mission_block.h | 5 +++++ src/modules/navigator/takeoff.cpp | 3 ++- 3 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 94ded8a4b9..b230bcab3f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -289,3 +289,29 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) item->origin = ORIGIN_ONBOARD; } } + +void +MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) +{ + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + /* use current position and use return altitude as clearance */ + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f) { + item->altitude += min_clearance; + } + + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ? + (min_clearance / 2) : _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = min_pitch; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index eb17a329ee..5490b4c6b8 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -96,6 +96,11 @@ protected: */ void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); + /** + * Set a takeoff mission item + */ + void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 13ba35c2b9..f669429449 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -74,12 +74,13 @@ void Takeoff::on_activation() { /* set current mission item to Takeoff */ - set_loiter_item(&_mission_item, _param_min_alt.get()); + set_takeoff_item(&_mission_item, _param_min_alt.get()); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; pos_sp_triplet->next.valid = false; _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); From 35af171575b6dd52897400e827b2e198e18aa444 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 10:41:41 +0100 Subject: [PATCH 269/624] Update sitl_gazebo version --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 6072426bbc..a3b5e54f32 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 6072426bbca9407f2f26698d3e20870c1744b4fb +Subproject commit a3b5e54f32ef41c60449e3a22cb152c6d16538bf From 4ae15e3d8456af7a98a21b06191cdc702d322418 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 12:40:33 +0100 Subject: [PATCH 270/624] SITL run: Better Gazebo output, less CMake warnings --- Tools/sitl_run.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 85d9034cb0..8e56657bd8 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -53,7 +53,7 @@ then if [ -x "$(command -v gazebo)" ] then # Set the plugin path so Gazebo finds our model and sim - export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build + export GAZEBO_PLUGIN_PATH=$curr_dir/Tools/sitl_gazebo/Build:${GAZEBO_PLUGIN_PATH} # Set the model path so Gazebo finds the airframes export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models # The next line would disable online model lookup, can be commented in, in case of unstable behaviour. @@ -61,11 +61,11 @@ then export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build - cmake .. + cmake -Wno-dev .. make -j4 - gzserver ../worlds/${model}.world & + gzserver --verbose ../worlds/${model}.world & SIM_PID=`echo $!` - gzclient & + gzclient --verbose & GUI_PID=`echo $!` else echo "You need to have gazebo simulator installed!" From 885635ed0c6a6cbc48c6cb5946f13492444737d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 12:40:54 +0100 Subject: [PATCH 271/624] Gazebo iris config: start correct RGB sim --- posix-configs/SITL/init/rcS_gazebo_iris | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index d5410cd2e2..00fdb610da 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -28,7 +28,7 @@ param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 param set COM_RC_IN_MODE 2 -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start From 0ec334128caab18cffcf4d31e83eb86fc8145ddf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 12:41:13 +0100 Subject: [PATCH 272/624] Gazebo tailsitter config: Start correct RGB sim --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index f7a74b20e3..09b361d8c5 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -29,7 +29,7 @@ param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 param set COM_RC_IN_MODE 2 -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start From 6b5ba23db8d959a551142d0d735ed8b0e7eb79c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 12:41:44 +0100 Subject: [PATCH 273/624] Makefile: Extend make clean with submodule sync which will save some developers --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index e0b17fe41f..e702ed1476 100644 --- a/Makefile +++ b/Makefile @@ -176,6 +176,7 @@ clean: @rm -rf build_*/ @(cd NuttX && git clean -d -f -x) @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) + @(git submodule sync) # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ From 5b278b0b1575b6d62693e306ed8445fb489cd7ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 12:45:08 +0100 Subject: [PATCH 274/624] Update Gazebo version --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index a3b5e54f32..1fb483cc15 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit a3b5e54f32ef41c60449e3a22cb152c6d16538bf +Subproject commit 1fb483cc15ec7d71e08c4058e0a79534751b4933 From 23cfe5c41f2068bf11ccd520e2f3d3ce95a6e50e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 13:38:05 +0100 Subject: [PATCH 275/624] SITL run: Abort on first error --- Tools/sitl_run.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 8e56657bd8..5375e50578 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -37,6 +37,8 @@ then kill $jmavsim_pid fi +set -e + cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit From fb24a54ca4ed5ebdb620fcfab7583c12e6214221 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 13:51:18 +0100 Subject: [PATCH 276/624] SITL run: Always execute cleanup action --- Tools/sitl_run.sh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 5375e50578..2b535c5e12 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -78,6 +78,10 @@ cd $build_path/src/firmware/posix mkdir -p rootfs/fs/microsd mkdir -p rootfs/eeprom touch rootfs/eeprom/parameters + +# Do not exit on failure now from here on because we want the complete cleanup +set +e + # Start Java simulator if [ "$debugger" == "lldb" ] then From e2894c4a464c02a241d1b504af46faaa1a31c8c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 19:50:44 +0100 Subject: [PATCH 277/624] Added timed wait semaphore --- src/platforms/posix/px4_layer/px4_sem.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 0aa56a1f3e..184b80a667 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -75,6 +75,20 @@ int px4_sem_wait(px4_sem_t *s) return 0; } +int px4_sem_timedwait(px4_sem_t * s, const struct timespec * abstime) +{ + pthread_mutex_lock(&(s->lock)); + s->value--; + + if (s->value < 0) { + pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + } + + pthread_mutex_unlock(&(s->lock)); + + return 0; +} + int px4_sem_post(px4_sem_t *s) { pthread_mutex_lock(&(s->lock)); From 1b91f784300d193cb175ea4fe7f4a93e41ba6476 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 19:51:15 +0100 Subject: [PATCH 278/624] Add timed wait semaphore --- src/platforms/px4_posix.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index ab53d03900..998342e057 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -62,6 +62,7 @@ typedef struct { __EXPORT int px4_sem_init(px4_sem_t *s, int pshared, unsigned value); __EXPORT int px4_sem_wait(px4_sem_t *s); +__EXPORT int px4_sem_timedwait(px4_sem_t * sem, const struct timespec * abstime); __EXPORT int px4_sem_post(px4_sem_t *s); __EXPORT int px4_sem_getvalue(px4_sem_t *s, int *sval); __EXPORT int px4_sem_destroy(px4_sem_t *s); @@ -76,6 +77,7 @@ typedef sem_t px4_sem_t; #define px4_sem_init sem_init #define px4_sem_wait sem_wait +#define px4_sem_timedwait sem_timedwait #define px4_sem_post sem_post #define px4_sem_getvalue sem_getvalue #define px4_sem_destroy sem_destroy From f42b338f16714ac0093f67f1a4c299d5de7234ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 19:51:45 +0100 Subject: [PATCH 279/624] VDev: Switch to a timed wait semaphore --- src/drivers/device/vdev_posix.cpp | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 745a2acb4c..4d0500c486 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -56,13 +56,6 @@ pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; extern "C" { - static void timer_cb(void *data) - { - px4_sem_t *p_sem = (px4_sem_t *)data; - px4_sem_post(p_sem); - PX4_DEBUG("timer_handler: Timer expired"); - } - #define PX4_MAX_FD 200 static device::file_t *filemap[PX4_MAX_FD] = {}; @@ -269,15 +262,13 @@ extern "C" { if (ret >= 0) { if (timeout > 0) { - // Use a work queue task - work_s _hpwork = {}; - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout); - px4_sem_wait(&sem); + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += timeout / 1000; + ts.tv_nsec += (1000 * 1000 * timeout) % (1000 * 1000 * 1000); - // Make sure timer thread is killed before sem goes - // out of scope - hrt_work_cancel(&_hpwork); + px4_sem_timedwait(&sem, &ts); } else if (timeout < 0) { px4_sem_wait(&sem); From 4ee04e35308b91263479a164fb76a01383372d1d Mon Sep 17 00:00:00 2001 From: DroneBuster Date: Mon, 23 Nov 2015 22:00:28 +0200 Subject: [PATCH 280/624] multirotor mixer: add H configuration --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 3 +++ src/modules/systemlib/mixer/multi_tables.py | 9 ++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 38074f7ae5..d46906385b 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -150,6 +150,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorGeometry::QUAD_X; + } else if (!strcmp(geomname, "4h")) { + geometry = MultirotorGeometry::QUAD_H; + } else if (!strcmp(geomname, "4v")) { geometry = MultirotorGeometry::QUAD_V; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py index e01fcaf8f2..4ca40a2591 100755 --- a/src/modules/systemlib/mixer/multi_tables.py +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -62,6 +62,13 @@ quad_x = [ [135, CW], ] +quad_h = [ + [ 45, CW], + [-135, CW], + [-45, CCW], + [135, CCW], +] + quad_plus = [ [ 90, CCW], [ -90, CCW], @@ -162,7 +169,7 @@ tri_y = [ ] -tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y] +tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y] def variableName(variable): for variableName, value in list(globals().items()): From 27b16375598957121c98d5e0752271deb1561a07 Mon Sep 17 00:00:00 2001 From: DroneBuster Date: Mon, 23 Nov 2015 22:00:56 +0200 Subject: [PATCH 281/624] Mixers: add mixer for H configuration --- ROMFS/px4fmu_common/mixers/quad_h.main.mix | 26 ++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/quad_h.main.mix diff --git a/ROMFS/px4fmu_common/mixers/quad_h.main.mix b/ROMFS/px4fmu_common/mixers/quad_h.main.mix new file mode 100644 index 0000000000..4a177d2ff0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_h.main.mix @@ -0,0 +1,26 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the H configuration. All controls +are mixed 100%. + +R: 4h 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 86a66fe771a2a0c3d3f6c6c77da950202f6230bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 21:50:51 +0100 Subject: [PATCH 282/624] POSIX Formatting change --- src/platforms/px4_posix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 998342e057..fa1c60adee 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -62,7 +62,7 @@ typedef struct { __EXPORT int px4_sem_init(px4_sem_t *s, int pshared, unsigned value); __EXPORT int px4_sem_wait(px4_sem_t *s); -__EXPORT int px4_sem_timedwait(px4_sem_t * sem, const struct timespec * abstime); +__EXPORT int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime); __EXPORT int px4_sem_post(px4_sem_t *s); __EXPORT int px4_sem_getvalue(px4_sem_t *s, int *sval); __EXPORT int px4_sem_destroy(px4_sem_t *s); From a7cb170cc43f661783f59fd7177a465a1b995ec0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Nov 2015 22:05:17 +0100 Subject: [PATCH 283/624] PX4 semaphores: Formatting fix --- src/platforms/posix/px4_layer/px4_sem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 184b80a667..479922f2c1 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -75,7 +75,7 @@ int px4_sem_wait(px4_sem_t *s) return 0; } -int px4_sem_timedwait(px4_sem_t * s, const struct timespec * abstime) +int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) { pthread_mutex_lock(&(s->lock)); s->value--; From da47e8ade8df8fd9b693dd7428eb9236da0393d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 00:11:49 +0100 Subject: [PATCH 284/624] Validator: Reset error state if no error condition triggers --- src/lib/ecl/validation/data_validator.cpp | 22 +++++++++++----------- src/lib/ecl/validation/data_validator.h | 2 +- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 299811ea43..4e55849df0 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -118,34 +118,34 @@ DataValidator::confidence(uint64_t timestamp) if (_time_last == 0) { _error_mask |= ERROR_FLAG_NO_DATA; ret = 0.0f; - } - + /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { + } else if (timestamp - _time_last > _timeout_interval) { _error_mask |= ERROR_FLAG_TIMEOUT; ret = 0.0f; - } /* we got the exact same sensor value N times in a row */ - if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + } else if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { _error_mask |= ERROR_FLAG_STALE_DATA; ret = 0.0f; - } - + /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { + } else if (_error_count > NORETURN_ERRCOUNT) { _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; ret = 0.0f; - } /* cap error density counter at window size */ - if (_error_density > ERROR_DENSITY_WINDOW) { + } else if (_error_density > ERROR_DENSITY_WINDOW) { _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _error_density = ERROR_DENSITY_WINDOW; + + /* no error */ + } else { + _error_mask = ERROR_FLAG_NO_ERROR; } /* no critical errors */ - if(ret > 0.0f) { + if (ret > 0.0f) { /* return local error density for last N measurements */ ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); } diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index 31f6d9d989..c313b1cb04 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -97,7 +97,7 @@ public: * Get the error state of this validator * @return the bitmask with the error status */ - uint32_t state() { return (_error_mask); } + uint32_t state() { return _error_mask; } /** * Reset the error state of this validator From 92e47eba76d4a6a4b1d0647700a0c6d89fcf5829 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 13:13:34 -0800 Subject: [PATCH 285/624] Added tool to decode stack backtraces Signed-off-by: Mark Charlebois --- Tools/decode_backtrace.py | 117 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100755 Tools/decode_backtrace.py diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py new file mode 100755 index 0000000000..469e891110 --- /dev/null +++ b/Tools/decode_backtrace.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2015 Mark Charlebois. 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. +# +############################################################################ + +import os +import subprocess + +# The PX4_BACKTRACE() macro can be used to generate backtraces in the +# running code. These provide the addresses only. This utility can be +# used to decode the backtrace into a readable form: +# +# Example +# If the following lines were pasted into the shell after running decode_backtrace.py +# INFO Backtrace: 10 +# INFO ./mainapp(px4_backtrace+0x27) [0x42b0d1] +# INFO ./mainapp() [0x42d44d] +# INFO ./mainapp() [0x42d3c6] +# +# The output would be: +# 0x42b0d1 _sighandler (0x42b15b) +# 0x42d44d ACCELSIM::devRead(void*, unsigned long) (0x42d4be) +# 0x42d3c6 ACCELSIM::stop() (0x42d442) +# +# The output is: +# () + + +def usage(): + msg = """ +Usage: Tools/decode_backtrace.py + +This will load the symbols for /src/firmware/posix/mainapp +The user just needs to copy and paste the backtrace into the terminal +where decode_backtrace.py is running. + +""" + print msg + +funcaddr = [] +func = [] + +# Load the symbols from the binary +def load_symbol_map(): + output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/mainapp"]) + data = output.split("\n") + data.sort() + + i = 0 + for line in data: + if line.startswith("0"): + funcaddr.append(int(line[0:16],16)) + func.append(line[19:]) + #print(hex(funcaddr[i]), func[i]) + i = i+1 + +# Find the function for the specified call stack address +def lookup(addr): + i=0 + while i < len(funcaddr) and addr > funcaddr[i]: + i=i+1 + if i >= len(funcaddr): + return -1 + return i + +if len(os.sys.argv) != 2: + usage() + raise SystemExit + +load_symbol_map() + +print +print "Paste the backtrace here. Use Ctrl-C to exit." +print +while 1: + # Wait for user to paste the backtrace into the shell + line = raw_input() + tmp = line.split("[") + if len(tmp) == 1: + if len(line.split("Backtrace:")) > 1: + print + continue + addrstr = tmp[1].split("]")[0] + addr = int(addrstr, 16) + idx = lookup(addr) + if idx >= 0: + print hex(addr), func[idx], "({0})".format(hex(funcaddr[idx])) + From 5f4ee6ad9d10312131fb1a1f0173961d235adb19 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 20:16:51 -0800 Subject: [PATCH 286/624] Fixed decode_backtrace.py to return proper index The algorithm was returning the function after the desired function Signed-off-by: Mark Charlebois --- Tools/decode_backtrace.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py index 469e891110..479e0f305a 100755 --- a/Tools/decode_backtrace.py +++ b/Tools/decode_backtrace.py @@ -85,12 +85,14 @@ def load_symbol_map(): # Find the function for the specified call stack address def lookup(addr): - i=0 + i=1 while i < len(funcaddr) and addr > funcaddr[i]: i=i+1 if i >= len(funcaddr): return -1 - return i + + # return the function before this one + return i-1 if len(os.sys.argv) != 2: usage() From 0a8075ee3311581c9d9b2758f95e56deaa8fd79e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 21:29:43 -0800 Subject: [PATCH 287/624] Fixed accelsim Accelsim runs the accel and mag separately now. Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 166 +++++++----------- 1 file changed, 63 insertions(+), 103 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 47da41c627..658eab6e0e 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -111,12 +111,6 @@ private: ACCELSIM_mag *_mag; - struct hrt_call _accel_call; - struct hrt_call _mag_call; - - unsigned _call_accel_interval; - unsigned _call_mag_interval; - ringbuffer::RingBuffer *_accel_reports; ringbuffer::RingBuffer *_mag_reports; @@ -157,26 +151,20 @@ private: // last temperature value float _last_temperature; - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset - /** * Override Start automatic measurement. */ - virtual int start(); + virtual int start(); /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. + * Override Stop automatic measurement. */ - void reset(); + virtual int stop(); /** * Fetch accel measurements from the sensor and update the report ring. */ - virtual void _measure(); + virtual void _measure(); /** * Fetch mag measurements from the sensor and update the report ring. @@ -210,26 +198,6 @@ private: */ int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); - /** - * Set the ACCELSIM internal accel sampling frequency. - * - * @param frequency The internal accel sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int accel_set_samplerate(unsigned frequency); - - /** - * Set the ACCELSIM internal mag sampling frequency. - * - * @param frequency The internal mag sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int mag_set_samplerate(unsigned frequency); - /* this class cannot be copied */ ACCELSIM(const ACCELSIM &); ACCELSIM operator=(const ACCELSIM &); @@ -247,6 +215,9 @@ public: virtual ssize_t devRead(void *buffer, size_t buflen); virtual int devIOCTL(unsigned long cmd, unsigned long arg); + virtual int start(); + virtual int stop(); + protected: friend class ACCELSIM; @@ -268,10 +239,6 @@ private: ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _mag(new ACCELSIM_mag(this)), - _accel_call{}, - _mag_call{}, - _call_accel_interval(0), - _call_mag_interval(0), _accel_reports(nullptr), _mag_reports(nullptr), _accel_scale{}, @@ -326,6 +293,7 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : ACCELSIM::~ACCELSIM() { /* make sure we are truly inactive */ + _mag->stop(); stop(); /* free any existing reports */ @@ -374,8 +342,6 @@ ACCELSIM::init() goto out; } - reset(); - /* do init for the mag device node */ ret = _mag->init(); @@ -413,11 +379,6 @@ out: return ret; } -void -ACCELSIM::reset() -{ -} - int ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { @@ -459,7 +420,7 @@ ACCELSIM::devRead(void *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_call_accel_interval > 0) { + if (m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. */ @@ -498,7 +459,7 @@ ACCELSIM::mag_read(void *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_call_mag_interval > 0) { + if (_mag->m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -539,7 +500,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); - _call_accel_interval = 0; + m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -558,14 +519,11 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / ul_arg; + unsigned interval = 1000000 / ul_arg; /* check against maximum sane rate */ - if (period < 500) { + if (interval < 500) { return -EINVAL; } @@ -573,13 +531,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = period; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } + setSampleInterval(interval); return OK; } @@ -587,11 +539,11 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) { + if (m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return 1000000 / _call_accel_interval; + return 1000000 / m_sample_interval_usecs; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -610,11 +562,12 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) return _accel_reports->size(); case SENSORIOCRESET: - reset(); + // Nothing to do for simulator return OK; case ACCELIOCSSAMPLERATE: - return accel_set_samplerate(ul_arg); + // No need to set internal sampling rate for simulator + return OK; case ACCELIOCGSAMPLERATE: return _accel_samplerate; @@ -671,8 +624,8 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); - _call_mag_interval = 0; + _mag->stop(); + _mag->m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -690,27 +643,16 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_mag_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / ul_arg; + unsigned interval = 1000000 / ul_arg; /* check against maximum sane rate (1ms) */ - if (period < 10000) { + if (interval < 10000) { return -EINVAL; } /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = period; - - //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } + _mag->setSampleInterval(interval); return OK; } @@ -718,11 +660,11 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) { + if (_mag->m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return 1000000 / _call_mag_interval; + return 1000000 / _mag->m_sample_interval_usecs; case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -741,11 +683,12 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) return _mag_reports->size(); case SENSORIOCRESET: - reset(); + // Nothing to do for simulator return OK; case MAGIOCSSAMPLERATE: - return mag_set_samplerate(ul_arg); + // No need to set internal sampling rate for simulator + return OK; case MAGIOCGSAMPLERATE: return _mag_samplerate; @@ -817,41 +760,46 @@ ACCELSIM::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) return OK; } -int -ACCELSIM::accel_set_samplerate(unsigned frequency) -{ - return OK; -} - -int -ACCELSIM::mag_set_samplerate(unsigned frequency) -{ - return OK; -} - int ACCELSIM::start() { + //PX4_INFO("ACCELSIM::start"); /* make sure we are stopped first */ - stop(); + int ret = stop(); + if (ret != 0) { + PX4_ERR("ACCELSIM::start stop failed"); + } /* reset the report ring */ _accel_reports->flush(); _mag_reports->flush(); - return VirtDevObj::start(); + int ret2 = VirtDevObj::start(); + if (ret2 != 0) { + PX4_ERR("ACCELSIM::start base class start failed"); + } + return (ret != 0 || ret2 != 0) ? -1 : 0; +} + +int +ACCELSIM::stop() +{ + //PX4_INFO("ACCELSIM::stop"); + //PX4_BACKTRACE(); + return VirtDevObj::stop(); } void ACCELSIM::_measure() { + //PX4_INFO("ACCELSIM::_measure"); /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; uint8_t status; - float temperature; + float temperature; float x; float y; float z; @@ -1052,9 +1000,21 @@ ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) } } -void -ACCELSIM_mag::_measure() +int ACCELSIM_mag::start() { + //PX4_INFO("ACCELSIM_mag::start"); + return VirtDevObj::start(); +} + +int ACCELSIM_mag::stop() +{ + //PX4_INFO("ACCELSIM_mag::stop"); + return VirtDevObj::stop(); +} + +void ACCELSIM_mag::_measure() +{ + //PX4_INFO("ACCELSIM_mag::_measure"); _parent->mag_measure(); } From 08714d2c751da1be531f05af02fda6d9b1f39681 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 21:31:50 -0800 Subject: [PATCH 288/624] Fixed whitespace Signed-off-by: Mark Charlebois --- Tools/decode_backtrace.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py index 479e0f305a..b085000087 100755 --- a/Tools/decode_backtrace.py +++ b/Tools/decode_backtrace.py @@ -35,7 +35,7 @@ import os import subprocess -# The PX4_BACKTRACE() macro can be used to generate backtraces in the +# The PX4_BACKTRACE() macro can be used to generate backtraces in the # running code. These provide the addresses only. This utility can be # used to decode the backtrace into a readable form: # From be0c5aaef57fa3be08991d01f17783b426f51e9f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 21:37:55 -0800 Subject: [PATCH 289/624] Fixed documentation for decode_backtrace.py Signed-off-by: Mark Charlebois --- Tools/decode_backtrace.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py index b085000087..62a9c16b46 100755 --- a/Tools/decode_backtrace.py +++ b/Tools/decode_backtrace.py @@ -39,19 +39,23 @@ import subprocess # running code. These provide the addresses only. This utility can be # used to decode the backtrace into a readable form: # -# Example +# Example: # If the following lines were pasted into the shell after running decode_backtrace.py +# # INFO Backtrace: 10 -# INFO ./mainapp(px4_backtrace+0x27) [0x42b0d1] -# INFO ./mainapp() [0x42d44d] -# INFO ./mainapp() [0x42d3c6] +# INFO ./mainapp(px4_backtrace+0x27) [0x42b212] +# INFO ./mainapp() [0x42d608] +# INFO ./mainapp() [0x42d57e] +# INFO ./mainapp() [0x4ba48d] # # The output would be: -# 0x42b0d1 _sighandler (0x42b15b) -# 0x42d44d ACCELSIM::devRead(void*, unsigned long) (0x42d4be) -# 0x42d3c6 ACCELSIM::stop() (0x42d442) # -# The output is: +# 0x42b212 px4_backtrace (0x42b1eb) +# 0x42d608 ACCELSIM::devRead(void*, unsigned long) (0x42d600) +# 0x42d57e ACCELSIM::start() (0x42d578) +# 0x4ba48d DriverFramework::DevObj::addHandle(DriverFramework::DevHandle&) (0x4ba42e) +# +# The output format is: # () From 1aeca4f64d8052c48d0b8d1eb861139a1db0eb2f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 23:09:10 -0800 Subject: [PATCH 290/624] Fixed code format and removed unused std::list Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 4 +++- src/platforms/posix/px4_layer/px4_sem.cpp | 2 -- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 658eab6e0e..b15a9c08c9 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -766,6 +766,7 @@ ACCELSIM::start() //PX4_INFO("ACCELSIM::start"); /* make sure we are stopped first */ int ret = stop(); + if (ret != 0) { PX4_ERR("ACCELSIM::start stop failed"); } @@ -775,9 +776,11 @@ ACCELSIM::start() _mag_reports->flush(); int ret2 = VirtDevObj::start(); + if (ret2 != 0) { PX4_ERR("ACCELSIM::start base class start failed"); } + return (ret != 0 || ret2 != 0) ? -1 : 0; } @@ -785,7 +788,6 @@ int ACCELSIM::stop() { //PX4_INFO("ACCELSIM::stop"); - //PX4_BACKTRACE(); return VirtDevObj::stop(); } diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 479922f2c1..84795eb16a 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -48,8 +48,6 @@ #include -#include - int px4_sem_init(px4_sem_t *s, int pshared, unsigned value) { // We do not used the process shared arg From 36208255bdc34d969d7d04463ae668681a1e7031 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 23 Nov 2015 23:23:02 -0800 Subject: [PATCH 291/624] Address Sanitizer found delete vs delete[] issue mavlink_main.cpp used new char[n] and delete vs delete[]. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b04eea1a0..e04f64e293 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1232,7 +1232,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) usleep(MAIN_LOOP_DELAY / 2); } while (_subscribe_to_stream != nullptr); - delete s; + delete[] s; } } From 6ccdc2c6fad7e98c887faba9c08d95d546248c93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 08:52:29 +0100 Subject: [PATCH 292/624] Fix Travis --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index a66f6b2f27..35747f01b9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -59,7 +59,7 @@ before_install: ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 - && brew update + && brew update; brew update && && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends From 92d701a1fec1306e9c163480de4b13f05f360354 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 09:03:55 +0100 Subject: [PATCH 293/624] Fix Travis syntax --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 35747f01b9..e7c3119377 100644 --- a/.travis.yml +++ b/.travis.yml @@ -59,7 +59,7 @@ before_install: ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 - && brew update; brew update && + && brew update; brew update && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends From d4c8be39333c81ba54944a63ac51b30509576995 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 09:29:00 +0100 Subject: [PATCH 294/624] Update driver framework with fixed DF list deletion --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 25b5597081..c06bbb16cf 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 25b559708140ecd56466adb7e30b29d002766e0b +Subproject commit c06bbb16cf529de0c09d332bb85d49004ce3c1c9 From 99921db5cfbb5bcde4a4bf6134a895dbb2a25a2a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 15:34:51 -1000 Subject: [PATCH 295/624] Temporary (or not) fix calibration failure due Due to the uOrb publishing rate difference beteen the mpu9250 driver and the mpu6000 driver on the ICM-20608-G. The latter driver has an integrator output the limit the publishing rate --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 1fcd48b088..a18a13dfa0 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -137,7 +137,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } for (unsigned s = 0; s < max_gyros; s++) { - if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { + if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 4) { mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } From fff75f60296a5eb06deb73375ca40db9f39e6b31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 11:48:42 +0100 Subject: [PATCH 296/624] Travis CI: Workaround for Homebrew Emacs fail --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2839e52437..e55e2cd152 100644 --- a/.travis.yml +++ b/.travis.yml @@ -59,7 +59,7 @@ before_install: ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 - && brew update + && brew update; brew update && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends From b7dfa3a9d0a592e178d3f57e8c3966bccbf22afc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:58:14 +0100 Subject: [PATCH 297/624] made inav compile with c++ --- src/modules/position_estimator_inav/CMakeLists.txt | 9 ++++++--- .../{inertial_filter.c => inertial_filter.cpp} | 0 ...nav_main.c => position_estimator_inav_main.cpp} | 14 +++++++------- ...params.c => position_estimator_inav_params.cpp} | 0 4 files changed, 13 insertions(+), 10 deletions(-) rename src/modules/position_estimator_inav/{inertial_filter.c => inertial_filter.cpp} (100%) rename src/modules/position_estimator_inav/{position_estimator_inav_main.c => position_estimator_inav_main.cpp} (99%) rename src/modules/position_estimator_inav/{position_estimator_inav_params.c => position_estimator_inav_params.cpp} (100%) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 60429f5d22..2f14727509 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -30,18 +30,21 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(MODULE_CFLAGS ) if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() + px4_add_module( MODULE modules__position_estimator_inav MAIN position_estimator_inav STACK 1200 COMPILE_FLAGS ${MODULE_CFLAGS} SRCS - position_estimator_inav_main.c - position_estimator_inav_params.c - inertial_filter.c + position_estimator_inav_main.cpp + position_estimator_inav_params.cpp + inertial_filter.cpp DEPENDS platforms__common ) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.cpp similarity index 100% rename from src/modules/position_estimator_inav/inertial_filter.c rename to src/modules/position_estimator_inav/inertial_filter.cpp diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp similarity index 99% rename from src/modules/position_estimator_inav/position_estimator_inav_main.c rename to src/modules/position_estimator_inav/position_estimator_inav_main.cpp index d8f29f1082..7316da727f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -95,7 +95,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distan static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s -__EXPORT int position_estimator_inav_main(int argc, char *argv[]); +extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]); int position_estimator_inav_thread_main(int argc, char *argv[]); @@ -390,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); - px4_pollfd_struct_t fds_init[1] = { - { .fd = sensor_combined_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds_init[1]; + fds_init[0].fd = sensor_combined_sub; + fds_init[0].events = POLLIN; /* wait for initial baro value */ bool wait_baro = true; @@ -434,9 +434,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* main loop */ - px4_pollfd_struct_t fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds[1]; + fds[0].fd = vehicle_attitude_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp similarity index 100% rename from src/modules/position_estimator_inav/position_estimator_inav_params.c rename to src/modules/position_estimator_inav/position_estimator_inav_params.cpp From a87ffe9bf3e7dbc81859e7e7297acfdfb2f81d1a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 23:17:44 +0100 Subject: [PATCH 298/624] added terrain estimator to inav --- .../position_estimator_inav_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 7316da727f..f97b7e944e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -74,6 +74,7 @@ #include #include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -397,6 +398,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* wait for initial baro value */ bool wait_baro = true; + TerrainEstimator *terrain_estimator = new TerrainEstimator(); + thread_running = true; while (wait_baro && !thread_should_exit) { @@ -1228,6 +1231,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } + /* run terrain estimator */ + terrain_estimator->predict(dt, &att, &sensor, &lidar); + terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att); + if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1318,6 +1325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.eph = eph; global_pos.epv = epv; + if (terrain_estimator->is_valid()) { + global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); + global_pos.terrain_alt_valid = true; + } else { + global_pos.terrain_alt_valid = false; + } + if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); From 67dd28e2c4e4e6fb39c345e74c40759baa3f5500 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 24 Nov 2015 10:30:18 +0100 Subject: [PATCH 299/624] update distance sensor separate from flow --- .../position_estimator_inav_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index f97b7e944e..57a118a9e4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -526,9 +526,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_check(optical_flow_sub, &updated); orb_check(distance_sensor_sub, &updated2); + /* update lidar separately, needed by terrain estimator */ + if (updated2) { + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); + } + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; From 6bec773423747f24fa815fdbea7eb9c279dd8e33 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 24 Nov 2015 13:37:06 +0100 Subject: [PATCH 300/624] changed isfinite to PX4_ISFINITE --- .../position_estimator_inav_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 57a118a9e4..72b250ebe9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1048,7 +1048,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += R_gps[j][i] * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1088,7 +1088,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1113,7 +1113,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1121,7 +1121,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1148,7 +1148,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1167,7 +1167,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1214,7 +1214,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); From 8ddc7a27c73475f1a8d4b5c5c98f0b3dd95e2189 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 18 Nov 2015 23:55:37 +0100 Subject: [PATCH 301/624] implemented altitude message draft --- src/modules/mavlink/mavlink_messages.cpp | 84 ++++++++++++++++++++++++ 1 file changed, 84 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index be3ab850f4..a4926aaed6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2537,6 +2537,89 @@ protected: } }; +class MavlinkStreamAltitude : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAltitude::get_name_static(); + } + + static const char *get_name_static() + { + return "ALTITUDE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ALTITUDE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAltitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time; + + MavlinkOrbSubscription *_local_pos_sub; + uint64_t _local_pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; + + /* do not allow top copying this class */ + MavlinkStreamAltitude(MavlinkStreamAltitude &); + MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &); + +protected: + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _global_pos_time(0), + _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _local_pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_global_position_s global_pos; + struct vehicle_local_position_s local_pos; + struct home_position_s home; + + bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos); + updated |= _local_pos_sub->update(&_local_pos_time, &local_pos); + updated |= _home_sub->update(&_home_time, &home); + + if (updated) { + mavlink_altitude_t msg; + + msg.altitude_monotonic = global_pos.alt; + msg.altitude_amsl = global_pos.alt; + msg.altitude_local = -local_pos.z; + msg.altitude_relative = home.alt; + + if (global_pos.terrain_alt_valid) { + msg.altitude_terrain = global_pos.terrain_alt; + msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt; + } else { + msg.altitude_terrain = -1001; + msg.bottom_clearance = -1; + } + + _mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); + } + } +}; + const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), @@ -2574,5 +2657,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), + new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static), nullptr }; From d9c332353aa24b56da96650fa0014541c8df2eb8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 18 Nov 2015 23:59:08 +0100 Subject: [PATCH 302/624] added altitude stream to configs --- src/modules/mavlink/mavlink_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b04eea1a0..3e6fc97845 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1689,6 +1689,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("ALTITUDE", 1.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1715,6 +1716,7 @@ Mavlink::task_main(int argc, char *argv[]) //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("ALTITUDE", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1730,6 +1732,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("ALTITUDE", 1.0f); break; case MAVLINK_MODE_MAGIC: From f5a844d4aa6c732c7aed3c5ffbc54062ef89c9c2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 20 Nov 2015 21:01:35 +0100 Subject: [PATCH 303/624] added field for pressure altitude to global position --- msg/vehicle_global_position.msg | 1 + src/modules/mavlink/mavlink_messages.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 6a38e2c9d7..cee381e871 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -18,3 +18,4 @@ float32 epv # Standard deviation of position vertically float32 terrain_alt # Terrain altitude in m, WGS84 bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning +float32 pressure_alt # Pressure altitude diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4926aaed6..b6653a83e0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2602,7 +2602,7 @@ protected: if (updated) { mavlink_altitude_t msg; - msg.altitude_monotonic = global_pos.alt; + msg.altitude_monotonic = global_pos.pressure_alt; msg.altitude_amsl = global_pos.alt; msg.altitude_local = -local_pos.z; msg.altitude_relative = home.alt; From 11d9e3a325bfdfebe4469b7b86a76822d35412f2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 20 Nov 2015 21:47:12 +0100 Subject: [PATCH 304/624] also adding message to usb config --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3e6fc97845..39b8a5c730 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1763,6 +1763,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 20.0f); configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("ALTITUDE", 10.0f); default: break; From c706c30e83735af6bfcbcd7ac4bee5bcc34df1cb Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 16:17:07 +0100 Subject: [PATCH 305/624] fill pressure altitude in global position from estimators --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 3 +++ .../local_position_estimator/BlockLocalPositionEstimator.cpp | 1 + .../position_estimator_inav/position_estimator_inav_main.cpp | 2 ++ 3 files changed, 6 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 24a198f0a5..23bc784daf 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1015,6 +1015,9 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.terrain_alt_valid = false; } + /* baro altitude */ + _global_pos.pressure_alt = _baro_alt_filt; + _global_pos.yaw = _local_pos.yaw; const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index d1262f9177..8b457a68db 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -711,6 +711,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().terrain_alt = 0; _pub_gpos.get().terrain_alt_valid = false; _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; _pub_gpos.update(); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 72b250ebe9..90bf30c606 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1336,6 +1336,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.terrain_alt_valid = false; } + global_pos.pressure_alt = sensor.baro_alt_meter[0]; + if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); From 2ceaab138f5de756fd5a993f9c2a3e1e5ea87139 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 21:19:24 +0100 Subject: [PATCH 306/624] added timestamp for altitude message --- src/modules/mavlink/mavlink_messages.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b6653a83e0..97fb3a81bf 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2602,6 +2602,8 @@ protected: if (updated) { mavlink_altitude_t msg; + msg.time_usec = hrt_absolute_time(); + msg.altitude_monotonic = global_pos.pressure_alt; msg.altitude_amsl = global_pos.alt; msg.altitude_local = -local_pos.z; From 82cf8d6ecd722747a7997355ba89c1932606b918 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:30:55 +0100 Subject: [PATCH 307/624] switch to latest mavlink --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 86a2b147e4..db5cf66d1a 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 86a2b147e4cec5066efdc6d030d69d902929e66f +Subproject commit db5cf66d1a7f549ccbaa92b5476e78754e008297 From 59f80581aa4be158a956d60ea71d45f5bb78a558 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:35:03 +0100 Subject: [PATCH 308/624] switch to _ekf->baroHgt for pressure alt in ekf --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 23bc784daf..9727f21c06 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1016,7 +1016,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* baro altitude */ - _global_pos.pressure_alt = _baro_alt_filt; + _global_pos.pressure_alt = _ekf->baroHgt; _global_pos.yaw = _local_pos.yaw; From 1de44036868dcd5a97fa87a54c6c7cbb30ea3507 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 14:23:42 +0100 Subject: [PATCH 309/624] Enable perf command --- cmake/configs/posix_sitl_default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index a177e3a2f8..b3c9205586 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -24,6 +24,7 @@ set(config_module_list systemcmds/esc_calib systemcmds/reboot systemcmds/topic_listener + systemcmds/perf modules/uORB modules/param modules/systemlib From bd4497f8837c6a68d162ab09de633353e54dfd89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 14:23:59 +0100 Subject: [PATCH 310/624] Simulator: Add performance counters for delay --- src/modules/simulator/simulator.h | 16 +++++++++++++++ src/modules/simulator/simulator_mavlink.cpp | 22 +++++++++++++++------ 2 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 6f9c35bb95..182be7916b 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -222,6 +223,13 @@ private: _mag(1), _gps(1), _airspeed(1), + _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay")), + _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")), + _perf_baro(perf_alloc_once(PC_ELAPSED, "sim_baro_delay")), + _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay")), + _perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")), + _perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")), + _perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -255,6 +263,14 @@ private: simulator::Report _gps; simulator::Report _airspeed; + perf_counter_t _perf_accel; + perf_counter_t _perf_mpu; + perf_counter_t _perf_baro; + perf_counter_t _perf_mag; + perf_counter_t _perf_gps; + perf_counter_t _perf_airspeed; + perf_counter_t _perf_sim_delay; + // uORB publisher handlers orb_advert_t _accel_pub; orb_advert_t _baro_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index e712e65be0..2f37eb2d63 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -182,6 +182,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) mpu.gyro_z = imu->zgyro; write_MPU_data((void *)&mpu); + perf_begin(_perf_mpu); RawAccelData accel = {}; accel.x = imu->xacc; @@ -189,6 +190,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) accel.z = imu->zacc; write_accel_data((void *)&accel); + perf_begin(_perf_accel); RawMagData mag = {}; mag.x = imu->xmag; @@ -196,6 +198,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) mag.z = imu->zmag; write_mag_data((void *)&mag); + perf_begin(_perf_mag); RawBaroData baro = {}; // calculate air pressure from altitude (valid for low altitude) @@ -235,14 +238,23 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_SENSOR: + { mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); + uint64_t sim_timestamp = imu.time_usec; + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; + + perf_set(_perf_sim_delay, timestamp - sim_timestamp); + if (publish) { publish_sensor_topics(&imu); } update_sensors(&imu); + } break; case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: @@ -379,12 +391,12 @@ void Simulator::initializeSensorData() RawMPUData mpu = {}; mpu.accel_z = 9.81f; - write_MPU_data((void *)&mpu); + write_MPU_data(&mpu); RawAccelData accel = {}; accel.z = 9.81f; - write_accel_data((void *)&accel); + write_accel_data(&accel); RawMagData mag = {}; mag.x = 0.4f; @@ -399,11 +411,11 @@ void Simulator::initializeSensorData() baro.altitude = 0.0f; baro.temperature = 25.0f; - write_baro_data((void *)&baro); + write_baro_data(&baro); RawAirspeedData airspeed {}; - write_airspeed_data((void *)&airspeed); + write_airspeed_data(&airspeed); } void Simulator::pollForMAVLinkMessages(bool publish) @@ -664,8 +676,6 @@ int openUart(const char *uart_name, int baud) int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) { - - //uint64_t timestamp = imu->time_usec; uint64_t timestamp = hrt_absolute_time(); if ((imu->fields_updated & 0x1FFF) != 0x1FFF) { From 17caae00aac16338af46c8481c5f3fd9257aa0b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 14:24:18 +0100 Subject: [PATCH 311/624] Attitude estimator Q: Add performance counters for delay --- .../attitude_estimator_q_main.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index ab87014d9a..d64d8b67c9 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -287,6 +287,13 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) void AttitudeEstimatorQ::task_main() { + +#ifdef __PX4_POSIX + perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay")); + perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")); + perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay")); +#endif + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); @@ -381,6 +388,12 @@ void AttitudeEstimatorQ::task_main() if (!_failsafe) { uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; + #ifdef __PX4_POSIX + perf_end(_perf_accel); + perf_end(_perf_mpu); + perf_end(_perf_mag); + #endif + if (_voter_gyro.failover_count() > 0) { _failsafe = true; flags = _voter_gyro.failover_state(); From 78ace92530894ac767714b05c75021d27787b6a9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 24 Nov 2015 16:57:19 +0100 Subject: [PATCH 312/624] reset position hold flag --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a26a65d468..6776d78871 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1141,6 +1141,16 @@ MulticopterPositionControl::task_main() _run_pos_control = true; _run_alt_control = true; + // reset the horizontal and vertical position hold flags for non-manual modes + // or if position is not controlled + if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { + _pos_hold_engaged = false; + } + + if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { + _alt_hold_engaged = false; + } + /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ From 7a31a8b1af1a34aa7178564e9284ccc9441d7b6e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 26 Aug 2015 15:59:09 +0200 Subject: [PATCH 313/624] implemented VEHICLE_CMD_DO_MOUNT_CONFIGURE and added possibility to set mount mode via RC switch when not in offboard use output 5 for mount mode (e.g. to support landing gears) --- src/drivers/gimbal/CMakeLists.txt | 1 + src/drivers/gimbal/gimbal.cpp | 134 +++++++++++++++++++++++++++-- src/drivers/gimbal/gimbal_params.c | 60 +++++++++++++ 3 files changed, 190 insertions(+), 5 deletions(-) create mode 100644 src/drivers/gimbal/gimbal_params.c diff --git a/src/drivers/gimbal/CMakeLists.txt b/src/drivers/gimbal/CMakeLists.txt index ac26a1876e..4a13480cd1 100644 --- a/src/drivers/gimbal/CMakeLists.txt +++ b/src/drivers/gimbal/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( -Os SRCS gimbal.cpp + gimbal_params.c DEPENDS platforms__common ) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 40b86d1421..96eaa1becf 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 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 +35,7 @@ * @file gimbal.cpp * @author Lorenz Meier * @author Anton Matosov + * @author Andreas Antener * * Driver to control a gimbal - relies on input via telemetry or RC * and output via the standardized control group #2 and a mixer. @@ -72,6 +73,8 @@ #include #include #include +#include +#include #include #include @@ -120,6 +123,9 @@ private: work_s _work; int _vehicle_command_sub; int _att_sub; + int _vehicle_control_mode_sub; + int _manual_control_sub; + int _params_sub; bool _attitude_compensation_roll; bool _attitude_compensation_pitch; @@ -127,6 +133,7 @@ private: bool _initialized; bool _control_cmd_set; bool _config_cmd_set; + int _mount_mode; // see MAV_MOUNT_MODE orb_advert_t _actuator_controls_2_topic; @@ -136,6 +143,16 @@ private: struct vehicle_command_s _control_cmd; struct vehicle_command_s _config_cmd; + struct manual_control_setpoint_s _manual_control; + struct vehicle_control_mode_s _control_mode; + + struct { + int aux_mnt_chn; + } _parameters; + + struct { + param_t aux_mnt_chn; + } _params_handles; /** * Initialise the automatic measurement state machine and start it. @@ -150,6 +167,11 @@ private: */ void stop(); + /** + * Update parameters + */ + void update_params(); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -176,6 +198,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), + _vehicle_control_mode_sub(-1), + _manual_control_sub(-1), + _params_sub(-1), _attitude_compensation_roll(true), _attitude_compensation_pitch(true), _attitude_compensation_yaw(true), @@ -188,6 +213,9 @@ Gimbal::Gimbal() : // disable debug() calls _debug_enabled = false; + _params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN"); + update_params(); + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -267,12 +295,22 @@ Gimbal::cycle_trampoline(void *arg) dev->cycle(); } +void +Gimbal::update_params() +{ + param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn); +} + void Gimbal::cycle() { if (!_initialized) { - /* get a subscription handle on the vehicle command topic */ + /* get subscription handles */ _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); /* get a publication handle on actuator output topic */ struct actuator_controls_s zero_report; @@ -284,6 +322,8 @@ Gimbal::cycle() warnx("advert err"); } + _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT; + _initialized = true; } @@ -294,13 +334,31 @@ Gimbal::cycle() float roll = 0.0f; float pitch = 0.0f; float yaw = 0.0f; + float out_mount_mode = 0.0f; + /* Parameter update */ + bool params_updated; - if (_att_sub < 0) { - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_check(_params_sub, ¶ms_updated); + + if (params_updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); // XXX: is this actually necessary? + + update_params(); } - vehicle_attitude_s att; + /* Control mode update */ + bool vehicle_control_mode_updated; + + orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated); + + if (vehicle_control_mode_updated) { + orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); + } + + /* Check attitude */ + struct vehicle_attitude_s att; orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); @@ -319,7 +377,45 @@ Gimbal::cycle() updated = true; } + /* Check manual input */ + bool manual_updated; + orb_check(_manual_control_sub, &manual_updated); + + if (manual_updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control); + + /* only check manual input for mount mode when not in offboard and aux chan is configured */ + if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) { + float aux = 0.0f; + + switch (_parameters.aux_mnt_chn) { + case 1: + aux = _manual_control.aux1; + break; + + case 2: + aux = _manual_control.aux2; + break; + + case 3: + aux = _manual_control.aux3; + break; + } + + if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) { + _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT; + updated = true; + } + + if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) { + _mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; + updated = true; + } + } + } + + /* Check command input */ struct vehicle_command_s cmd; bool cmd_updated; @@ -353,6 +449,13 @@ Gimbal::cycle() _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; + /* only check commanded mount mode when in offboard */ + if (_control_mode.flag_control_offboard_enabled && + fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) { + _mount_mode = int(_config_cmd.param1 + 0.5f); + updated = true; + } + } if (_control_cmd_set) { @@ -385,6 +488,25 @@ Gimbal::cycle() } + switch (_mount_mode) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + out_mount_mode = -1.0f; + roll = 0.0f; + pitch = 0.0f; + yaw = 0.0f; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + out_mount_mode = 1.0f; + break; + + default: + out_mount_mode = -1.0f; + } + if (updated) { struct actuator_controls_s controls; @@ -396,6 +518,8 @@ Gimbal::cycle() controls.control[0] = roll; controls.control[1] = pitch; controls.control[2] = yaw; + //controls.control[3] = ; // camera shutter + controls.control[4] = out_mount_mode; /* publish it */ orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls); diff --git a/src/drivers/gimbal/gimbal_params.c b/src/drivers/gimbal/gimbal_params.c new file mode 100644 index 0000000000..6fcd31cb82 --- /dev/null +++ b/src/drivers/gimbal/gimbal_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 gimbal_params.c + * + * Parameters for the gimbal controller. + * + * @author Andreas Antener + */ + +#include + +#include + + +/** + * Auxilary switch to use for mount control. + * + * Set to 0 to disable manual mount control. + * + * Mount control off means the gimbal is put into safe/locked position. + * Mount control on means the gimbal can move freely, and landing gear + * will be retracted if applicable. + * + * @min 0 + * @max 3 + * @group Gimbal + */ +PARAM_DEFINE_INT32(GMB_AUX_MNT_CHN, 0); From 9252414c76bde863ec014ce2ed27241d187929c9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 19 Oct 2015 21:47:40 +0200 Subject: [PATCH 314/624] added new parameter to enable/disable enforcing mount operation mode --- src/drivers/gimbal/gimbal.cpp | 37 ++++++++++++++++++------------ src/drivers/gimbal/gimbal_params.c | 19 +++++++++++---- 2 files changed, 37 insertions(+), 19 deletions(-) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 96eaa1becf..bb76d1229b 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -148,10 +148,12 @@ private: struct { int aux_mnt_chn; + int use_mnt; } _parameters; struct { param_t aux_mnt_chn; + param_t use_mnt; } _params_handles; /** @@ -214,6 +216,7 @@ Gimbal::Gimbal() : _debug_enabled = false; _params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN"); + _params_handles.use_mnt = param_find("GMB_USE_MNT"); update_params(); // work_cancel in the dtor will explode if we don't do this... @@ -299,6 +302,7 @@ void Gimbal::update_params() { param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn); + param_get(_params_handles.use_mnt, &_parameters.use_mnt); } void @@ -488,23 +492,26 @@ Gimbal::cycle() } - switch (_mount_mode) { - case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: - out_mount_mode = -1.0f; - roll = 0.0f; - pitch = 0.0f; - yaw = 0.0f; - break; + /* consider mount mode if parameter is set */ + if (_parameters.use_mnt > 0) { + switch (_mount_mode) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + out_mount_mode = -1.0f; + roll = 0.0f; + pitch = 0.0f; + yaw = 0.0f; + break; - case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: - case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: - case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: - case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: - out_mount_mode = 1.0f; - break; + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + out_mount_mode = 1.0f; + break; - default: - out_mount_mode = -1.0f; + default: + out_mount_mode = -1.0f; + } } if (updated) { diff --git a/src/drivers/gimbal/gimbal_params.c b/src/drivers/gimbal/gimbal_params.c index 6fcd31cb82..955bfa8b23 100644 --- a/src/drivers/gimbal/gimbal_params.c +++ b/src/drivers/gimbal/gimbal_params.c @@ -43,14 +43,25 @@ #include +/** + * Consider mount operation mode. + * + * If set to 1, mount mode will be enforced. + * + * @min 0 + * @max 1 + * @group Gimbal + */ +PARAM_DEFINE_INT32(GMB_USE_MNT, 0); /** - * Auxilary switch to use for mount control. + * Auxilary switch to set mount operation mode. * - * Set to 0 to disable manual mount control. + * Set to 0 to disable manual mode control. * - * Mount control off means the gimbal is put into safe/locked position. - * Mount control on means the gimbal can move freely, and landing gear + * If set to an auxilary switch: + * Switch off means the gimbal is put into safe/locked position. + * Switch on means the gimbal can move freely, and landing gear * will be retracted if applicable. * * @min 0 From 483cb1115711d252cb8e7b3bf06acf06734fce9c Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 23 Oct 2015 11:18:09 -0400 Subject: [PATCH 315/624] Fix comparison error in RC+GPS triggered FTS failsafe check This bug would cause loss of RC + loss of GPS to trigger a FTS when flying in non-manual modes with a good data link --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 49e6459b03..851529bb54 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2320,7 +2320,7 @@ int commander_thread_main(int argc, char *argv[]) if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || - status.main_state !=vehicle_status_s::MAIN_STATE_STAB || + status.main_state ==vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || From 56a68094865939006c7e709ac42152e03660ec21 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 24 Nov 2015 18:05:42 -0500 Subject: [PATCH 316/624] HIL calculate TAS from IAS --- src/modules/mavlink/mavlink_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c2180a4d8..bf79b6e09f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1325,8 +1325,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; + float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature); airspeed.timestamp = timestamp; airspeed.indicated_airspeed_m_s = ias; From f3126e9d3a2eff6d0e9dea0d88abb5a5d1ea7d59 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 24 Nov 2015 22:46:20 -0800 Subject: [PATCH 317/624] Fixes for DriverFramework, accelsim and gyrosim DriverFramework was updated to properly delete nodes in managed lists. Baro was fixed to use DriverFramework. Accelsim was fixed with change to DriverFramework to return < 0 when start() is called and the driver was already started, or stop() is called but the driver was not running. Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- .../posix/drivers/accelsim/accelsim.cpp | 18 +- src/platforms/posix/drivers/barosim/baro.cpp | 223 +++++------------- .../posix/drivers/barosim/baro_sim.cpp | 144 +++-------- src/platforms/posix/drivers/barosim/barosim.h | 37 ++- .../posix/drivers/gyrosim/gyrosim.cpp | 13 +- src/systemcmds/mixer/CMakeLists.txt | 4 +- 7 files changed, 150 insertions(+), 291 deletions(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c06bbb16cf..2ae8d8118d 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c06bbb16cf529de0c09d332bb85d49004ce3c1c9 +Subproject commit 2ae8d8118db0e95867cd1946d5b8b85d7e4ef9d3 diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index b15a9c08c9..030791415c 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -530,9 +530,14 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* adjust filters */ accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); + bool want_start = (m_sample_interval_usecs == 0); + /* update interval for next measurement */ setSampleInterval(interval); + if (want_start) { + start(); + } return OK; } } @@ -651,9 +656,14 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) return -EINVAL; } + bool want_start = (_mag->m_sample_interval_usecs == 0); + /* update interval for next measurement */ _mag->setSampleInterval(interval); + if (want_start) { + _mag->start(); + } return OK; } } @@ -765,11 +775,7 @@ ACCELSIM::start() { //PX4_INFO("ACCELSIM::start"); /* make sure we are stopped first */ - int ret = stop(); - - if (ret != 0) { - PX4_ERR("ACCELSIM::start stop failed"); - } + stop(); /* reset the report ring */ _accel_reports->flush(); @@ -781,7 +787,7 @@ ACCELSIM::start() PX4_ERR("ACCELSIM::start base class start failed"); } - return (ret != 0 || ret2 != 0) ? -1 : 0; + return (ret2 != 0) ? -1 : 0; } int diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 15c0882aea..37cc5656c9 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -68,16 +68,13 @@ #include "barosim.h" #include "VirtDevObj.hpp" -enum BAROSIM_BUS { - BAROSIM_BUS_SIM_EXTERNAL -}; - /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* helper macro for arithmetic - returns the square of the argument */ #define POW2(_x) ((_x) * (_x)) +#define BAROSIM_DEV_PATH "/dev/barosim" /* * BAROSIM internal constants and data structures. */ @@ -89,7 +86,7 @@ enum BAROSIM_BUS { class BAROSIM : public VirtDevObj { public: - BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path); + BAROSIM(const char *path); ~BAROSIM(); virtual int init(); @@ -103,12 +100,9 @@ public: void print_info(); protected: - DevObj *_interface; - - barosim::prom_s _prom; + BAROSIM_DEV *_interface; struct work_s _work; - unsigned _measure_ticks; ringbuffer::RingBuffer *_reports; @@ -127,7 +121,6 @@ protected: orb_advert_t _baro_topic; int _orb_class_instance; - int _class_instance; perf_counter_t _sample_perf; perf_counter_t _measure_perf; @@ -185,7 +178,7 @@ protected: int measure(); // Unused - virtual void _measure() {} + virtual void _measure(); /** * Collect the result of the most recent measurement. @@ -193,16 +186,16 @@ protected: virtual int collect(); }; +static BAROSIM *g_barosim = nullptr; + /* * Driver 'main' command. */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) : +BAROSIM::BAROSIM(const char *path) : VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100), - _interface(interface), - _prom(prom_buf.s), - _measure_ticks(0), + _interface(nullptr), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -212,7 +205,6 @@ BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) _msl_pressure(101325), _baro_topic(nullptr), _orb_class_instance(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")), _measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")), _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")), @@ -220,6 +212,8 @@ BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) { // work_cancel in stop_cycle called from the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + + _interface = new BAROSIM_DEV; } BAROSIM::~BAROSIM() @@ -334,7 +328,7 @@ BAROSIM::devRead(void *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (m_sample_interval_usecs > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -396,7 +390,6 @@ BAROSIM::devRead(void *buffer, size_t buflen) int BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { - switch (cmd) { case SENSORIOCSPOLLRATE: @@ -405,7 +398,8 @@ BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); - _measure_ticks = 0; + setSampleInterval(0); + m_sample_interval_usecs = 0; return OK; /* external signalling not supported */ @@ -418,39 +412,27 @@ BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start_cycle(); - } - + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return OK; } /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned long ticks = USEC2TICK(1000000 / arg); + unsigned long interval = 1000000 / arg; /* check against maximum rate */ - if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { + if (interval < BAROSIM_CONVERSION_INTERVAL) { return -EINVAL; } - /* update interval for next measurement */ - _measure_ticks = ticks; + bool want_start = (m_sample_interval_usecs == 0); + + /* update interval for next measurement */ + setSampleInterval(interval); - /* if we need to start the poll state machine, do it */ if (want_start) { - start_cycle(); + start(); } return OK; @@ -458,11 +440,11 @@ BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { + if (m_sample_interval_usecs == 0) { return SENSOR_POLLRATE_MANUAL; } - return (1000 / _measure_ticks); + return (1000000 / m_sample_interval_usecs); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -519,21 +501,20 @@ BAROSIM::start_cycle() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&BAROSIM::cycle_trampoline, this, 1); + setSampleInterval(1000); + start(); } void BAROSIM::stop_cycle() { - work_cancel(HPWORK, &_work); + setSampleInterval(0); } void -BAROSIM::cycle_trampoline(void *arg) +BAROSIM::_measure() { - BAROSIM *dev = reinterpret_cast(arg); - - dev->cycle(); + cycle(); } void @@ -551,8 +532,10 @@ BAROSIM::cycle() if (ret != OK) { /* issue a reset command to the sensor */ _interface->devIOCTL(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ start_cycle(); + return; } @@ -564,15 +547,9 @@ BAROSIM::cycle() * Don't inject one after temperature measurements, so we can keep * doing pressure measurements at something close to the desired rate. */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { + if (_measure_phase != 0) { - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&BAROSIM::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return; } @@ -585,6 +562,7 @@ BAROSIM::cycle() //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ _interface->devIOCTL(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ start_cycle(); return; @@ -593,12 +571,7 @@ BAROSIM::cycle() /* next phase is collection */ _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&BAROSIM::cycle_trampoline, - this, - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + setSampleInterval(BAROSIM_CONVERSION_INTERVAL); } int @@ -702,7 +675,7 @@ BAROSIM::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - PX4_INFO("poll interval: %u ticks", _measure_ticks); + PX4_INFO("poll interval: %u usec", m_sample_interval_usecs); _reports->print_info("report queue"); PX4_INFO("TEMP: %ld", (long)_TEMP); PX4_INFO("SENS: %lld", (long long)_SENS); @@ -710,44 +683,9 @@ BAROSIM::print_info() PX4_INFO("P: %.3f", (double)_P); PX4_INFO("T: %.3f", (double)_T); PX4_INFO("MSL pressure: %10.4f", (double)(_msl_pressure / 100.f)); - - PX4_INFO("factory_setup %u", _prom.factory_setup); - PX4_INFO("c1_pressure_sens %u", _prom.c1_pressure_sens); - PX4_INFO("c2_pressure_offset %u", _prom.c2_pressure_offset); - PX4_INFO("c3_temp_coeff_pres_sens %u", _prom.c3_temp_coeff_pres_sens); - PX4_INFO("c4_temp_coeff_pres_offset %u", _prom.c4_temp_coeff_pres_offset); - PX4_INFO("c5_reference_temp %u", _prom.c5_reference_temp); - PX4_INFO("c6_temp_coeff_temp %u", _prom.c6_temp_coeff_temp); - PX4_INFO("serial_and_crc %u", _prom.serial_and_crc); } -/** - * Local functions in support of the shell command. - */ -namespace barosim -{ - -/* - list of supported bus configurations - */ -struct barosim_bus_option { - enum BAROSIM_BUS busid; - const char *devpath; - BAROSIM_constructor interface_constructor; - uint8_t busnum; - BAROSIM *dev; -} bus_options[] = { - { BAROSIM_BUS_SIM_EXTERNAL, "/dev/baro_sim", &BAROSIM_sim_interface, PX4_SIM_BUS_TEST, NULL }, -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct barosim_bus_option &bus); -int start(); -int test(); -int reset(); -int info(); -int calibrate(unsigned altitude); -void usage(); +namespace barosim { /** * BAROSIM crc4 cribbed from the datasheet @@ -795,38 +733,26 @@ crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } - /** * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. */ -bool -start_bus(struct barosim_bus_option &bus) +static int +start() { - if (bus.dev != nullptr) { - PX4_ERR("bus option already started"); - return false; - } + g_barosim = new BAROSIM(BAROSIM_DEV_PATH); - prom_u prom_buf; - DevObj *interface = bus.interface_constructor(prom_buf, bus.busnum); - - if (interface->init() != OK) { - delete interface; - PX4_ERR("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); - - if (bus.dev != nullptr && OK != bus.dev->init()) { - delete bus.dev; - bus.dev = NULL; - PX4_ERR("bus init failed %p", bus.dev); + if (g_barosim != nullptr && OK != g_barosim->init()) { + delete g_barosim; + g_barosim = NULL; + PX4_ERR("bus init failed"); return false; } DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); /* set the poll rate to default, starts automatic data collection */ if (!h.isValid()) { @@ -845,44 +771,20 @@ start_bus(struct barosim_bus_option &bus) } -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -int -start() -{ - bool started = false; - - started |= start_bus(bus_options[0]); - - if (!started) { - PX4_ERR("driver start failed"); - return 1; - } - - // driver started OK - return 0; -} - - /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ -int +static int test() { - struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; ssize_t sz; int ret; DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); if (!h.isValid()) { PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)"); @@ -953,13 +855,11 @@ test() /** * Reset the driver. */ -int +static int reset() { - struct barosim_bus_option &bus = bus_options[0]; - DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); if (!h.isValid()) { PX4_ERR("failed "); @@ -982,16 +882,12 @@ reset() /** * Print a little info about the driver. */ -int +static int info() { - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct barosim_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_INFO("%s", bus.devpath); - bus.dev->print_info(); - } + if (g_barosim != nullptr) { + PX4_INFO("%s", BAROSIM_DEV_PATH); + g_barosim->print_info(); } return 0; @@ -1000,16 +896,15 @@ info() /** * Calculate actual MSL pressure given current altitude */ -int +static int calibrate(unsigned altitude) { - struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; float pressure; float p1; DevHandle h; - DevMgr::getHandle(bus.devpath, h); + DevMgr::getHandle(BAROSIM_DEV_PATH, h); if (!h.isValid()) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); @@ -1077,13 +972,13 @@ calibrate(unsigned altitude) return 0; } -void +static void usage() { PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate '"); } -} // namespace +}; // namespace barosim int barosim_main(int argc, char *argv[]) diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 22574c27dc..5c5b2105fa 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -42,11 +42,7 @@ #include #include -#include -#include #include -//#include -#include #include #include @@ -54,96 +50,33 @@ #include "barosim.h" #include "board_config.h" -DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf); - -class BARO_SIM : public device::SIM -{ -public: - BARO_SIM(uint8_t bus, barosim::prom_u &prom_buf); - virtual ~BARO_SIM(); - - virtual int init(); - virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned arg); - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); -private: - //barosim::prom_u &_prom; - - /** - * Send a reset command to the barometer simulator. - * - * This is required after any bus reset. - */ - int _reset(); - - /** - * Send a measure command to the barometer simulator. - * - * @param addr Which address to use for the measure operation. - */ - int _measure(unsigned addr); - - /** - * Read the MS5611 PROM - * - * @return PX4_OK if the PROM reads successfully. - */ - int _read_prom(); - -}; - -DevObj * -BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) -{ - return reinterpret_cast(new BARO_SIM(busnum, prom_buf)); -} - -BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : - SIM("BARO_SIM", "/dev/BARO_SIM", bus, 0) +BAROSIM_DEV::BAROSIM_DEV() : + VirtDevObj("BAROSIM_DEV", "/dev/BAROSIM_DEV", BARO_BASE_DEVICE_PATH, 0) { } -BARO_SIM::~BARO_SIM() +BAROSIM_DEV::~BAROSIM_DEV() { } int -BARO_SIM::init() +BAROSIM_DEV::init() { - return SIM::init(); + return VirtDevObj::init(); } -int -BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) +ssize_t +BAROSIM_DEV::devRead(void *data, size_t count) { - /* - union _cvt { - uint8_t b[4]; - uint32_t w; - } *cvt = (_cvt *)data; - */ - /* read the most recent measurement */ uint8_t cmd = 0; int ret = transfer(&cmd, 1, static_cast(data), count); - /* - if (ret == PX4_OK) { - // fetch the raw value - cvt->b[0] = buf[2]; - cvt->b[1] = buf[1]; - cvt->b[2] = buf[0]; - cvt->b[3] = 0; - } - */ - return ret; } int -BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) +BAROSIM_DEV::devIOCTL(unsigned long operation, unsigned long arg) { int ret; @@ -153,7 +86,7 @@ BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) break; case IOCTL_MEASURE: - ret = _measure(arg); + ret = _doMeasurement(arg); break; default: @@ -164,69 +97,60 @@ BARO_SIM::dev_ioctl(unsigned operation, unsigned arg) } int -BARO_SIM::_reset() +BAROSIM_DEV::_reset() { - unsigned old_retrycount = _retries; uint8_t cmd = ADDR_RESET_CMD; int result; /* bump the retry count */ - _retries = 10; result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; return result; } int -BARO_SIM::_measure(unsigned addr) +BAROSIM_DEV::_doMeasurement(unsigned addr) { - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; - uint8_t cmd = addr; return transfer(&cmd, 1, nullptr, 0); } int -BARO_SIM::_read_prom() +BAROSIM_DEV::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - int ret = 1; - // TODO input simlation data - return ret; -} - -int -BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) -{ - // TODO add Simulation data connection so calls retrieve - // data from the simulator - if (recv_len == 0) { - PX4_DEBUG("BARO_SIM measurement requested"); - - } else if (send_len == 1 || send[0] != 12) { - /* measure command */ + if (send_len == 1 && send[0] == ADDR_RESET_CMD) { + /* reset command */ return 0; - } else if (send_len != 1 || send[0] != 0) { - PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); - return 1; + } else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) { + /* measure command */ + if (send[0] == ADDR_CMD_CONVERT_D2) { + } else { + } + return 0; - } else { + } else if (send[0] == 0 && send_len == 1) { + /* read requested */ Simulator *sim = Simulator::getInstance(); if (sim == NULL) { - PX4_ERR("Error BARO_SIM::transfer no simulator"); + PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); return -ENODEV; } - PX4_DEBUG("BARO_SIM::transfer getting sample"); + PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); sim->getBaroSample(recv, recv_len); + return recv_len; + + } else { + PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len); + return 1; } + return 0; } + +void BAROSIM_DEV::_measure() +{ +} diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index db7f009a51..92d959753f 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -36,7 +36,7 @@ * * Shared defines for the ms5611 driver. */ -#include "DevObj.hpp" +#include "VirtDevObj.hpp" #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ #define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ @@ -82,6 +82,35 @@ extern bool crc4(uint16_t *n_prom); using namespace DriverFramework; -/* interface factories */ -extern DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); -typedef DevObj *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); +class BAROSIM_DEV : public VirtDevObj +{ +public: + BAROSIM_DEV(); + virtual ~BAROSIM_DEV(); + + virtual int init(); + virtual ssize_t devRead(void *data, size_t count); + virtual int devIOCTL(unsigned long operation, unsigned long arg); + + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + +protected: + virtual void _measure(); + +private: + /** + * Send a reset command to the barometer simulator. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the barometer simulator. + * + * @param addr Which address to use for the measure operation. + */ + int _doMeasurement(unsigned addr); + +}; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 43bfa61288..413abb0932 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -384,6 +384,7 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { + PX4_INFO("GYROSIM::init"); int ret = 1; struct accel_report arp = {}; @@ -443,7 +444,13 @@ GYROSIM::init() return ret; } - start(); + ret = start(); + if (ret != OK) { + PX4_ERR("gyro accel start failed (%d)", ret); + return ret; + } + + // Do not call _gyro->start() because polling is done in accel _measure(); @@ -1179,8 +1186,8 @@ int GYROSIM_gyro::init() { int ret = VirtDevObj::init(); - printf("init ret: %d\n", ret); - return ret ? ret : start(); + PX4_INFO("GYROSIM_gyro::init base class ret: %d", ret); + return ret; } void diff --git a/src/systemcmds/mixer/CMakeLists.txt b/src/systemcmds/mixer/CMakeLists.txt index 703d8210fb..117f96ff2a 100644 --- a/src/systemcmds/mixer/CMakeLists.txt +++ b/src/systemcmds/mixer/CMakeLists.txt @@ -31,9 +31,7 @@ # ############################################################################ set(MIXER_CFLAGS -Os) -if(${OS} STREQUAL "qurt") - list(APPEND MIXER_CFLAGS -Wframe-larger-than=2176) -else() +if(${OS} STREQUAL "nuttx") list(APPEND MIXER_CFLAGS -Wframe-larger-than=2100) endif() From 1216efc5b4f3c7b7fcebd18eeb09557a88eb9082 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:12:36 +0100 Subject: [PATCH 318/624] Estimator Q: Fixed code style --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d64d8b67c9..64994a7956 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -388,11 +388,11 @@ void AttitudeEstimatorQ::task_main() if (!_failsafe) { uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; - #ifdef __PX4_POSIX +#ifdef __PX4_POSIX perf_end(_perf_accel); perf_end(_perf_mpu); perf_end(_perf_mag); - #endif +#endif if (_voter_gyro.failover_count() > 0) { _failsafe = true; From c1a2d1945933c0b54f8d5622bdcc392a8ee87034 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:13:32 +0100 Subject: [PATCH 319/624] MAVLink Sim: Use time delta --- src/modules/simulator/simulator_mavlink.cpp | 25 ++++++++++----------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2f37eb2d63..2d625a21ac 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -237,23 +237,22 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) void Simulator::handle_message(mavlink_message_t *msg, bool publish) { switch (msg->msgid) { - case MAVLINK_MSG_ID_HIL_SENSOR: - { - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); + case MAVLINK_MSG_ID_HIL_SENSOR: { + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); - uint64_t sim_timestamp = imu.time_usec; - struct timespec ts; - px4_clock_gettime(CLOCK_REALTIME, &ts); - uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; + uint64_t sim_timestamp = imu.time_usec; + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; - perf_set(_perf_sim_delay, timestamp - sim_timestamp); + perf_set(_perf_sim_delay, timestamp - sim_timestamp); - if (publish) { - publish_sensor_topics(&imu); - } + if (publish) { + publish_sensor_topics(&imu); + } - update_sensors(&imu); + update_sensors(&imu); } break; From 58aa8107deb215d222923e27a923e3d354b722d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:15:43 +0100 Subject: [PATCH 320/624] Accelsim: Code style fix --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 030791415c..63e3785118 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -538,6 +538,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) if (want_start) { start(); } + return OK; } } @@ -664,6 +665,7 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) if (want_start) { _mag->start(); } + return OK; } } From 6a80d8b3dc1b93dcc548d7a040f888b61c9aa865 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:16:00 +0100 Subject: [PATCH 321/624] Baro sim: Code style fix --- src/platforms/posix/drivers/barosim/baro.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 37cc5656c9..0bbb2c1c0d 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -685,7 +685,8 @@ BAROSIM::print_info() PX4_INFO("MSL pressure: %10.4f", (double)(_msl_pressure / 100.f)); } -namespace barosim { +namespace barosim +{ /** * BAROSIM crc4 cribbed from the datasheet From 24767547e9a02dee80f1352991227825a665f1bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:16:16 +0100 Subject: [PATCH 322/624] Baro sim: Code style fix --- src/platforms/posix/drivers/barosim/baro_sim.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 5c5b2105fa..224025bc71 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -65,7 +65,7 @@ BAROSIM_DEV::init() return VirtDevObj::init(); } -ssize_t +ssize_t BAROSIM_DEV::devRead(void *data, size_t count) { /* read the most recent measurement */ @@ -127,6 +127,7 @@ BAROSIM_DEV::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, uns if (send[0] == ADDR_CMD_CONVERT_D2) { } else { } + return 0; } else if (send[0] == 0 && send_len == 1) { From 176fb3b185f4cae8f5d4c6dcae6f4c6cf99d8118 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:16:28 +0100 Subject: [PATCH 323/624] Gyrosim: Code style fix --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 413abb0932..1eef3f88eb 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -445,6 +445,7 @@ GYROSIM::init() } ret = start(); + if (ret != OK) { PX4_ERR("gyro accel start failed (%d)", ret); return ret; From e5b2c652e22d174ea4f3b2f3ddd5c5877e359a33 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 25 Nov 2015 10:28:19 +0100 Subject: [PATCH 324/624] channel airspeed meas over control state for q estimator --- .../attitude_estimator_q_main.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 4b4fba54c8..f6037564e7 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ private: int _params_sub = -1; int _vision_sub = -1; int _mocap_sub = -1; + int _airspeed_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; orb_advert_t _ctrl_state_pub = nullptr; @@ -161,6 +163,8 @@ private: att_pos_mocap_s _mocap = {}; Vector<3> _mocap_hdg; + airspeed_s _airspeed = {}; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -292,6 +296,8 @@ void AttitudeEstimatorQ::task_main() _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -473,6 +479,14 @@ void AttitudeEstimatorQ::task_main() _mocap_hdg = Rmoc.transposed() * v; } + // Update airspeed + bool airspeed_updated = false; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + } + // Check for timeouts on data if (_ext_hdg_mode == 1) { _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); @@ -586,6 +600,9 @@ void AttitudeEstimatorQ::task_main() ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); + /* Airspeed - take airspeed measurement directly here as no wind is estimated */ + ctrl_state.airspeed = _airspeed.true_airspeed_m_s; + /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); From 1f500413117a850fb669b2bc4d9a8bdbab20b2dc Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Aug 2015 16:23:01 +0200 Subject: [PATCH 325/624] added flag indicating if vtol is doing a transition Conflicts: msg/vtol_vehicle_status.msg --- msg/vehicle_status.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 60ab34e90d..b35d0cab60 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -113,6 +113,7 @@ uint32 system_id # system id, inspired by MAVLink's system ID field uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +bool vtol_in_transition # True if VTOL is doing a transition bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode bool in_transition_mode From d55ccd96c69f35880bc7927b10a8d5768591088b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Aug 2015 16:24:57 +0200 Subject: [PATCH 326/624] let vtol attitude control module publish attitude setpoint during transition Conflicts: src/modules/commander/commander.cpp src/modules/fw_att_control/fw_att_control_main.cpp src/modules/mc_pos_control/mc_pos_control_main.cpp --- src/modules/commander/commander.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 851529bb54..20efb4db3c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1599,7 +1599,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + /* Make sure that this is only adjusted if vehicle really is of type vtol */ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6776d78871..53eff5977f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1610,7 +1610,9 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, - * in this case the attitude setpoint is published by the mavlink app + * in this case the attitude setpoint is published by the mavlink app. Also do not publish + * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate + * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || From 0fa8a5286b152c8b37075c0c6a973ed0a28bb218 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Aug 2015 16:25:27 +0200 Subject: [PATCH 327/624] tiltrotor: publish attitude setpoint when doing a transition Conflicts: src/modules/vtol_att_control/vtol_att_control_main.h --- src/modules/vtol_att_control/tiltrotor.cpp | 25 +++++++++++++++++++ .../vtol_att_control_main.cpp | 18 ++++++++++--- .../vtol_att_control/vtol_att_control_main.h | 2 ++ src/modules/vtol_att_control/vtol_type.h | 4 +++ 4 files changed, 45 insertions(+), 4 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 270aa065a9..c4fa37eb2c 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _flag_was_in_trans_mode = false; + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); @@ -292,6 +294,12 @@ void Tiltrotor::update_fw_state() void Tiltrotor::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att->yaw; + _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _flag_was_in_trans_mode = true; + } if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { @@ -354,6 +362,23 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0; + _v_att_sp->pitch_body = 0; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->thrust = _throttle_transition; + + math::Matrix<3,3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); + _v_att_sp->R_valid = true; + + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + _v_att_sp->q_d_valid = true; + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } void Tiltrotor::update_external_state() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0248a20727..5725139ef4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -59,7 +59,6 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), - _v_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -75,13 +74,13 @@ VtolAttitudeControl::VtolAttitudeControl() : _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr) + _v_rates_sp_pub(nullptr), + _v_att_sp_pub(nullptr) { memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -439,6 +438,17 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } +void VtolAttitudeControl::publish_transition_att_sp() +{ + if (_v_att_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + } else { + /* advertise and publish */ + _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -451,7 +461,6 @@ void VtolAttitudeControl::task_main() fflush(stdout); /* do subscriptions */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -599,6 +608,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); + publish_transition_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c10f7b0dd7..2e68d5d85d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -151,6 +151,7 @@ private: orb_advert_t _actuators_1_pub; orb_advert_t _vtol_vehicle_status_pub; orb_advert_t _v_rates_sp_pub; + orb_advert_t _v_att_sp_pub; //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint @@ -215,6 +216,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); + void publish_transition_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d8557110c0..4345f506a1 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -116,6 +116,10 @@ protected: float _mc_pitch_weight; // weight for multicopter attitude controller pitch output float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + float _yaw_transition; // yaw angle in which transition will take place + float _throttle_transition; // throttle value used for the transition phase + bool _flag_was_in_trans_mode; // true if mode has just switched to transition + }; #endif From 6c5638062a818e264fbccc978fd169c6f44883df Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 20 Aug 2015 12:38:49 +0300 Subject: [PATCH 328/624] tailsitter auto transition test! Conflicts: src/modules/vtol_att_control/tailsitter.cpp --- src/modules/vtol_att_control/tailsitter.cpp | 306 ++++++++++++++++-- src/modules/vtol_att_control/tailsitter.h | 102 +++++- .../vtol_att_control/tailsitter_params.c | 56 ++++ 3 files changed, 433 insertions(+), 31 deletions(-) create mode 100644 src/modules/vtol_att_control/tailsitter_params.c diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 7368870847..5bae1e0f93 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,23 +31,49 @@ * ****************************************************************************/ -/** -* @file tailsitter.cpp -* -* @author Roman Bapst -* -*/ + /** + * @file tailsitter.cpp + * + * @author Roman Bapst + * @author David Vorsin + * + */ #include "tailsitter.h" #include "vtol_att_control_main.h" -Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : - VtolType(att_controller), - _airspeed_tot(0), - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) -{ + #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition + #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition + #define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2 + #define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW + #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC +Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : +/**VtolType(att_controller),*/ +VtolType(attc), +_roll_weight_mc(1.0f), +_yaw_weight_mc(1.0f), +_min_front_trans_dur(0.5f) +_airspeed_tot(0), +_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), +_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + } Tailsitter::~Tailsitter() @@ -55,23 +81,153 @@ Tailsitter::~Tailsitter() } +int +Taulsitter::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_tiltrotor.front_trans_dur, &v); + _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); + _params_tiltrotor.front_trans_dur_p2 = v; + + /* vtol duration of a back transition */ + param_get(_params_handles_tiltrotor.back_trans_dur, &v); + _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tiltrotor.airspeed_trans, &v); + _params_tiltrotor.airspeed_trans = v; + + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); + _params_tiltrotor.airspeed_blend_start = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); + _params_tiltrotor.elevons_mc_lock = l; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + + if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { + _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; + } + + return OK; +} + void Tailsitter::update_vtol_state() { - // simply switch between the two modes - if (!_attc->is_fixed_wing_requested()) { - _vtol_mode = ROTARY_WING; + parameters_update(); - } else { - _vtol_mode = FIXED_WING; + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting rotors, picking up + * forward speed. After the vehicle has picked up enough speed the rotors are tilted + * forward completely. For the backtransition the motors simply rotate back. + */ + + if (_manual_control_sp->aux1 < 0.0f) { // user switchig to MC mode + + // plane is in multicopter mode + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + break; + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + case TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + case TRANSITION_BACK: + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { + _vtol_schedule.flight_mode = MC_MODE; + } + break; + } + } else { // user switchig to FW mode + + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + case FW_MODE: + break; + case TRANSITION_FRONT_P1: + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + break; + case TRANSITION_FRONT_P2: + // check if we have reached pitch angle to switch to FW mode + if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + _vtol_schedule.flight_mode = FW_MODE; + } + break; + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; + } + } + + // map tailsitter specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } + + void Tailsitter::update_mc_state() { - if (!flag_idle_mc) { + // set idle speed for rotary wing mode + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } void Tailsitter::update_fw_state() @@ -80,13 +236,124 @@ void Tailsitter::update_fw_state() set_idle_fw(); flag_idle_mc = false; } + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; } void Tailsitter::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att->yaw; + _pitch_transition_start = _v_att->pitch; + _throttle_transition_start = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _flag_was_in_trans_mode = true; + } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { + _v_att_sp->pitch_body = _pitch_transition_start - + fabsf(PITCH_TRANSITION_FRONT_P1 / _params_tiltrotor.front_trans_dur) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + } + + /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ + if (_airspeed->true_airspeed_m_s <= _params_tiltrotor.airspeed_trans) ) { + _v_att_sp->thrust = _throttle_transition_start + (THROTTLE_TRANSITION_MAX * _throttle_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + /** if the limit reached stop adding thrust */ + if (_v_att_sp->thrust >= (THROTTLE_TRANSITION_MAX * _throttle_transition_start) ) { + _v_att_sp->thrust = (THROTTLE_TRANSITION_MAX * _throttle_transition_start); + } + } + + /** ^ else condition nescesary for publishing values ? ^ */ + + // disable mc yaw control once the plane has picked up speed + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + }else { + _mc_yaw_weight = 1.0f; + } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down + + /** FW motor is switched */ + /** is thrust assignment nescesary? */ + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) / _params_tiltrotor.front_trans_dur_p2) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + } + + /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ + + _mc_roll_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_pitch_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { + _v_att_sp->pitch_body = _pitch_transition_start + + fabsf(PITCH_TRANSITION_BACK / _params_tiltrotor.back_trans_dur) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + } + + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + // set zero throttle for backtransition otherwise unwanted moments will be created + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + + } + + + + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->R_valid = true; + + math::Matrix<3,3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); + + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } + void Tailsitter::update_external_state() { @@ -110,8 +377,7 @@ void Tailsitter::calc_tot_airspeed() _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; } -void -Tailsitter::scale_mc_output() +void Tailsitter::scale_mc_output() { // scale around tuning airspeed float airspeed; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index ec8d007567..d54748cc75 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,18 +31,21 @@ * ****************************************************************************/ -/** -* @file tiltrotor.h -* -* @author Roman Bapst -* -*/ + /** + * @file tiltrotor.h + * + * @author Roman Bapst + * @author David Vorsin + * + */ #ifndef TAILSITTER_H #define TAILSITTER_H #include "vtol_type.h" -#include +#include /** is it necsacery? **/ +#include +#include class Tailsitter : public VtolType { @@ -51,21 +54,98 @@ public: Tailsitter(VtolAttitudeControl *_att_controller); ~Tailsitter(); + /** + * Update vtol state. + */ void update_vtol_state(); + + /** + * Update multicopter state. + */ void update_mc_state(); + + /** + * Update fixed wing state. + */ void update_fw_state(); + + /** + * Update transition state. + */ void update_transition_state(); + + /** + * Update external state. + */ void update_external_state(); private: - void fill_actuator_outputs(); - void calc_tot_airspeed(); - void scale_mc_output(); - float _airspeed_tot; + + struct { + float front_trans_dur; /**< duration of first part of front transition */ + float front_trans_dur_p2; + float back_trans_dur; /**< duration of back transition */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + + } _params_tailsitter; + + struct { + param_t front_trans_dur; + param_t front_trans_dur_p2; + param_t back_trans_dur; + param_t airspeed_trans; + param_t airspeed_blend_start; + param_t elevons_mc_lock; + + } _params_handles_tailsitter; + + enum vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + }_vtol_schedule; + + float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ + float _roll_weight_mc; /**< multicopter desired roll moment weight */ + float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ + + /** not sure about it yet ?! **/ + const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + + + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + /** + * Speed estimation for propwash controlled surfaces. + */ + void calc_tot_airspeed(); + + + /** is this one still needed? */ + void scale_mc_output(); + + /** + * Write control values to actuator output topics. + */ + void fill_actuator_outputs(); + + /** + * Update parameters. + */ + int parameters_update(); + }; #endif diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c new file mode 100644 index 0000000000..c72d3957c8 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 tailsitter_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author David Vorsin + */ + +#include + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + + From c39935f248e0a522f4b12dbba36b70d6eced0fa9 Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 20 Aug 2015 13:36:48 +0300 Subject: [PATCH 329/624] mixer for transition not final yet --- src/modules/vtol_att_control/tailsitter.cpp | 86 ++++++++++----------- 1 file changed, 42 insertions(+), 44 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 5bae1e0f93..108e46aad8 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -325,8 +325,8 @@ void Tailsitter::update_transition_state() /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - // set zero throttle for backtransition otherwise unwanted moments will be created - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + // set throttle value same as started + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition_start; } @@ -416,50 +416,48 @@ void Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - switch (_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + switch(_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + } + break; + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + case TRANSITION: + // in transition engines are mixed by weight (FRONT_P2 , BACK) + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - } else { // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon - } - - break; - - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; - - case TRANSITION: - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + break; + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } From 093d4fdd15469f071bf6ac74c0ac028a6d1241f1 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:16:34 +0300 Subject: [PATCH 330/624] development of autonomous transition --- src/modules/vtol_att_control/tailsitter.cpp | 98 ++++++++++----------- 1 file changed, 49 insertions(+), 49 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 108e46aad8..2fd5e60382 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -48,13 +48,14 @@ #define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC -Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : -/**VtolType(att_controller),*/ +Tailsitter::Tailsitter (VtolAttitudeControl *attc) : VtolType(attc), +_airspeed_tot(0.0f), _roll_weight_mc(1.0f), +_pitch_weight_mc(1.0f), _yaw_weight_mc(1.0f), -_min_front_trans_dur(0.5f) -_airspeed_tot(0), +_min_front_trans_dur(0.5f), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { @@ -67,12 +68,12 @@ _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinit _flag_was_in_trans_mode = false; - _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); - _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); - _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); - _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); - _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); } @@ -82,40 +83,40 @@ Tailsitter::~Tailsitter() } int -Taulsitter::parameters_update() +Tailsitter::parameters_update() { float v; int l; /* vtol duration of a front transition */ - param_get(_params_handles_tiltrotor.front_trans_dur, &v); - _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + param_get(_params_handles_tailsitter.front_trans_dur, &v); + _params_tailsitter.front_trans_dur = math::constrain(v,1.0f,5.0f); /* vtol front transition phase 2 duration */ - param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); - _params_tiltrotor.front_trans_dur_p2 = v; + param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); + _params_tailsitter.front_trans_dur_p2 = v; /* vtol duration of a back transition */ - param_get(_params_handles_tiltrotor.back_trans_dur, &v); - _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + param_get(_params_handles_tailsitter.back_trans_dur, &v); + _params_tailsitter.back_trans_dur = math::constrain(v,0.0f,5.0f); /* vtol airspeed at which it is ok to switch to fw mode */ - param_get(_params_handles_tiltrotor.airspeed_trans, &v); - _params_tiltrotor.airspeed_trans = v; + param_get(_params_handles_tailsitter.airspeed_trans, &v); + _params_tailsitter.airspeed_trans = v; /* vtol airspeed at which we start blending mc/fw controls */ - param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); - _params_tiltrotor.airspeed_blend_start = v; + param_get(_params_handles_tailsitter.airspeed_blend_start, &v); + _params_tailsitter.airspeed_blend_start = v; /* vtol lock elevons in multicopter */ - param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); - _params_tiltrotor.elevons_mc_lock = l; + param_get(_params_handles_tailsitter.elevons_mc_lock, &l); + _params_tailsitter.elevons_mc_lock = l; /* avoid parameters which will lead to zero division in the transition code */ - _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); - if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { - _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; + if ( _params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f ) { + _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; } return OK; @@ -168,7 +169,7 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_FRONT_P1: // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) { + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } @@ -246,9 +247,9 @@ void Tailsitter::update_transition_state() { if (!_flag_was_in_trans_mode) { // save desired heading for transition and last thrust value - _yaw_transition = _v_att->yaw; - _pitch_transition_start = _v_att->pitch; - _throttle_transition_start = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _yaw_transition = _v_att_sp->yaw_body; + _pitch_transition_start = _v_att_sp->pitch_body; + _throttle_transition = _v_att_sp->thrust; _flag_was_in_trans_mode = true; } @@ -257,16 +258,16 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start - - fabsf(PITCH_TRANSITION_FRONT_P1 / _params_tiltrotor.front_trans_dur) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); } /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ - if (_airspeed->true_airspeed_m_s <= _params_tiltrotor.airspeed_trans) ) { - _v_att_sp->thrust = _throttle_transition_start + (THROTTLE_TRANSITION_MAX * _throttle_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); + /** if the limit reached stop adding thrust */ - if (_v_att_sp->thrust >= (THROTTLE_TRANSITION_MAX * _throttle_transition_start) ) { - _v_att_sp->thrust = (THROTTLE_TRANSITION_MAX * _throttle_transition_start); + if (_v_att_sp->thrust >= ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition) ) { + _v_att_sp->thrust = ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } } @@ -282,23 +283,21 @@ void Tailsitter::update_transition_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down - /** FW motor is switched */ - /** is thrust assignment nescesary? */ + /** no motor switching */ /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) / _params_tiltrotor.front_trans_dur_p2) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - _mc_roll_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); - _mc_pitch_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + _mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -314,19 +313,19 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { _v_att_sp->pitch_body = _pitch_transition_start + - fabsf(PITCH_TRANSITION_BACK / _params_tiltrotor.back_trans_dur) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); } /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); /** keep yaw disabled */ _mc_yaw_weight = 0.0f; // set throttle value same as started - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition_start; + //_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition; } @@ -340,7 +339,7 @@ void Tailsitter::update_transition_state() // compute desired attitude and thrust setpoint for the transition _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0; + _v_att_sp->roll_body = 0.0f; _v_att_sp->yaw_body = _yaw_transition; _v_att_sp->R_valid = true; @@ -429,7 +428,7 @@ void Tailsitter::fill_actuator_outputs() } else { // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =0; //pitch elevon } break; case FIXED_WING: @@ -455,6 +454,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + //_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: // not yet implemented, we are switching brute force at the moment From 9e28365bbbc501c559aff444538f53ec60545628 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:16:55 +0300 Subject: [PATCH 331/624] adding pitch weight for tailsitters --- src/modules/vtol_att_control/tailsitter.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index d54748cc75..1b2caca676 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -118,10 +118,11 @@ private: float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ float _roll_weight_mc; /**< multicopter desired roll moment weight */ + float _pitch_weight_mc; /**< multicopter desired pitch moment weight */ float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ /** not sure about it yet ?! **/ - const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ /** should this anouncement stay? **/ From 28cc7521ff8f1c82e909cfce56147d395af58e30 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:19:20 +0300 Subject: [PATCH 332/624] unnecessary parameter --- src/modules/vtol_att_control/tailsitter_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index c72d3957c8..52801d9e8c 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -50,7 +50,7 @@ * @min 0.1 * @max 2 * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ From c129ebb0ea54ec0b62a7eff39353fe6cdcaa6e61 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:19:59 +0300 Subject: [PATCH 333/624] adding poll for att_sp and make the MC attitude controller work in transition --- .../vtol_att_control_main.cpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5725139ef4..142c2a26ff 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #include "vtol_att_control_main.h" @@ -59,6 +60,7 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), + _v_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -81,6 +83,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -270,6 +273,36 @@ VtolAttitudeControl::vehicle_airspeed_poll() } } +/** +* Check for attitude set points update. +*/ +void +VtolAttitudeControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +/** +* Check for attitude update. +*/ +void +VtolAttitudeControl::vehicle_attitude_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + } +} + /** * Check for battery updates. */ @@ -464,6 +497,7 @@ void VtolAttitudeControl::task_main() _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -535,6 +569,8 @@ void VtolAttitudeControl::task_main() vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. + vehicle_attitude_setpoint_poll();//Check for changes in attitude set points + vehicle_attitude_poll(); //Check for changes in attitude actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output vehicle_rates_sp_mc_poll(); @@ -591,6 +627,7 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition bool got_new_data = false; From 0d58429565d223f13b8a63bae77aad26be53b743 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:20:25 +0300 Subject: [PATCH 334/624] poll on att_sp and att --- src/modules/vtol_att_control/vtol_att_control_main.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2e68d5d85d..9485d23d4f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #ifndef VTOL_ATT_CONTROL_MAIN_H @@ -209,6 +210,8 @@ private: void vehicle_rates_sp_fw_poll(); void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. + void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void parameters_update_poll(); //Check if parameters have changed From 2bf460cdfe9af1ef70ed18463b3fae77d575ddeb Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:21:00 +0300 Subject: [PATCH 335/624] adding variables for tailsitter transition --- src/modules/vtol_att_control/vtol_type.h | 31 ++++++++++++------------ 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 4345f506a1..5c112ac207 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -91,22 +91,22 @@ protected: VtolAttitudeControl *_attc; mode _vtol_mode; - struct vehicle_attitude_s *_v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; - struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s *_armed; //actuator arming status + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s *_armed; //actuator arming status struct vehicle_local_position_s *_local_pos; - struct airspeed_s *_airspeed; // airspeed - struct battery_status_s *_batt_status; // battery status + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status struct Params *_params; @@ -117,7 +117,8 @@ protected: float _mc_yaw_weight; // weight for multicopter attitude controller yaw output float _yaw_transition; // yaw angle in which transition will take place - float _throttle_transition; // throttle value used for the transition phase + float _throttle_transition; // throttle value used for the transition phase + float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) bool _flag_was_in_trans_mode; // true if mode has just switched to transition }; From 30bb05487aba847cffc8e1aa46be946efbffa359 Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 27 Aug 2015 14:06:31 +0300 Subject: [PATCH 336/624] mixer setup for tests --- ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix index 4fd323353a..bd332a2688 100644 --- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0 #mixer for the elevons M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 5000 5000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 -10000 -10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 -5000 -5000 0 -10000 10000 From 2642b3fbead3d849e1b107dcacb03f6d4d870a7d Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 27 Aug 2015 14:07:13 +0300 Subject: [PATCH 337/624] limit for pitch angle set point during transition --- src/modules/vtol_att_control/tailsitter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 2fd5e60382..5528abe59f 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -259,6 +259,10 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1-0.2f; + } } /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ From 2721330040698610c83adfe6737b16636eac1691 Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 27 Aug 2015 14:07:26 +0300 Subject: [PATCH 338/624] errors --- errors.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 errors.txt diff --git a/errors.txt b/errors.txt new file mode 100644 index 0000000000..e69de29bb2 From c32f5910e0b1bfe5f6fd1dbc7013ec9702ff1011 Mon Sep 17 00:00:00 2001 From: davidvor Date: Fri, 28 Aug 2015 13:38:15 +0300 Subject: [PATCH 339/624] name change --- src/modules/vtol_att_control/tailsitter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 5528abe59f..63f3e40384 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -83,7 +83,7 @@ Tailsitter::~Tailsitter() } int -Tailsitter::parameters_update() +Trailsitter::parameters_update() { float v; int l; From 059cffeb40253736a874c117e3b2384ac61471b8 Mon Sep 17 00:00:00 2001 From: davidvor Date: Fri, 28 Aug 2015 13:41:19 +0300 Subject: [PATCH 340/624] name change --- src/modules/vtol_att_control/tailsitter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 63f3e40384..5528abe59f 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -83,7 +83,7 @@ Tailsitter::~Tailsitter() } int -Trailsitter::parameters_update() +Tailsitter::parameters_update() { float v; int l; From 89b01fd45a5a20f0cc5489f7b08c7a5325db04e2 Mon Sep 17 00:00:00 2001 From: davidvor Date: Sun, 30 Aug 2015 21:19:09 +0300 Subject: [PATCH 341/624] turning on fw controler during p2 front transiion turning on fw controler during p2 front transiion simpling the weight for testing setting output more like fw for transition --- src/modules/vtol_att_control/tailsitter.cpp | 37 +++++++++++++++------ 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 5528abe59f..d9adb297d3 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -45,7 +45,7 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition #define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2 - #define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW + #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC Tailsitter::Tailsitter (VtolAttitudeControl *attc) : @@ -292,17 +292,30 @@ void Tailsitter::update_transition_state() /** no motor switching */ + if (flag_idle_mc) { + set_idle_fw(); + //flag_idle_mc = false; + } + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2-0.2f; + } + } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - _mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - _mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -316,15 +329,19 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start + - fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_BACK+0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_BACK+0.2f; } /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + //_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + //_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -455,10 +472,10 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //change _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - //_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: // not yet implemented, we are switching brute force at the moment From 19b960648664d29147632052180e91f8b31863d8 Mon Sep 17 00:00:00 2001 From: davidvor Date: Tue, 1 Sep 2015 12:04:03 +0300 Subject: [PATCH 342/624] simplifying transition for mc control only --- src/modules/vtol_att_control/tailsitter.cpp | 61 +++++++-------------- 1 file changed, 21 insertions(+), 40 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d9adb297d3..04139c292b 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -44,9 +44,9 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition - #define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2 + #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC + #define PITCH_TRANSITION_BACK -0.6f // pitch angle to switch to MC Tailsitter::Tailsitter (VtolAttitudeControl *attc) : VtolType(attc), @@ -170,8 +170,8 @@ void Tailsitter::update_vtol_state() case TRANSITION_FRONT_P1: // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - _vtol_schedule.transition_start = hrt_absolute_time(); + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); } break; case TRANSITION_FRONT_P2: @@ -255,28 +255,16 @@ void Tailsitter::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { - /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1-0.2f; - } - } + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body =math::constrain( _v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1-0.2f , _pitch_transition_start ); /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - - /** if the limit reached stop adding thrust */ - if (_v_att_sp->thrust >= ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition) ) { - _v_att_sp->thrust = ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); - } + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } - /** ^ else condition nescesary for publishing values ? ^ */ - // disable mc yaw control once the plane has picked up speed if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; @@ -294,7 +282,7 @@ void Tailsitter::update_transition_state() if (flag_idle_mc) { set_idle_fw(); - //flag_idle_mc = false; + flag_idle_mc = false; } /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ @@ -327,26 +315,19 @@ void Tailsitter::update_transition_state() flag_idle_mc = true; } - /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_BACK+0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_BACK+0.2f; - } - - /** smoothly move control weight to MC */ - //_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - //_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - - _mc_roll_weight = 0.0f; - _mc_pitch_weight = 0.0f; - + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - // set throttle value same as started - //_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition; + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + + + // throttle value is the same as started } @@ -472,9 +453,9 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //change - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + - (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight + + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: From 76c479170a3fdb61e21b302f20e0d3f2fa08e01e Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 3 Sep 2015 17:19:40 +0300 Subject: [PATCH 343/624] add transition back throttle value and angular rate --- src/modules/vtol_att_control/tailsitter.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 04139c292b..d2ef6b3db2 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -46,7 +46,7 @@ #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.6f // pitch angle to switch to MC + #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC Tailsitter::Tailsitter (VtolAttitudeControl *attc) : VtolType(attc), @@ -316,8 +316,11 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + + + _v_att_sp->thrust = _throttle_transition*0.9f; /** keep yaw disabled */ _mc_yaw_weight = 0.0f; From 945dda04ca397a800bc7592490f8981a37a93b44 Mon Sep 17 00:00:00 2001 From: davidvor Date: Sun, 6 Sep 2015 12:01:59 +0300 Subject: [PATCH 344/624] comments --- src/modules/vtol_att_control/tailsitter.cpp | 35 ++++++++++----------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d2ef6b3db2..c557936b15 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -127,15 +127,15 @@ void Tailsitter::update_vtol_state() parameters_update(); /* simple logic using a two way switch to perform transitions. - * after flipping the switch the vehicle will start tilting rotors, picking up - * forward speed. After the vehicle has picked up enough speed the rotors are tilted - * forward completely. For the backtransition the motors simply rotate back. + * after flipping the switch the vehicle will start tilting in MC control mode, picking up + * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. + * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - if (_manual_control_sp->aux1 < 0.0f) { // user switchig to MC mode + if (_manual_control_sp->aux1 < 0.0f) { - // plane is in multicopter mode - switch(_vtol_schedule.flight_mode) { + + switch(_vtol_schedule.flight_mode) { // user switchig to MC mode case MC_MODE: break; case FW_MODE: @@ -147,8 +147,9 @@ void Tailsitter::update_vtol_state() _vtol_schedule.flight_mode = MC_MODE; break; case TRANSITION_FRONT_P2: + // NOT USED // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + //_vtol_schedule.flight_mode = MC_MODE; break; case TRANSITION_BACK: // check if we have reached pitch angle to switch to MC mode @@ -175,10 +176,11 @@ void Tailsitter::update_vtol_state() } break; case TRANSITION_FRONT_P2: + // NOT USED // check if we have reached pitch angle to switch to FW mode - if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - _vtol_schedule.flight_mode = FW_MODE; - } + //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + // _vtol_schedule.flight_mode = FW_MODE; + //} break; case TRANSITION_BACK: // failsafe into fixed wing mode @@ -316,10 +318,10 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); - + // throttle value is decreesed _v_att_sp->thrust = _throttle_transition*0.9f; /** keep yaw disabled */ @@ -329,9 +331,6 @@ void Tailsitter::update_transition_state() _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - - // throttle value is the same as started - } @@ -449,7 +448,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle break; case TRANSITION: - // in transition engines are mixed by weight (FRONT_P2 , BACK) + // in transition engines are mixed by weight (BACK TRANSITION ONLY) _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; @@ -457,8 +456,8 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight - + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: From 932883e30367b4ebf81b0d7c4f3703a3d1074d32 Mon Sep 17 00:00:00 2001 From: davidvor Date: Sun, 6 Sep 2015 12:02:33 +0300 Subject: [PATCH 345/624] errors --- errors.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 errors.txt diff --git a/errors.txt b/errors.txt deleted file mode 100644 index e69de29bb2..0000000000 From d1dc8ed432f766b24e8d324d252ba5df8e13f5ce Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 7 Sep 2015 09:21:24 +0200 Subject: [PATCH 346/624] if vtol, publish virtual attitude setpoint Conflicts: src/modules/mc_pos_control/mc_pos_control_main.cpp --- .../fw_att_control/fw_att_control_main.cpp | 20 ++++++------ .../fw_pos_control_l1_main.cpp | 22 ++++++++++--- .../mc_pos_control/mc_pos_control_main.cpp | 31 +++++++++++++------ 3 files changed, 50 insertions(+), 23 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 8772ceb835..1328bca48d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -142,6 +143,7 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ @@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _rates_sp_id(0), _actuators_id(0), + _attitude_setpoint_id(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll() if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } } @@ -1021,15 +1026,12 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - - } else { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); + } else if (_attitude_setpoint_id) { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); } } 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 24cf1f084f..8b4e4224eb 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 @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -166,6 +167,8 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_id_t _attitude_setpoint_id; + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), +/* publication ID */ + _attitude_setpoint_id(0), + /* states */ _ctrl_state(), _att_sp(), @@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } } @@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + } else if (_attitude_setpoint_id) { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /* XXX check if radius makes sense here */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 53eff5977f..a631336273 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,8 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + orb_id_t _attitude_setpoint_id; + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), + _attitude_setpoint_id(0), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), _vel_x_deriv(this, "VELD"), @@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } orb_check(_ctrl_state_sub, &updated); @@ -1183,10 +1195,9 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } else { @@ -1615,13 +1626,13 @@ MulticopterPositionControl::task_main() * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { - } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } From 0a0a074194705329d19e08f6441a7d6e38d29535 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 7 Sep 2015 09:22:01 +0200 Subject: [PATCH 347/624] use virtual attitude setpoint Conflicts: src/modules/vtol_att_control/tailsitter.cpp src/modules/vtol_att_control/tiltrotor.cpp src/modules/vtol_att_control/vtol_att_control_main.cpp src/modules/vtol_att_control/vtol_att_control_main.h src/modules/vtol_att_control/vtol_type.h --- msg/fw_virtual_attitude_setpoint.msg | 23 +++++++++ msg/mc_virtual_attitude_setpoint.msg | 23 +++++++++ src/modules/uORB/objects_common.cpp | 8 +++- src/modules/uORB/uORB.h | 1 - src/modules/vtol_att_control/standard.cpp | 11 ++++- src/modules/vtol_att_control/tailsitter.cpp | 6 +++ src/modules/vtol_att_control/tiltrotor.cpp | 24 ++++------ .../vtol_att_control_main.cpp | 48 +++++++++++++++++-- .../vtol_att_control/vtol_att_control_main.h | 13 ++++- src/modules/vtol_att_control/vtol_type.cpp | 2 + src/modules/vtol_att_control/vtol_type.h | 18 +++---- 11 files changed, 143 insertions(+), 34 deletions(-) create mode 100644 msg/fw_virtual_attitude_setpoint.msg create mode 100644 msg/mc_virtual_attitude_setpoint.msg diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/fw_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/mc_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 8fe70d7604..1b8fb18735 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); + +#include "topics/mc_virtual_attitude_setpoint.h" +ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s); + +#include "topics/fw_virtual_attitude_setpoint.h" +ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 6ead26186b..cfcf39b408 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -366,7 +366,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; typedef uint8_t hil_state_t; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 05c1a525b1..5a329d52f9 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -186,6 +186,9 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value @@ -242,12 +245,16 @@ void Standard::update_transition_state() void Standard::update_mc_state() { - // do nothing + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Standard::update_fw_state() { - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index c557936b15..827e3ec617 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -231,6 +231,9 @@ void Tailsitter::update_mc_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_fw_state() @@ -243,6 +246,9 @@ void Tailsitter::update_fw_state() _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_transition_state() diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c4fa37eb2c..24ff7ece2a 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -252,6 +252,9 @@ void Tiltrotor::update_vtol_state() void Tiltrotor::update_mc_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are not tilted _tilt_control = _params_tiltrotor.tilt_mc; @@ -273,6 +276,9 @@ void Tiltrotor::update_mc_state() void Tiltrotor::update_fw_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -363,22 +369,8 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); - // compute desired attitude and thrust setpoint for the transition - _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0; - _v_att_sp->pitch_body = 0; - _v_att_sp->yaw_body = _yaw_transition; - _v_att_sp->thrust = _throttle_transition; - - math::Matrix<3,3> R_sp; - R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); - memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); - _v_att_sp->R_valid = true; - - math::Quaternion q_sp; - q_sp.from_dcm(R_sp); - _v_att_sp->q_d_valid = true; - memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); + // copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp) + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tiltrotor::update_external_state() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 142c2a26ff..80bb62eccd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -60,7 +60,9 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), - _v_att_sp_sub(-1), + _v_att_sp_sub(-1), + _mc_virtual_att_sp_sub(-1), + _fw_virtual_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -84,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp)); + memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -351,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for mc virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::mc_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_mc_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp); + } + +} + +/** +* Check for fw virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::fw_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_fw_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp); + } + +} + /** * Check for command updates. */ @@ -471,7 +507,7 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -void VtolAttitudeControl::publish_transition_att_sp() +void VtolAttitudeControl::publish_att_sp() { if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ @@ -494,6 +530,9 @@ void VtolAttitudeControl::task_main() fflush(stdout); /* do subscriptions */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); + _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -566,6 +605,8 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + mc_virtual_att_sp_poll(); + fw_virtual_att_sp_poll(); vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. @@ -645,7 +686,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); - publish_transition_att_sp(); + publish_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { @@ -653,6 +694,7 @@ void VtolAttitudeControl::task_main() _vtol_type->update_external_state(); } + publish_att_sp(); _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 9485d23d4f..c7c51247ae 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -63,6 +63,8 @@ #include #include #include +#include +#include #include #include #include @@ -107,6 +109,8 @@ public: struct vehicle_attitude_s *get_att() {return &_v_att;} struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp () {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp () {return &_fw_virtual_att_sp;} struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} @@ -124,7 +128,6 @@ public: struct Params *get_params() {return &_params;} - private: //******************flags & handlers****************************************************** bool _task_should_exit; @@ -133,6 +136,8 @@ private: /* handlers for subscriptions */ int _v_att_sub; //vehicle attitude subscription int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_att_sp_sub; + int _fw_virtual_att_sp_sub; int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _v_control_mode_sub; //vehicle control mode subscription @@ -156,6 +161,8 @@ private: //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint @@ -204,6 +211,8 @@ private: void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. void vehicle_manual_poll(); //Check for changes in manual inputs. void arming_status_poll(); //Check for arming status updates. + void mc_virtual_att_sp_poll(); + void fw_virtual_att_sp_poll(); void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); @@ -219,7 +228,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); - void publish_transition_att_sp(); + void publish_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index f57080a2a7..0120cadc84 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); + _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); + _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); _v_rates_sp = _attc->get_rates_sp(); _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5c112ac207..196751016c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -91,13 +91,15 @@ protected: VtolAttitudeControl *_attc; mode _vtol_mode; - struct vehicle_attitude_s *_v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) @@ -117,8 +119,8 @@ protected: float _mc_yaw_weight; // weight for multicopter attitude controller yaw output float _yaw_transition; // yaw angle in which transition will take place - float _throttle_transition; // throttle value used for the transition phase float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + float _throttle_transition; // throttle value used for the transition phase bool _flag_was_in_trans_mode; // true if mode has just switched to transition }; From 7127f6a92fb6060c365c1032c2609ad9d4f60c94 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 22:48:53 +0100 Subject: [PATCH 348/624] fixed code style --- src/modules/vtol_att_control/standard.cpp | 2 +- src/modules/vtol_att_control/tailsitter.cpp | 380 ++++++++++-------- src/modules/vtol_att_control/tailsitter.h | 18 +- .../vtol_att_control/tailsitter_params.c | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 1 + .../vtol_att_control_main.cpp | 5 + .../vtol_att_control/vtol_att_control_main.h | 37 +- 7 files changed, 250 insertions(+), 195 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5a329d52f9..8a5896999e 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -254,7 +254,7 @@ void Standard::update_fw_state() // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 827e3ec617..1776dc141d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,33 +31,33 @@ * ****************************************************************************/ - /** - * @file tailsitter.cpp - * - * @author Roman Bapst - * @author David Vorsin - * - */ +/** +* @file tailsitter.cpp +* +* @author Roman Bapst +* @author David Vorsin +* +*/ #include "tailsitter.h" #include "vtol_att_control_main.h" - #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition - #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition - #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 - #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC -Tailsitter::Tailsitter (VtolAttitudeControl *attc) : -VtolType(attc), -_airspeed_tot(0.0f), -_roll_weight_mc(1.0f), -_pitch_weight_mc(1.0f), -_yaw_weight_mc(1.0f), -_min_front_trans_dur(0.5f), +Tailsitter::Tailsitter(VtolAttitudeControl *attc) : + VtolType(attc), + _airspeed_tot(0.0f), + _roll_weight_mc(1.0f), + _pitch_weight_mc(1.0f), + _yaw_weight_mc(1.0f), + _min_front_trans_dur(0.5f), -_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), -_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -74,7 +74,7 @@ _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinit _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); - + } Tailsitter::~Tailsitter() @@ -90,7 +90,7 @@ Tailsitter::parameters_update() /* vtol duration of a front transition */ param_get(_params_handles_tailsitter.front_trans_dur, &v); - _params_tailsitter.front_trans_dur = math::constrain(v,1.0f,5.0f); + _params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f); /* vtol front transition phase 2 duration */ param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); @@ -98,7 +98,7 @@ Tailsitter::parameters_update() /* vtol duration of a back transition */ param_get(_params_handles_tailsitter.back_trans_dur, &v); - _params_tailsitter.back_trans_dur = math::constrain(v,0.0f,5.0f); + _params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f); /* vtol airspeed at which it is ok to switch to fw mode */ param_get(_params_handles_tailsitter.airspeed_trans, &v); @@ -115,7 +115,7 @@ Tailsitter::parameters_update() /* avoid parameters which will lead to zero division in the transition code */ _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); - if ( _params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f ) { + if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) { _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; } @@ -124,97 +124,115 @@ Tailsitter::parameters_update() void Tailsitter::update_vtol_state() { - parameters_update(); + parameters_update(); - /* simple logic using a two way switch to perform transitions. + /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting in MC control mode, picking up * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. - */ + */ - if (_manual_control_sp->aux1 < 0.0f) { + if (_manual_control_sp->aux1 < 0.0f) { - - switch(_vtol_schedule.flight_mode) { // user switchig to MC mode - case MC_MODE: - break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case TRANSITION_FRONT_P1: - // failsafe into multicopter mode + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // failsafe into multicopter mode + //_vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_FRONT_P2: - // NOT USED - // failsafe into multicopter mode - //_vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_BACK: - // check if we have reached pitch angle to switch to MC mode - if (_v_att->pitch >= PITCH_TRANSITION_BACK) { - _vtol_schedule.flight_mode = MC_MODE; - } - break; + } + + break; } + } else { // user switchig to FW mode - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case FW_MODE: - break; - case TRANSITION_FRONT_P1: - // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { - _vtol_schedule.flight_mode = FW_MODE; - //_vtol_schedule.transition_start = hrt_absolute_time(); - } - break; - case TRANSITION_FRONT_P2: - // NOT USED - // check if we have reached pitch angle to switch to FW mode - //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - // _vtol_schedule.flight_mode = FW_MODE; - //} - break; - case TRANSITION_BACK: - // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; - /* **LATER*** if pitch is closer to mc (-45>) - * go to transition P1 - */ - break; + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) + && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // check if we have reached pitch angle to switch to FW mode + //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + // _vtol_schedule.flight_mode = FW_MODE; + //} + break; + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; } } // map tailsitter specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - case TRANSITION_FRONT_P1: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - case TRANSITION_FRONT_P2: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - case TRANSITION_BACK: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } @@ -223,7 +241,7 @@ void Tailsitter::update_vtol_state() void Tailsitter::update_mc_state() { // set idle speed for rotary wing mode - if (!flag_idle_mc) { + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } @@ -264,19 +282,24 @@ void Tailsitter::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->pitch_body =math::constrain( _v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1-0.2f , _pitch_transition_start ); + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f , + _pitch_transition_start); /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { - _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , + (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } // disable mc yaw control once the plane has picked up speed if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; - }else { + + } else { _mc_yaw_weight = 1.0f; } @@ -294,27 +317,29 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2-0.2f; + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; } } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - + _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { @@ -323,20 +348,23 @@ void Tailsitter::update_transition_state() flag_idle_mc = true; } - /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f); // throttle value is decreesed - _v_att_sp->thrust = _throttle_transition*0.9f; - + _v_att_sp->thrust = _throttle_transition * 0.9f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + } @@ -353,8 +381,8 @@ void Tailsitter::update_transition_state() _v_att_sp->yaw_body = _yaw_transition; _v_att_sp->R_valid = true; - math::Matrix<3,3> R_sp; - R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); math::Quaternion q_sp; @@ -425,49 +453,69 @@ void Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - switch(_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + switch (_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - } else { - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =0; //pitch elevon - } - break; - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; - case TRANSITION: - // in transition engines are mixed by weight (BACK TRANSITION ONLY) - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - break; - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; //pitch elevon + } + + break; + + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + + case TRANSITION: + // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + break; + + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 1b2caca676..de5ab0ba3e 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,13 +31,13 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * @author David Vorsin - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* @author David Vorsin +* +*/ #ifndef TAILSITTER_H #define TAILSITTER_H @@ -114,7 +114,7 @@ private: struct { vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ hrt_abstime transition_start; /**< absoulte time at which front transition started */ - }_vtol_schedule; + } _vtol_schedule; float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ float _roll_weight_mc; /**< multicopter desired roll moment weight */ @@ -125,7 +125,7 @@ private: float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ - /** should this anouncement stay? **/ + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index 52801d9e8c..af38b55693 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -50,7 +50,7 @@ * @min 0.1 * @max 2 * @group VTOL Attitude Control - + PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 24ff7ece2a..3890a71843 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -306,6 +306,7 @@ void Tiltrotor::update_transition_state() _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; _flag_was_in_trans_mode = true; } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 80bb62eccd..5ab0d01b1c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -512,6 +512,7 @@ void VtolAttitudeControl::publish_att_sp() if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + } else { /* advertise and publish */ _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); @@ -740,7 +741,11 @@ VtolAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, +<<<<<<< HEAD (px4_main_t)&VtolAttitudeControl::task_main_trampoline, +======= + (main_t)&VtolAttitudeControl::task_main_trampoline, +>>>>>>> fixed code style nullptr); if (_control_task < 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c7c51247ae..ccf644142c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -107,27 +107,28 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s *get_att() {return &_v_att;} - struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} - struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp () {return &_mc_virtual_att_sp;} - struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp () {return &_fw_virtual_att_sp;} - struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} - struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} - struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} - struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} - struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} - struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} - struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} - struct actuator_armed_s *get_armed() {return &_armed;} - struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} - struct airspeed_s *get_airspeed() {return &_airspeed;} - struct battery_status_s *get_batt_status() {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} struct Params *get_params() {return &_params;} + private: //******************flags & handlers****************************************************** bool _task_should_exit; From 27f027b3e4fd49cd02a84c690ed925b36a788f2d Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 09:58:43 +0100 Subject: [PATCH 349/624] ported vtol module to posix --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5ab0d01b1c..b9c2d64fe2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -741,11 +741,7 @@ VtolAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, -<<<<<<< HEAD (px4_main_t)&VtolAttitudeControl::task_main_trampoline, -======= - (main_t)&VtolAttitudeControl::task_main_trampoline, ->>>>>>> fixed code style nullptr); if (_control_task < 0) { From 330f174967ecd26793bf7ededf6e3c592b2cdde5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 3 Nov 2015 15:10:56 +0100 Subject: [PATCH 350/624] fix --- src/modules/vtol_att_control/tailsitter.cpp | 3 --- src/modules/vtol_att_control/tailsitter.h | 3 --- src/modules/vtol_att_control/vtol_att_control_main.h | 3 +++ 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 1776dc141d..3977959859 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -51,9 +51,6 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : VtolType(attc), _airspeed_tot(0.0f), - _roll_weight_mc(1.0f), - _pitch_weight_mc(1.0f), - _yaw_weight_mc(1.0f), _min_front_trans_dur(0.5f), _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index de5ab0ba3e..f68fbe9106 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -117,9 +117,6 @@ private: } _vtol_schedule; float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ - float _roll_weight_mc; /**< multicopter desired roll moment weight */ - float _pitch_weight_mc; /**< multicopter desired pitch moment weight */ - float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ /** not sure about it yet ?! **/ float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index ccf644142c..0e99d58aab 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -197,6 +197,9 @@ private: param_t elevons_mc_lock; } _params_handles; + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ int _transition_command; VtolType *_vtol_type; // base class for different vtol types From 2e7c1e2b363dd6ad0ed50d71310690815dada7ce Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 3 Nov 2015 15:34:51 +0100 Subject: [PATCH 351/624] rename vtol model for SITL to tailsitter --- Tools/sitl_run.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7dee02868a..f32f6e585e 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -79,7 +79,7 @@ touch rootfs/eeprom/parameters # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../${rc_script}_${program} + lldb -- mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "gdb" ] then gdb --args mainapp ../../../../${rc_script}_${program} From 990eb68bbf98a177f11c96b0cac1170d898685f9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 19 Nov 2015 15:24:17 +0100 Subject: [PATCH 352/624] allow direct switch to fw if disarmed --- src/modules/vtol_att_control/tailsitter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 3977959859..20f975b7fb 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -177,8 +177,8 @@ void Tailsitter::update_vtol_state() case TRANSITION_FRONT_P1: // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) - && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; //_vtol_schedule.transition_start = hrt_absolute_time(); } From 7cb7245a84db391e0d1872ac80e097931399bb30 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 23 Nov 2015 08:03:43 +0100 Subject: [PATCH 353/624] corrected tailsitter actuator control outputs --- src/modules/vtol_att_control/tailsitter.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 20f975b7fb..106f91a8e4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -186,12 +186,6 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_FRONT_P2: - // NOT USED - // check if we have reached pitch angle to switch to FW mode - //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - // _vtol_schedule.flight_mode = FW_MODE; - //} - break; case TRANSITION_BACK: // failsafe into fixed wing mode @@ -467,7 +461,7 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; From 8832e178b0791808967e06593c1fba78da7c71a3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 15 Aug 2015 16:20:41 +0200 Subject: [PATCH 354/624] added class for vtol landing detection --- .../land_detector/MulticopterLandDetector.cpp | 7 ++ .../land_detector/MulticopterLandDetector.h | 17 ++- .../land_detector/VtolLandDetector.cpp | 116 ++++++++++++++++++ src/modules/land_detector/VtolLandDetector.h | 94 ++++++++++++++ .../land_detector/land_detector_main.cpp | 4 + 5 files changed, 232 insertions(+), 6 deletions(-) create mode 100644 src/modules/land_detector/VtolLandDetector.cpp create mode 100644 src/modules/land_detector/VtolLandDetector.h diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index fa636e496c..2f46c21f3f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -95,6 +95,13 @@ bool MulticopterLandDetector::update() // first poll for new data from our subscriptions updateSubscriptions(); + updateParameterCache(false); + + return get_landed_state(); +} + +bool MulticopterLandDetector::get_landed_state() +{ // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d001be4e7f..3ee697a468 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -60,24 +60,29 @@ protected: /** * @brief polls all subscriptions and pulls any data that has changed **/ - void updateSubscriptions(); + virtual void updateSubscriptions(); /** * @brief Runs one iteration of the land detection algorithm **/ - bool update() override; + virtual bool update() override; /** * @brief Initializes the land detection algorithm **/ - void initialize() override; + virtual void initialize() override; - -private: /** * @brief download and update local parameter cache **/ - void updateParameterCache(const bool force); + virtual void updateParameterCache(const bool force); + + /** + * @brief get multicopter landed state + **/ + bool get_landed_state(); + +private: /** * @brief Handles for interesting parameters diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp new file mode 100644 index 0000000000..3643993b18 --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 + * 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 VtolLandDetector.cpp + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#include "VtolLandDetector.h" +#include + +VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), + _paramHandle(), + _params(), + _airspeedSub(-1), + _parameterSub(-1), + _airspeed{}, + _was_in_air(false), + _airspeed_filtered(0) +{ + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); +} + +void VtolLandDetector::initialize() +{ + MulticopterLandDetector::initialize(); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + // download parameters + updateParameterCache(true); +} + +void VtolLandDetector::updateSubscriptions() +{ + MulticopterLandDetector::updateSubscriptions(); + + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool VtolLandDetector::update() +{ + updateSubscriptions(); + updateParameterCache(false); + + // this is returned from the mutlicopter land detector + bool landed = get_landed_state(); + + // for vtol we additionally consider airspeed + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + } else { + // if airspeed does not update, set it to zero and rely on multicopter land detector + _airspeed_filtered = 0.0f; + } + + // only consider airspeed if we have been in air before to avoid false + // detections in the case of wind on the ground + if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) { + landed = false; + } + + _was_in_air = !landed; + + return landed; +} + +void VtolLandDetector::updateParameterCache(const bool force) +{ + MulticopterLandDetector::updateParameterCache(force); + + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h new file mode 100644 index 0000000000..d36140d64f --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 + * 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 VtolLandDetector.h + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#ifndef __VTOL_LAND_DETECTOR_H__ +#define __VTOL_LAND_DETECTOR_H__ + +#include "MulticopterLandDetector.h" +#include + +class VtolLandDetector : public MulticopterLandDetector +{ +public: + VtolLandDetector(); + + +private: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions() override; + + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force) override; + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxAirSpeed; + } _params; + + int _airspeedSub; + int _parameterSub; + + struct airspeed_s _airspeed; + + bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */ + float _airspeed_filtered; /**< low pass filtered airspeed */ +}; + +#endif diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 0aa6e1fad2..1646bf89da 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -53,6 +53,7 @@ #include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" +#include "VtolLandDetector.h" //Function prototypes static int land_detector_start(const char *mode); @@ -113,6 +114,9 @@ static int land_detector_start(const char *mode) } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); + } else if (!strcmp(mode, "vtol")) { + land_detector_task = new VtolLandDetector(); + } else { warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; From c9526af7af78aca439c8236abff3ffa118c4fef4 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 15 Aug 2015 16:30:29 +0200 Subject: [PATCH 355/624] start correct land detector for vtol --- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 7d0c153f09..6462f6954d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -18,4 +18,4 @@ fw_pos_control_l1 start # Start Land Detector # Multicopter for now until we have something for VTOL # -land_detector start multicopter +land_detector start vtol From c6408924cf2bf2507600cb5548a3d7d864be5cd7 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 6 Nov 2015 22:58:24 +0100 Subject: [PATCH 356/624] add vtol land detector to CMakeLists.txt --- src/modules/land_detector/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt index 3882f246c2..9ef684d3b1 100644 --- a/src/modules/land_detector/CMakeLists.txt +++ b/src/modules/land_detector/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( LandDetector.cpp MulticopterLandDetector.cpp FixedwingLandDetector.cpp + VtolLandDetector.cpp DEPENDS platforms__common ) From b83170b7c3e062e377878831aa29173a2467303a Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 25 Nov 2015 15:25:39 +0100 Subject: [PATCH 357/624] use indicated airspeed instead of tas --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f6037564e7..d069786699 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -601,7 +601,7 @@ void AttitudeEstimatorQ::task_main() ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); /* Airspeed - take airspeed measurement directly here as no wind is estimated */ - ctrl_state.airspeed = _airspeed.true_airspeed_m_s; + ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { From e7d2295565a2c486560d45dfa4387ca2a66fd6ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 22:01:35 +0100 Subject: [PATCH 358/624] Bumped max SITL args to 10 --- src/platforms/posix/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index bc350c5b95..7bf431b4f4 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -147,10 +147,10 @@ static void process_line(string &line, bool exit_on_fail) printf("\n"); } - vector appargs(8); + vector appargs(10); stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> - appargs[7]; + appargs[7] >> appargs[8] >> appargs[9]; run_cmd(appargs, exit_on_fail); } From 87202a08b1738affdce4531aa6872a08474d62d4 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Thu, 26 Nov 2015 09:34:48 +1100 Subject: [PATCH 359/624] Navigator: Reorder data members to fix valgrind warnings During construction of an Navigator object, a pointer to the incomplete object was passed to the RTL constructor which then called a method on the incomplete Navigator object accessing uninitialized data. --- src/modules/navigator/navigator.h | 10 +++++----- src/modules/navigator/navigator_main.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 6facdd9081..b154d21769 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -217,6 +217,11 @@ private: bool _inside_fence; /**< vehicle is inside fence */ + bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ + bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated; /**< flags if mission result has seen an update */ + NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ @@ -231,11 +236,6 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ - bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ - bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ - bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ - bool _mission_result_updated; /**< flags if mission result has seen an update */ - control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0227a2ed07..cd6021a971 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -134,6 +134,10 @@ Navigator::Navigator() : _geofence{}, _geofence_violation_warning_sent(false), _inside_fence(true), + _can_loiter_at_sp(false), + _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), + _mission_result_updated(false), _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), @@ -143,10 +147,6 @@ Navigator::Navigator() : _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), - _can_loiter_at_sp(false), - _pos_sp_triplet_updated(false), - _pos_sp_triplet_published_invalid_once(false), - _mission_result_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -694,7 +694,7 @@ void Navigator::publish_mission_result() { _mission_result.instance_count = _mission_instance_count; - + /* lazily publish the mission result only once available */ if (_mission_result_pub != nullptr) { /* publish mission result */ From 5e9a8d0c039a6828b1c8ae70c299402620990831 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Thu, 26 Nov 2015 09:39:24 +1100 Subject: [PATCH 360/624] Baro sim: Add missing initializers --- src/platforms/posix/drivers/barosim/baro.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 0bbb2c1c0d..ac5d31b6b9 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -202,6 +202,8 @@ BAROSIM::BAROSIM(const char *path) : _TEMP(0), _OFF(0), _SENS(0), + _P(0.0), + _T(0.0), _msl_pressure(101325), _baro_topic(nullptr), _orb_class_instance(-1), From 4f7ab6f4f359b7af3b91f87e1926fdc2069c97e6 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Thu, 26 Nov 2015 12:18:42 +1100 Subject: [PATCH 361/624] uORBManager: Make `orb_check` fail safely The `orb_check` function takes a pointer to a `bool` which it then passes to `px4_ioctl`. However, if the call to `px4_ioctl` fails, the bool doesn't get updated (neither set nor cleared). Therefore, in `orb_check` we explicitly set the bool to `false` before passing the pointer to `px4_ioctl`. --- src/modules/uORB/uORBManager_posix.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 7fe3f6a687..3a01038b4a 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -164,6 +164,8 @@ int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *b int uORB::Manager::orb_check(int handle, bool *updated) { + /* Set to false here so that if `px4_ioctl` fails to false. */ + *updated = false; return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } From 0b3889e2e3e9ab50dc9a37d8c7165322bbf80c27 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 11:30:24 +1100 Subject: [PATCH 362/624] INAV: Valgrind fix Make sure `struct position_estimator_inav_params params` is properly initialized. At first there was no indication that this struct was un-initialized because valgrind was issuing warnings about values that were derived (via float calculations) from values in this struct. Maybe instruction re-ordering during optimization caused this disconnect between the source of the problem and the symptom. --- .../position_estimator_inav_main.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 56db8b4c0a..449b9c059f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -216,7 +216,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e fclose(f); } #else -#define write_debug_log(...) +#define write_debug_log(...) #endif /**************************************************************************** @@ -380,6 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; + memset(¶ms, 0, sizeof(params)); struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); @@ -595,7 +596,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f; - + //moving average if (n_flow >= 100) { gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; @@ -617,7 +618,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); n_flow++; } - + /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); @@ -841,7 +842,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; @@ -981,7 +982,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); @@ -1147,7 +1148,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); @@ -1274,7 +1275,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } - + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; From 6c2c2b19a7a588afbd26754fdabe0dc114d23fdc Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 15:49:26 +1100 Subject: [PATCH 363/624] MAVLink: Only update rx count on successful read --- src/modules/mavlink/mavlink_receiver.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c2180a4d8..d139ac99dd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1837,8 +1837,10 @@ MavlinkReceiver::receive_thread(void *arg) } } - /* count received bytes */ - _mavlink->count_rxbytes(nread); + /* count received bytes (nread will be -1 on read error) */ + if (nread > 0) { + _mavlink->count_rxbytes(nread); + } } } From fb4bf7c59c47f9dd536dd4df87252b7d51727afe Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 16:01:06 +1100 Subject: [PATCH 364/624] MAVLink: Fix call to orb_advertise_multi Previously, pointer to an uninitialized int was being passed as the instance pointer. Now we pass a NULL pointer instead and the code being called will use the default instance value of zero. --- src/modules/mavlink/mavlink_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d139ac99dd..37d070718d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1183,8 +1183,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) tstatus.heartbeat_time = tstatus.timestamp; if (_telemetry_status_pub == nullptr) { - int multi_instance; - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, NULL, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); From 67d61488da341d9311ef7b2f7ed8d970de6dd313 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 16:45:01 +1100 Subject: [PATCH 365/624] sensors: Initialize _parameters --- 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 3fbbdc80ba..827f1be916 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -542,6 +542,7 @@ Sensors::Sensors() : memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */ From c23562c9ed05cc9ff8f7ab6c244ffeaeb9b68d2f Mon Sep 17 00:00:00 2001 From: Amir Date: Fri, 27 Nov 2015 22:28:41 +0100 Subject: [PATCH 366/624] GPS: Add height above the ellipsoid info Modification of the GPS message, add height above the ellipsoid info. --- msg/vehicle_gps_position.msg | 1 + src/drivers/gps/ubx.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 25c532ccaa..718144f327 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -3,6 +3,7 @@ uint64 timestamp_position # Timestamp for position information int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters (millimeters) above MSL +int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid uint64 timestamp_variance float32 s_variance_m_s # speed accuracy estimate m/s diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 643b339be3..f6c3fd7882 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -874,6 +874,7 @@ UBX::payload_rx_done(void) _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m + _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height; _gps_position->timestamp_position = hrt_absolute_time(); From 2641ebb99b808fb822240715d710dcb0b70429f9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 13 Nov 2015 12:45:07 -1000 Subject: [PATCH 367/624] Build without pio TEMP - uses nuttx_next_cmake version long term --- src/firmware/nuttx/CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index be6f9b1062..649ce1e8fe 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -41,13 +41,17 @@ add_custom_target(check_weak if(NOT ${BOARD} STREQUAL "sim") + if (config_io_board) + set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin") + endif() px4_nuttx_add_romfs(OUT romfs ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin + EXTRAS ${extras} ) - add_dependencies(romfs fw_io) - + if (config_io_board) + add_dependencies(romfs fw_io) + endif() set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) From d6c6cb72b2ce8eef284817b1ededb7d3be6a6db3 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:24:53 -1000 Subject: [PATCH 368/624] Support PX4_I2C_BUS_ONBOARD not defined --- src/drivers/ll40ls/ll40ls.cpp | 22 +++++++++++----------- src/drivers/px4flow/px4flow.cpp | 2 ++ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8591c4dee3..51e230f189 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -97,7 +97,7 @@ LidarLite *get_dev(const bool use_i2c, const int bus) LidarLite *g_dev = nullptr; if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + g_dev = static_cast(bus == PX4_I2C_BUS_EXPANSION ? g_dev_ext : g_dev_int); if (g_dev == nullptr) { errx(1, "i2c driver not running"); @@ -231,11 +231,12 @@ void start(const bool use_i2c, const int bus) fail: +#ifdef PX4_I2C_BUS_ONBOARD if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } - +#endif if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -255,17 +256,16 @@ fail: void stop(const bool use_i2c, const int bus) { if (use_i2c) { - if (bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } - - } else { + if (bus == PX4_I2C_BUS_EXPANSION) { if (g_dev_ext != nullptr) { delete g_dev_ext; g_dev_ext = nullptr; } + } else { + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } } } else { @@ -293,7 +293,7 @@ test(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; @@ -366,7 +366,7 @@ reset(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 18acccc00c..5b7f1e5557 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -694,7 +694,9 @@ start() #ifdef PX4_I2C_BUS_ESC PX4_I2C_BUS_ESC, #endif +#ifdef PX4_I2C_BUS_ONBOARD PX4_I2C_BUS_ONBOARD, +#endif -1 }; From 09f83e78e5c6cfdae035afb2030db21066ecc751 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:41:16 -1000 Subject: [PATCH 369/624] Support PX4IO_DEVICE_PATH not defined --- src/modules/gpio_led/gpio_led.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 2ff3fc2767..f8c62a88a7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { @@ -207,12 +207,16 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; +#if defined(PX4IO_DEVICE_PATH) if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; } else { gpio_dev = PX4FMU_DEVICE_PATH; } +#else + gpio_dev = PX4FMU_DEVICE_PATH; +#endif /* open GPIO device */ priv->gpio_fd = open(gpio_dev, 0); From 32ae638974684f037dcd99913fc00fd1eab21bc6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:52:36 -1000 Subject: [PATCH 370/624] Support GPIO_CAN2_RX not defined --- src/modules/uavcan/uavcan_main.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 00671decd2..ee79e0573b 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -533,12 +533,17 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - #ifdef GPIO_CAN1_RX +#if defined(GPIO_CAN1_RX) stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); - #endif +#endif +#if defined(GPIO_CAN2_RX) stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); +#endif +#if !defined(GPIO_CAN1_RX) && !defined(GPIO_CAN2_RX) +# error "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX" +#endif /* * CAN driver init From 1b5db91c356dcc1a33f08fb201d8c0e375371b90 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 14:39:32 -1000 Subject: [PATCH 371/624] Added Fix me re chan 1/2 interactions --- src/drivers/stm32/drv_hrt.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index ba148c1335..475c9c9c75 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -221,6 +221,7 @@ /* * Specific registers and bits used by HRT sub-functions */ +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ #if HRT_TIMER_CHANNEL == 1 # define rCCR_HRT rCCR1 /* compare register for HRT */ # define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ @@ -294,7 +295,7 @@ static void hrt_call_invoke(void); # define GTIM_CCER_CC4NP 0 # define PPM_EDGE_FLIP # endif - +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ # if HRT_PPM_CHANNEL == 1 # define rCCR_PPM rCCR1 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ From f3b7585a89482586487d9e9c35c57f0d0a090e9f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 13 Nov 2015 12:39:00 -1000 Subject: [PATCH 372/624] Wip FMUV3 --- Images/px4fmu-v3.prototype | 12 + Makefile | 3 + cmake/configs/nuttx_px4fmu-v3_default.cmake | 178 +++ cmake/nuttx/px4_impl_nuttx.cmake | 8 + nuttx-configs/px4fmu-v3/include/board.h | 295 +++++ .../px4fmu-v3/include/nsh_romfsimg.h | 42 + nuttx-configs/px4fmu-v3/nsh/Make.defs | 177 +++ nuttx-configs/px4fmu-v3/nsh/appconfig | 52 + nuttx-configs/px4fmu-v3/nsh/defconfig | 1068 +++++++++++++++++ nuttx-configs/px4fmu-v3/nsh/setenv.sh | 67 ++ nuttx-configs/px4fmu-v3/scripts/ld.script | 159 +++ nuttx-configs/px4fmu-v3/src/Makefile | 84 ++ nuttx-configs/px4fmu-v3/src/empty.c | 4 + src/drivers/boards/px4fmu-v2/CMakeLists.txt | 2 +- src/drivers/boards/px4fmu-v3/CMakeLists.txt | 47 + src/drivers/boards/px4fmu-v3/board_config.h | 279 +++++ src/drivers/boards/px4fmu-v3/px4fmu3_init.c | 344 ++++++ src/drivers/boards/px4fmu-v3/px4fmu3_led.c | 106 ++ src/drivers/boards/px4fmu-v3/px4fmu_can.c | 144 +++ .../boards/px4fmu-v3/px4fmu_pwm_servo.c | 105 ++ src/drivers/boards/px4fmu-v3/px4fmu_spi.c | 163 +++ src/drivers/boards/px4fmu-v3/px4fmu_usb.c | 108 ++ src/drivers/drv_gpio.h | 25 +- src/drivers/drv_led.h | 2 + src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +- src/drivers/px4fmu/fmu.cpp | 13 +- src/drivers/stm32/adc/adc.cpp | 34 + src/lib/version/version.h | 4 + 28 files changed, 3524 insertions(+), 5 deletions(-) create mode 100644 Images/px4fmu-v3.prototype create mode 100644 cmake/configs/nuttx_px4fmu-v3_default.cmake create mode 100755 nuttx-configs/px4fmu-v3/include/board.h create mode 100644 nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4fmu-v3/nsh/Make.defs create mode 100644 nuttx-configs/px4fmu-v3/nsh/appconfig create mode 100644 nuttx-configs/px4fmu-v3/nsh/defconfig create mode 100755 nuttx-configs/px4fmu-v3/nsh/setenv.sh create mode 100644 nuttx-configs/px4fmu-v3/scripts/ld.script create mode 100644 nuttx-configs/px4fmu-v3/src/Makefile create mode 100644 nuttx-configs/px4fmu-v3/src/empty.c create mode 100644 src/drivers/boards/px4fmu-v3/CMakeLists.txt create mode 100644 src/drivers/boards/px4fmu-v3/board_config.h create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu3_init.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu3_led.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_usb.c diff --git a/Images/px4fmu-v3.prototype b/Images/px4fmu-v3.prototype new file mode 100644 index 0000000000..ada86b9a93 --- /dev/null +++ b/Images/px4fmu-v3.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 11, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv3 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv3", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index b0f49eda3e..61582fc296 100644 --- a/Makefile +++ b/Makefile @@ -130,6 +130,9 @@ px4fmu-v1_default: px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) +px4fmu-v3_default: + $(call cmake-build,nuttx_px4fmu-v3_default) + px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake new file mode 100644 index 0000000000..82f4c1b9a7 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -0,0 +1,178 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + drivers/boards/px4fmu-v3 + drivers/rgbled + drivers/mpu9250 + drivers/hmc5883 + drivers/ms5611 + drivers/mb12xx + drivers/srf02 + drivers/sf0x + drivers/ll40ls + drivers/trone + drivers/gps + drivers/pwm_out_sim + drivers/hott + drivers/hott/hott_telemetry + drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + #drivers/mkblctrl + drivers/px4flow + drivers/oreoled + drivers/gimbal + drivers/pwm_input + drivers/camera_trigger + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib + systemcmds/reboot + #systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/sdlog2 + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + modules/dataman + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_extra_libs + ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver + ) + +set(config_io_extra_libs + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index af7391f45c..e54276b099 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -458,6 +458,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "px4fmu-v3") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "aerocore") set(cpu_flags -mcpu=cortex-m4 diff --git a/nuttx-configs/px4fmu-v3/include/board.h b/nuttx-configs/px4fmu-v3/include/board.h new file mode 100755 index 0000000000..54e21bbfe7 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/include/board.h @@ -0,0 +1,295 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* ESP8266 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v3/nsh/Make.defs b/nuttx-configs/px4fmu-v3/nsh/Make.defs new file mode 100644 index 0000000000..6177f4c36d --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/Make.defs @@ -0,0 +1,177 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -Os +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wnested-externs +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v3/nsh/appconfig b/nuttx-configs/px4fmu-v3/nsh/appconfig new file mode 100644 index 0000000000..0e18aa8ef1 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/appconfig @@ -0,0 +1,52 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4fmu-v3/nsh/defconfig b/nuttx-configs/px4fmu-v3/nsh/defconfig new file mode 100644 index 0000000000..957578233b --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/defconfig @@ -0,0 +1,1068 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=n +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +# CONFIG_USART6_RXDMA is not set +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +# CONFIG_ARCH_IRQPRIO is not set +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V3=y +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HSECLOCK=y +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=4000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=1600 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_USBDEV_TRACE is not set +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/nuttx-configs/px4fmu-v3/nsh/setenv.sh b/nuttx-configs/px4fmu-v3/nsh/setenv.sh new file mode 100755 index 0000000000..265520997d --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v3/scripts/ld.script b/nuttx-configs/px4fmu-v3/scripts/ld.script new file mode 100644 index 0000000000..3649227a43 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/scripts/ld.script @@ -0,0 +1,159 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v3/src/Makefile b/nuttx-configs/px4fmu-v3/src/Makefile new file mode 100644 index 0000000000..a93664b08b --- /dev/null +++ b/nuttx-configs/px4fmu-v3/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v3/src/empty.c b/nuttx-configs/px4fmu-v3/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/px4fmu-v3/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt index 27cb595c34..19b3f02754 100644 --- a/src/drivers/boards/px4fmu-v2/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt @@ -44,4 +44,4 @@ px4_add_module( DEPENDS platforms__common ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v3/CMakeLists.txt new file mode 100644 index 0000000000..e83c7fd7fd --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 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 +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4fmu-v3 + COMPILE_FLAGS + -Os + SRCS + px4fmu_can.c + px4fmu3_init.c + px4fmu_pwm_servo.c + px4fmu_spi.c + px4fmu_usb.c + px4fmu3_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v3/board_config.h b/src/drivers/boards/px4fmu-v3/board_config.h new file mode 100644 index 0000000000..d2cea93b87 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/board_config.h @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 board_config.h + * + * PX4FMUv2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ +//{GPIO_RSSI_IN, 0, 0}, - pio Analog used as PWM +//{0, GPIO_LED_SAFETY, 0}, pio replacement +//{GPIO_SAFETY_SWITCH_IN, 0, 0}, pio replacement +//{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver +//{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c +//{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver +//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot +//{0, GPIO_8266_PD, 0}, Owned by the 8266 driver +//{0, GPIO_8266_RST, 0}, Owned by the 8266 driver + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + +#define GPIO_LED_RED GPIO_LED1 +#define GPIO_LED_GREEN GPIO_LED2 +#define GPIO_LED_BLUE GPIO_LED3 + +/* Define the Chip Selects */ + +#define GPIO_SPI_CS_MPU9250 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_HMC5983 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) +#define GPIO_SPI_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Define the Ready interrupts */ + +#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_DRDY_HMC5983 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) +#define GPIO_DRDY_ICM_20608_G (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) + + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +#define GPIO_SPI_CS_OFF_MPU9250 _PIN_OFF(GPIO_SPI_CS_MPU9250) +#define GPIO_SPI_CS_OFF_HMC5983 _PIN_OFF(GPIO_SPI_CS_HMC5983) +#define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611) +#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G) + +#define GPIO_DRDY_OFF_MPU9250 _PIN_OFF(GPIO_DRDY_MPU9250) +#define GPIO_DRDY_OFF_HMC5983 _PIN_OFF(GPIO_DRDY_HMC5983) +#define GPIO_DRDY_OFF_ICM_20608_G _PIN_OFF(GPIO_DRDY_ICM_20608_G) + + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +/* Devices on the external bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ + +#define HRT_PPM_CHANNEL 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF3|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN7) + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + +#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) +#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_init.c b/src/drivers/boards/px4fmu-v3/px4fmu3_init.c new file mode 100644 index 0000000000..a1e53d41c5 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_init.c @@ -0,0 +1,344 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include + +/* TODO XXX commented this out to get cmake build working */ +/*#ifdef __cplusplus*/ +/*__EXPORT int matherr(struct __exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#else*/ +/*__EXPORT int matherr(struct exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#endif*/ + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_GPIO5_OUTPUT); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + +#ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + message("[boot] Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + + if (ret != OK) { + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + +#endif + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c new file mode 100644 index 0000000000..59ed10483e --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + + + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, // Indexed by LED_BLUE + GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap)/sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_can.c b/src/drivers/boards/px4fmu-v3/px4fmu_can.c new file mode 100644 index 0000000000..62f170957d --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c new file mode 100644 index 0000000000..600dfef412 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c new file mode 100644 index 0000000000..e0c92c1448 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + + stm32_configgpio(GPIO_DRDY_MPU9250); + stm32_configgpio(GPIO_DRDY_HMC5983); + stm32_configgpio(GPIO_DRDY_ICM_20608_G); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif + +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_usb.c b/src/drivers/boards/px4fmu-v3/px4fmu_usb.c new file mode 100644 index 0000000000..e4cfc27e15 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index acddadfdbe..c5894be9e7 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,29 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +/* + * PX4FMUv3 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE /* * AeroCore GPIO numbers and configuration. @@ -121,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index dbcfde91fa..3daafc963f 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -52,6 +52,8 @@ #define LED_RED 1 /* some boards have red rather than amber */ #define LED_BLUE 0 #define LED_SAFETY 2 +#define LED_GREEN 3 + #define LED_ON _PX4_IOC(_LED_BASE, 0) #define LED_OFF _PX4_IOC(_LED_BASE, 1) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ae77a07fd0..a07cc5fd5e 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -335,7 +335,7 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); @@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) } temperature -= voltage_diff * temp_slope; -#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) } /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 64d9dced4a..5b5dbf3a6f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -121,7 +121,7 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -232,6 +232,17 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V3) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, +#endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) /* AeroCore breaks out User GPIOs on J11 */ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index ab458895a6..43189d29f3 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -348,6 +348,40 @@ ADC::update_system_power(void) } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active High + system_power.brick_valid = stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = 1; + + // OC pins are not supported + system_power.periph_5V_OC = 0; + system_power.hipower_5V_OC = 0; + + /* lazily publish */ + if (_to_system_power != nullptr) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } + +#endif // CONFIG_ARCH_BOARD_PX4FMU_V3 } uint16_t diff --git a/src/lib/version/version.h b/src/lib/version/version.h index b71ba1d0d9..0f3af2ab8e 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,6 +51,10 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#define HW_ARCH "PX4FMU_V3" +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE #define HW_ARCH "AEROCORE" #endif From 30bc968ed6c936fcc596fa7be1cb70344b7ef8d1 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 15:06:50 -1000 Subject: [PATCH 373/624] Ran Astyle --- src/drivers/ll40ls/ll40ls.cpp | 4 ++++ src/modules/gpio_led/gpio_led.c | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 51e230f189..5e50d34934 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -232,11 +232,14 @@ void start(const bool use_i2c, const int bus) fail: #ifdef PX4_I2C_BUS_ONBOARD + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } + #endif + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -261,6 +264,7 @@ void stop(const bool use_i2c, const int bus) delete g_dev_ext; g_dev_ext = nullptr; } + } else { if (g_dev_int != nullptr) { delete g_dev_int; diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index f8c62a88a7..302405ce76 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -208,12 +208,14 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; #if defined(PX4IO_DEVICE_PATH) + if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; } else { gpio_dev = PX4FMU_DEVICE_PATH; } + #else gpio_dev = PX4FMU_DEVICE_PATH; #endif From 7fb90d751f80cfacbe25df3b6eadb36c1b37fccb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 15:07:11 -1000 Subject: [PATCH 374/624] Ran Astyle --- src/drivers/boards/px4fmu-v3/px4fmu3_led.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c index 59ed10483e..ddb34e8a56 100644 --- a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c @@ -72,7 +72,7 @@ static uint32_t g_ledmap[] = { __EXPORT void led_init(void) { /* Configure LED GPIOs for output */ - for (size_t l = 0; l < (sizeof(g_ledmap)/sizeof(g_ledmap[0])); l++) { + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { stm32_configgpio(g_ledmap[l]); } } From a67097731a903c256b17a162fcdec85b3167a09f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 04:50:53 -1000 Subject: [PATCH 375/624] Renamed pax4fmu-v3 to pax4fmu-v4 --- Images/{px4fmu-v3.prototype => px4fmu-v4.prototype} | 4 ++-- Makefile | 4 ++-- ...x4fmu-v3_default.cmake => nuttx_px4fmu-v4_default.cmake} | 2 +- cmake/nuttx/px4_impl_nuttx.cmake | 2 +- nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/board.h | 0 .../{px4fmu-v3 => px4fmu-v4}/include/nsh_romfsimg.h | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/Make.defs | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/appconfig | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/defconfig | 2 +- nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/setenv.sh | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/scripts/ld.script | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/Makefile | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/empty.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/CMakeLists.txt | 6 +++--- src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/board_config.h | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_can.c | 0 .../{px4fmu-v3/px4fmu3_init.c => px4fmu-v4/px4fmu_init.c} | 0 .../{px4fmu-v3/px4fmu3_led.c => px4fmu-v4/px4fmu_led.c} | 0 .../boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_pwm_servo.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_spi.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_usb.c | 0 src/drivers/drv_gpio.h | 4 ++-- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- src/drivers/px4fmu/fmu.cpp | 4 ++-- src/drivers/stm32/adc/adc.cpp | 4 ++-- src/lib/version/version.h | 4 ++-- src/modules/gpio_led/gpio_led.c | 6 +++--- 27 files changed, 23 insertions(+), 23 deletions(-) rename Images/{px4fmu-v3.prototype => px4fmu-v4.prototype} (68%) rename cmake/configs/{nuttx_px4fmu-v3_default.cmake => nuttx_px4fmu-v4_default.cmake} (99%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/board.h (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/nsh_romfsimg.h (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/Make.defs (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/appconfig (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/defconfig (99%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/setenv.sh (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/scripts/ld.script (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/Makefile (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/empty.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/CMakeLists.txt (96%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/board_config.h (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_can.c (100%) rename src/drivers/boards/{px4fmu-v3/px4fmu3_init.c => px4fmu-v4/px4fmu_init.c} (100%) rename src/drivers/boards/{px4fmu-v3/px4fmu3_led.c => px4fmu-v4/px4fmu_led.c} (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_pwm_servo.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_spi.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_usb.c (100%) diff --git a/Images/px4fmu-v3.prototype b/Images/px4fmu-v4.prototype similarity index 68% rename from Images/px4fmu-v3.prototype rename to Images/px4fmu-v4.prototype index ada86b9a93..24e72f868d 100644 --- a/Images/px4fmu-v3.prototype +++ b/Images/px4fmu-v4.prototype @@ -1,10 +1,10 @@ { "board_id": 11, "magic": "PX4FWv1", - "description": "Firmware for the PX4FMUv3 board", + "description": "Firmware for the PX4FMUv4 board", "image": "", "build_time": 0, - "summary": "PX4FMUv3", + "summary": "PX4FMUv4", "version": "0.1", "image_size": 0, "git_identity": "", diff --git a/Makefile b/Makefile index 61582fc296..cb7c7fbc8e 100644 --- a/Makefile +++ b/Makefile @@ -130,8 +130,8 @@ px4fmu-v1_default: px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) -px4fmu-v3_default: - $(call cmake-build,nuttx_px4fmu-v3_default) +px4fmu-v4_default: + $(call cmake-build,nuttx_px4fmu-v4_default) px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake similarity index 99% rename from cmake/configs/nuttx_px4fmu-v3_default.cmake rename to cmake/configs/nuttx_px4fmu-v4_default.cmake index 82f4c1b9a7..cc1db95eef 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -12,7 +12,7 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/boards/px4fmu-v3 + drivers/boards/px4fmu-v4 drivers/rgbled drivers/mpu9250 drivers/hmc5883 diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index e54276b099..79de5c3939 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -458,7 +458,7 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) - elseif (${BOARD} STREQUAL "px4fmu-v3") + elseif (${BOARD} STREQUAL "px4fmu-v4") set(cpu_flags -mcpu=cortex-m4 -mthumb diff --git a/nuttx-configs/px4fmu-v3/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h similarity index 100% rename from nuttx-configs/px4fmu-v3/include/board.h rename to nuttx-configs/px4fmu-v4/include/board.h diff --git a/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h similarity index 100% rename from nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h rename to nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h diff --git a/nuttx-configs/px4fmu-v3/nsh/Make.defs b/nuttx-configs/px4fmu-v4/nsh/Make.defs similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/Make.defs rename to nuttx-configs/px4fmu-v4/nsh/Make.defs diff --git a/nuttx-configs/px4fmu-v3/nsh/appconfig b/nuttx-configs/px4fmu-v4/nsh/appconfig similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/appconfig rename to nuttx-configs/px4fmu-v4/nsh/appconfig diff --git a/nuttx-configs/px4fmu-v3/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig similarity index 99% rename from nuttx-configs/px4fmu-v3/nsh/defconfig rename to nuttx-configs/px4fmu-v4/nsh/defconfig index 957578233b..551a9cd4ff 100644 --- a/nuttx-configs/px4fmu-v3/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -384,7 +384,7 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_PX4FMU_V3=y +CONFIG_ARCH_BOARD_PX4FMU_V4=y CONFIG_ARCH_BOARD_CUSTOM=y CONFIG_ARCH_BOARD="" diff --git a/nuttx-configs/px4fmu-v3/nsh/setenv.sh b/nuttx-configs/px4fmu-v4/nsh/setenv.sh similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/setenv.sh rename to nuttx-configs/px4fmu-v4/nsh/setenv.sh diff --git a/nuttx-configs/px4fmu-v3/scripts/ld.script b/nuttx-configs/px4fmu-v4/scripts/ld.script similarity index 100% rename from nuttx-configs/px4fmu-v3/scripts/ld.script rename to nuttx-configs/px4fmu-v4/scripts/ld.script diff --git a/nuttx-configs/px4fmu-v3/src/Makefile b/nuttx-configs/px4fmu-v4/src/Makefile similarity index 100% rename from nuttx-configs/px4fmu-v3/src/Makefile rename to nuttx-configs/px4fmu-v4/src/Makefile diff --git a/nuttx-configs/px4fmu-v3/src/empty.c b/nuttx-configs/px4fmu-v4/src/empty.c similarity index 100% rename from nuttx-configs/px4fmu-v3/src/empty.c rename to nuttx-configs/px4fmu-v4/src/empty.c diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt similarity index 96% rename from src/drivers/boards/px4fmu-v3/CMakeLists.txt rename to src/drivers/boards/px4fmu-v4/CMakeLists.txt index e83c7fd7fd..b16a991331 100644 --- a/src/drivers/boards/px4fmu-v3/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt @@ -31,16 +31,16 @@ # ############################################################################ px4_add_module( - MODULE drivers__boards__px4fmu-v3 + MODULE drivers__boards__px4fmu-v4 COMPILE_FLAGS -Os SRCS px4fmu_can.c - px4fmu3_init.c + px4fmu_init.c px4fmu_pwm_servo.c px4fmu_spi.c px4fmu_usb.c - px4fmu3_led.c + px4fmu_led.c DEPENDS platforms__common ) diff --git a/src/drivers/boards/px4fmu-v3/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h similarity index 100% rename from src/drivers/boards/px4fmu-v3/board_config.h rename to src/drivers/boards/px4fmu-v4/board_config.h diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_can.c b/src/drivers/boards/px4fmu-v4/px4fmu_can.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_can.c rename to src/drivers/boards/px4fmu-v4/px4fmu_can.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu3_init.c rename to src/drivers/boards/px4fmu-v4/px4fmu_init.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu3_led.c rename to src/drivers/boards/px4fmu-v4/px4fmu_led.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c rename to src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_spi.c rename to src/drivers/boards/px4fmu-v4/px4fmu_spi.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_usb.c b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_usb.c rename to src/drivers/boards/px4fmu-v4/px4fmu_usb.c diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index c5894be9e7..971c22b2d4 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,7 +94,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 /* * PX4FMUv3 GPIO numbers. * @@ -144,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a07cc5fd5e..eaf68a3191 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -335,7 +335,7 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); @@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) } temperature -= voltage_diff * temp_slope; -#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5b5dbf3a6f..ae1320b491 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -121,7 +121,7 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -232,7 +232,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 43189d29f3..9c6e60947a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -348,7 +348,7 @@ ADC::update_system_power(void) } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 system_power_s system_power; system_power.timestamp = hrt_absolute_time(); @@ -381,7 +381,7 @@ ADC::update_system_power(void) _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } -#endif // CONFIG_ARCH_BOARD_PX4FMU_V3 +#endif // CONFIG_ARCH_BOARD_PX4FMU_V4 } uint16_t diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 0f3af2ab8e..9492cec90f 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,8 +51,8 @@ #define HW_ARCH "PX4FMU_V2" #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 -#define HW_ARCH "PX4FMU_V3" +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 +#define HW_ARCH "PX4FMU_V4" #endif #ifdef CONFIG_ARCH_BOARD_AEROCORE diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 302405ce76..531a921b83 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { From 75b96732b5a0de6b17f0646948077576052a0a11 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 09:20:40 -1000 Subject: [PATCH 376/624] Extended to support PX4FMU_V4 hw --- ROMFS/px4fmu_common/init.d/rc.sensors | 53 +++++++++++++++++++-------- 1 file changed, 38 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 08a044329f..7b8f20839d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -67,32 +67,55 @@ then fi fi else - # FMUv1 - if mpu6000 start + if ver hwcmp PX4FMU_V4 then - fi + # External I2C bus + if hmc5883 -C -T -X start + then + fi - if l3gd20 start - then - fi + # Internal SPI bus + if hmc5883 -C -T -S -R 8 start + then + fi - # MAG selection - if param compare SENS_EXT_MAG 2 - then - if hmc5883 -C -I start + # Internal SPI bus mpu9250 is rotated ?? + if mpu9250 -R 0 start + then + fi + + # Internal SPI bus try MPU6K on ICM-20608-G is rotated 180 deg roll, 270 deg yaw + if mpu6000 -R 14 start then fi else - # Use only external as primary - if param compare SENS_EXT_MAG 1 + # FMUv1 + if mpu6000 start then - if hmc5883 -C -X start + fi + + if l3gd20 start + then + fi + + # MAG selection + if param compare SENS_EXT_MAG 2 + then + if hmc5883 -C -I start then fi else - # auto-detect the primary, prefer external - if hmc5883 start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then + if hmc5883 -C -X start + then + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + fi fi fi fi From cb6327ebabf20a23d394e3ab35602851599bf39d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 09:23:21 -1000 Subject: [PATCH 377/624] Force USE_IO to no on FMUV4 --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 411eb0d892..44b85c35bc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -217,7 +217,12 @@ then # if param compare SYS_USE_IO 1 then - set USE_IO yes + if ver hwcmp PX4FMU_V4 + then + set USE_IO no + else + set USE_IO yes + fi else set USE_IO no fi From 46c63da8bed2ce65fe86de3ed0bff293153df4c4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 12:13:24 -1000 Subject: [PATCH 378/624] Added support for ICM-20608-G to MPU6000 driver --- src/drivers/mpu6000/mpu6000.cpp | 68 ++++++++++++++++++++++++++------- 1 file changed, 55 insertions(+), 13 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ab2eede349..0c902e2739 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,12 @@ #define BIT_INT_STATUS_DATA 0x01 #define MPU_WHOAMI_6000 0x68 +#define ICM_WHOAMI_20608 0xaf + +// Product ID Description for ICM2608 +// There is none + +#define ICM20608_REV_00 0 // Product ID Description for MPU6000 // high 4 bits low 4 bits @@ -208,7 +214,8 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type); virtual ~MPU6000(); virtual int init(); @@ -242,6 +249,7 @@ protected: virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: + int _device_type; MPU6000_gyro *_gyro; uint8_t _product; /** product code */ @@ -326,6 +334,15 @@ private: */ int reset(); + /** + * is_icm_device + */ + bool is_icm_device() { return _device_type == 20608;} + /** + * is_mpu_device + */ + bool is_mpu_device() { return _device_type == 6000;} + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -507,8 +524,10 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call{}, @@ -780,16 +799,15 @@ int MPU6000::reset() int MPU6000::probe() { - uint8_t whoami; - whoami = read_reg(MPUREG_WHOAMI); + uint8_t whoami = read_reg(MPUREG_WHOAMI); + uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; - if (whoami != MPU_WHOAMI_6000) { + if (whoami != expected) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; - } - /* look for a product ID we recognise */ + /* look for a product ID we recognize */ _product = read_reg(MPUREG_PRODUCT_ID); // verify product revision @@ -806,6 +824,7 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: + case ICM20608_REV_00: DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; @@ -1485,6 +1504,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + // There is no MPUREG_PRODUCT_ID on the icm device + // so lets make dummy it up and allow the rest of the + // code to run as is + if (reg == MPUREG_PRODUCT_ID && is_icm_device()) { + return ICM20608_REV_00; + } + // general register transfer at low clock speed set_frequency(speed); @@ -2054,7 +2080,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation, int range); +void start(bool, enum Rotation, int range, int device_type); void stop(bool); void test(bool); void reset(bool); @@ -2071,7 +2097,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation, int range) +start(bool external_bus, enum Rotation rotation, int range, int device_type) { int fd; MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; @@ -2087,13 +2113,23 @@ start(bool external_bus, enum Rotation rotation, int range) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); +# if defined(PX4_SPIDEV_EXT_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM); +# else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU; +# endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); +#if defined(PX4_SPIDEV_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM); +#else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU; +#endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type); } if (*g_dev_ptr == nullptr) { @@ -2337,6 +2373,7 @@ usage() warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -M 6000|20608 (default 6000)"); warnx(" -R rotation"); warnx(" -a accel range (in g)"); } @@ -2347,17 +2384,22 @@ int mpu6000_main(int argc, char *argv[]) { bool external_bus = false; + int device_type = 6000; int ch; enum Rotation rotation = ROTATION_NONE; int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { + while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'T': + device_type = atoi(optarg); + break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; @@ -2379,7 +2421,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation, accel_range); + mpu6000::start(external_bus, rotation, accel_range, device_type); } if (!strcmp(verb, "stop")) { From 6df5aab064e656b712363bea6a683b8bf8fd332a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 12:19:35 -1000 Subject: [PATCH 379/624] px4fmu-v4 uses MPU6000 driver for ICM-20609-G --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++-- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + src/drivers/boards/px4fmu-v4/board_config.h | 1 + src/drivers/boards/px4fmu-v4/px4fmu_spi.c | 2 +- 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 7b8f20839d..f11e61036a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -84,8 +84,8 @@ else then fi - # Internal SPI bus try MPU6K on ICM-20608-G is rotated 180 deg roll, 270 deg yaw - if mpu6000 -R 14 start + # Internal SPI bus ICM-20608-G is rotated 180 deg roll, 270 deg yaw + if mpu6000 -R 14 -T 20608 start then fi else diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index cc1db95eef..0b873ccb98 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -14,6 +14,7 @@ set(config_module_list drivers/px4fmu drivers/boards/px4fmu-v4 drivers/rgbled + drivers/mpu6000 drivers/mpu9250 drivers/hmc5883 drivers/ms5611 diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index d2cea93b87..1fa6f0f2d6 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -127,6 +127,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_ICM 6 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c index e0c92c1448..bdfa47f62c 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -101,7 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, /* SPI select is active low, so write !selected to select the device */ switch (devid) { - case PX4_SPIDEV_GYRO: + case PX4_SPIDEV_ICM: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); From 731daec744c1f854c2f3320c4f01048532bc08cf Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 13:58:57 -1000 Subject: [PATCH 380/624] Added missing conditional compilation control for FMUV4 --- src/drivers/px4fmu/fmu.cpp | 108 +++++++++++++++++- .../drivers/airspeedsim/meas_airspeed_sim.cpp | 2 +- 2 files changed, 104 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index ae1320b491..4c8b03b44c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1277,7 +1277,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_4PWM); break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) case 6: set_mode(MODE_6PWM); @@ -1490,6 +1490,79 @@ PX4FMU::sensor_reset(int ms) // stm32_configgpio(GPIO_ACCEL_DRDY); // stm32_configgpio(GPIO_EXTI_MPU_DRDY); +#endif +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250); + stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983); + stm32_configgpio(GPIO_SPI_CS_OFF_MS5611); + stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0); + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + stm32_configgpio(GPIO_DRDY_OFF_MPU9250); + stm32_configgpio(GPIO_DRDY_OFF_HMC5983); + stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0); + + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1516,6 +1589,31 @@ PX4FMU::peripheral_reset(int ms) /* switch the peripheral rail back on */ stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 10; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER); + /* Keep Spektum on to discharge rail*/ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last); + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); +#endif } void @@ -1707,7 +1805,7 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) servo_mode = PX4FMU::MODE_6PWM; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -1715,7 +1813,7 @@ fmu_new_mode(PortMode new_mode) #endif break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) case PORT_PWM4: /* select 4-pin PWM mode */ @@ -2023,7 +2121,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; @@ -2112,7 +2210,7 @@ fmu_main(int argc, char *argv[]) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 89691f7483..ae43a2cd5d 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -291,7 +291,7 @@ MEASAirspeedSim::cycle() void MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); From 6585b629e1d0b38ce4f598f66f85e00fb22195f7 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 16:32:08 -1000 Subject: [PATCH 381/624] Set Rotations for 6 and 9 axis --- ROMFS/px4fmu_common/init.d/rc.sensors | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f11e61036a..a822b82a4a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -75,17 +75,18 @@ else fi # Internal SPI bus - if hmc5883 -C -T -S -R 8 start + if hmc5883 -C -T -S -R 0 + start then fi - # Internal SPI bus mpu9250 is rotated ?? - if mpu9250 -R 0 start + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start then fi - # Internal SPI bus ICM-20608-G is rotated 180 deg roll, 270 deg yaw - if mpu6000 -R 14 -T 20608 start + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start then fi else From 775b64595c9a0cd4fa64610c5850308918cda8d9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 04:53:51 -1000 Subject: [PATCH 382/624] Fixed hmc5983 --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a822b82a4a..ad8f55c382 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -74,9 +74,8 @@ else then fi - # Internal SPI bus - if hmc5883 -C -T -S -R 0 - start + # Internal SPI bus is rotated 90 deg yaw + if hmc5883 -C -T -S -R 2 start then fi From a631a595e550af517160d813fe795c71b81d0279 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 05:11:58 -1000 Subject: [PATCH 383/624] Added lib/terrain_estimation and lib/runway_takeoff requierd from rebase on master --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 0b873ccb98..850b1a55ea 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -115,6 +115,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config From fa3cccc96a7bd8b3f8f91ebd02bdc3f43e7811d6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 20 Nov 2015 02:22:40 -1000 Subject: [PATCH 384/624] Start mpu6000 driver before mpu9250 -> need to change cal code --- ROMFS/px4fmu_common/init.d/rc.sensors | 8 ++++---- src/modules/commander/gyro_calibration.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ad8f55c382..a7bc6b0815 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -79,13 +79,13 @@ else then fi - # Internal SPI bus mpu9250 is rotated 90 deg yaw - if mpu9250 -R 2 start + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start then fi - # Internal SPI bus ICM-20608-G is rotated 90 deg yaw - if mpu6000 -R 2 -T 20608 start + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start then fi else diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a18a13dfa0..1fcd48b088 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -137,7 +137,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } for (unsigned s = 0; s < max_gyros; s++) { - if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 4) { + if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } From ba4fdf197e38dd0f367a2cd00d7c5405e1b0ef70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:06 +0100 Subject: [PATCH 385/624] Move S.BUS and DSM decoders into RC lib --- src/{modules/px4iofirmware => lib/rc}/dsm.c | 0 src/{modules/px4iofirmware => lib/rc}/sbus.c | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{modules/px4iofirmware => lib/rc}/dsm.c (100%) rename src/{modules/px4iofirmware => lib/rc}/sbus.c (100%) diff --git a/src/modules/px4iofirmware/dsm.c b/src/lib/rc/dsm.c similarity index 100% rename from src/modules/px4iofirmware/dsm.c rename to src/lib/rc/dsm.c diff --git a/src/modules/px4iofirmware/sbus.c b/src/lib/rc/sbus.c similarity index 100% rename from src/modules/px4iofirmware/sbus.c rename to src/lib/rc/sbus.c From d2b154cd07045106bca1284d4bdc9814e2405849 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:26 +0100 Subject: [PATCH 386/624] Build S.BUS and DSM decoders in RC lib --- src/lib/rc/CMakeLists.txt | 1 + src/lib/rc/dsm.c | 37 ++++++++++++++------------ src/lib/rc/dsm.h | 52 ++++++++++++++++++++++++++++++++++++ src/lib/rc/sbus.c | 35 ++++++++++--------------- src/lib/rc/sbus.h | 55 +++++++++++++++++++++++++++++++++++++++ src/lib/rc/sumd.h | 2 +- 6 files changed, 144 insertions(+), 38 deletions(-) create mode 100644 src/lib/rc/dsm.h create mode 100644 src/lib/rc/sbus.h diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index 686f398e27..dd253a3f0a 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( SRCS st24.c sumd.c + sbus.c DEPENDS platforms__common ) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index fc6fbf82b8..7f446685d7 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -46,9 +46,16 @@ #include #include +#include "dsm.h" #include -#include "px4io.h" +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + #include +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + #include +#endif #define DSM_FRAME_SIZE 16 /** + */ + +#pragma once + +#include + +__BEGIN_DECLS + +__EXPORT int dsm_init(const char *device); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); +__EXPORT void dsm_bind(uint16_t cmd, int pulses); + +__END_DECLS diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 11f336a286..39642f0c1e 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -43,15 +43,9 @@ #include #include -#include - +#include "sbus.h" #include -#define DEBUG -#include "px4io.h" -#include "protocol.h" -#include "debug.h" - #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 @@ -77,8 +71,6 @@ #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; @@ -93,11 +85,9 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_ bool *sbus_frame_drop, uint16_t max_channels); int -sbus_init(const char *device) +sbus_init(const char *device, bool singlewire) { - if (sbus_fd < 0) { - sbus_fd = open(device, O_RDWR | O_NONBLOCK); - } + int sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -108,21 +98,24 @@ sbus_init(const char *device) t.c_cflag |= (CSTOPB | PARENB); tcsetattr(sbus_fd, TCSANOW, &t); + if (singlewire) { + /* only defined in configs capable of IOCTL */ + #ifdef SBUS_SERIAL_PORT + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + #endif + } + /* initialise the decoder */ partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - debug("S.Bus: ready"); - - } else { - debug("S.Bus: open failed"); } return sbus_fd; } void -sbus1_output(uint16_t *values, uint16_t num_values) +sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) { uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; @@ -161,14 +154,14 @@ sbus1_output(uint16_t *values, uint16_t num_values) } } void -sbus2_output(uint16_t *values, uint16_t num_values) +sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) { char b = 'B'; write(sbus_fd, &b, 1); } bool -sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -331,7 +324,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } /* decode switch channels if data fields are wide enough */ - if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { + if (max_values > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h new file mode 100644 index 0000000000..afe7a66e52 --- /dev/null +++ b/src/lib/rc/sbus.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 + * 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 sbus.h + * + * RC protocol definition for S.BUS + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +__BEGIN_DECLS + +__EXPORT int sbus_init(const char *device, bool singlewire); +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels); +__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); +__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); + +__END_DECLS diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index ba9e830b91..65c669f1ed 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -101,7 +101,7 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value); __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); */ -int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, +__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); From 2d5b02e967840f0a3808c0efda7395e2fae741bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:53 +0100 Subject: [PATCH 387/624] Update IOv1 --- src/drivers/boards/px4io-v1/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index e05e030713..f83f18e503 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -65,6 +65,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) From 83ed9e10967593baca4edb180554739b9a039352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:06 +0100 Subject: [PATCH 388/624] Update IOv2 config --- src/drivers/boards/px4io-v2/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index e9b17e7dc7..d97f3b203c 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -79,6 +79,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *******************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) From 10a3954232fb44061c6574c57fb459b286e84e93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:23 +0100 Subject: [PATCH 389/624] IO firmware: Depend on external RC lib --- src/modules/px4iofirmware/CMakeLists.txt | 5 +++-- src/modules/px4iofirmware/controls.c | 9 ++++++--- src/modules/px4iofirmware/mixer.cpp | 11 +++++++---- src/modules/px4iofirmware/px4io.h | 8 -------- src/modules/px4iofirmware/registers.c | 1 + 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 3319bec6a2..854bbdcc80 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -64,6 +64,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ") include_directories( ${include_dirs} ${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer + . ) link_directories(${link_dirs}) add_definitions(${definitions}) @@ -71,11 +72,9 @@ add_definitions(${definitions}) set(srcs adc.c controls.c - dsm.c px4io.c registers.c safety.c - sbus.c ../systemlib/up_cxxinitialize.c ../systemlib/perf_counter.c mixer.cpp @@ -86,6 +85,8 @@ set(srcs ../systemlib/pwm_limit/pwm_limit.c ../../lib/rc/st24.c ../../lib/rc/sumd.c + ../../lib/rc/sbus.c + ../../lib/rc/dsm.c ../../drivers/stm32/drv_hrt.c ../../drivers/stm32/drv_pwm_servo.c ../../drivers/boards/${config_io_board}/px4io_init.c diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 65d49a388a..25874868b1 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -46,6 +46,8 @@ #include #include #include +#include +#include #include "px4io.h" @@ -60,7 +62,8 @@ static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; -static int _dsm_fd; +static int _dsm_fd = -1; +int _sbus_fd = -1; static uint16_t rc_value_override = 0; @@ -158,7 +161,7 @@ controls_init(void) _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ - sbus_init("/dev/ttyS2"); + _sbus_fd = sbus_init("/dev/ttyS2", false); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { @@ -240,7 +243,7 @@ controls_tick() perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0a0962367d..4ff95bd5af 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -71,6 +72,8 @@ static bool should_arm_nothrottle = false; static bool should_always_enable_pwm = false; static volatile bool in_mixer = false; +extern int _sbus_fd; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -292,10 +295,10 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } } else if (mixer_servos_armed && should_always_enable_pwm) { @@ -306,11 +309,11 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 51ed9c6e40..2197b7b776 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -224,14 +224,6 @@ extern uint16_t adc_measure(unsigned channel); */ extern void controls_init(void); extern void controls_tick(void); -extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); -extern void dsm_bind(uint16_t cmd, int pulses); -extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, - uint16_t max_channels); -extern void sbus1_output(uint16_t *values, uint16_t num_values); -extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 18f5db5645..a3351bcbd5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -47,6 +47,7 @@ #include #include #include +#include #include "px4io.h" #include "protocol.h" From c2c6f3c9584bd585dc6f8d855ed22cc0bffa9de2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:59 +0100 Subject: [PATCH 390/624] FMU driver: Add S.BUS and DSM FDs, not active yet --- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4c8b03b44c..9e1ea99861 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -141,6 +141,8 @@ private: orb_advert_t _outputs_pub; unsigned _num_outputs; int _class_instance; + int _sbus_fd; + int _dsm_fd; volatile bool _initialized; bool _servo_armed; @@ -285,6 +287,8 @@ PX4FMU::PX4FMU() : _outputs_pub(nullptr), _num_outputs(0), _class_instance(0), + _sbus_fd(-1), + _dsm_fd(-1), _initialized(false), _servo_armed(false), _pwm_on(false), @@ -318,6 +322,14 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + +#ifdef DSM_SERIAL_PORT + _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + /* only enable this during development */ _debug_enabled = false; } From b137a24f306cb731eb76dd0074718e2835e73c3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:54:39 +0100 Subject: [PATCH 391/624] Free timer 3 --- nuttx-configs/px4fmu-v4/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 551a9cd4ff..96cd833d45 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -241,12 +241,12 @@ CONFIG_STM32_SPI2=y CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM1=y # CONFIG_STM32_TIM2 is not set -CONFIG_STM32_TIM3=y +# CONFIG_STM32_TIM3 is not set CONFIG_STM32_TIM4=y # CONFIG_STM32_TIM5 is not set # CONFIG_STM32_TIM6 is not set # CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM8=y CONFIG_STM32_TIM9=y CONFIG_STM32_TIM10=y CONFIG_STM32_TIM11=y From abfd471fed6904ff74837f525d5e65f19e1324bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:57:40 +0100 Subject: [PATCH 392/624] v4 board config: Move HRT to TIM3 --- src/drivers/boards/px4fmu-v4/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 1fa6f0f2d6..3500d35e61 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -208,11 +208,11 @@ __BEGIN_DECLS #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER 3 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ -#define HRT_PPM_CHANNEL 2 /* use capture/compare channel 2 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF3|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN7) +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 From 09ef7fa9ca7a97c4b7da3e79b220aadd340c5d5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 12:08:56 +0100 Subject: [PATCH 393/624] DSM: Fix code style --- src/lib/rc/dsm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 7f446685d7..c068cdae0c 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -50,11 +50,11 @@ #include #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 - #include +#include #endif #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - #include +#include #endif #define DSM_FRAME_SIZE 16 /** Date: Wed, 25 Nov 2015 12:09:08 +0100 Subject: [PATCH 394/624] S.BUS: Fix code style --- src/lib/rc/sbus.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 39642f0c1e..1ba1ff298e 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -100,9 +100,9 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ - #ifdef SBUS_SERIAL_PORT +#ifdef SBUS_SERIAL_PORT ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - #endif +#endif } /* initialise the decoder */ @@ -161,7 +161,8 @@ sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) } bool -sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels) { ssize_t ret; hrt_abstime now; From f101ad6e853d54950e0e5bd660964cf1366df449 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 12:09:22 +0100 Subject: [PATCH 395/624] S.BUS header: Fix code style --- src/lib/rc/sbus.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index afe7a66e52..1404a9eabd 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -47,7 +47,8 @@ __BEGIN_DECLS __EXPORT int sbus_init(const char *device, bool singlewire); -__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); From 38724d9e1209fece818a6dda2b6132184ee6bbc1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 12:10:42 +0100 Subject: [PATCH 396/624] Sumd: Fix code style --- src/lib/rc/sumd.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index 65c669f1ed..03d7364186 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -102,7 +102,7 @@ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_ uint16_t *channels, uint16_t max_chan_count); */ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS From 46631b5c8ad0c5e5ae653c7af5ffc87cea36d85f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 16:20:18 +1100 Subject: [PATCH 397/624] mpu6000: work around apparent ICM20608 bug undocumented register 0x11 sometimes starts with value 0, which gives an offset on the Y accel axis of 2.7m/s/s. It sometimes boots with 0xc9, which gives a zero offset. Force it to 0xc9 to get consistently correct behaviour --- src/drivers/mpu6000/mpu6000.cpp | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0c902e2739..109728fecf 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -151,6 +151,15 @@ #define MPU_WHOAMI_6000 0x68 #define ICM_WHOAMI_20608 0xaf +// ICM2608 specific registers + +/* this is an undocumented register which + if set incorrectly results in getting a 2.7m/s/s offset + on the Y axis of the accelerometer +*/ +#define MPUREG_ICM_UNDOC1 0x11 +#define MPUREG_ICM_UNDOC1_VALUE 0xc9 + // Product ID Description for ICM2608 // There is none @@ -301,7 +310,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 9 +#define MPU6000_NUM_CHECKED_REGISTERS 10 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -486,7 +495,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG + MPUREG_INT_PIN_CFG, + MPUREG_ICM_UNDOC1 }; @@ -789,6 +799,10 @@ int MPU6000::reset() write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); + if (is_icm_device()) { + write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); + } + // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); @@ -1669,6 +1683,11 @@ MPU6000::check_registers(void) */ uint8_t v; + // the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented) + if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) { + _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; + } + if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* From 05840535f183b8b1f12e72869f9fe9face51df03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 14:14:41 +0100 Subject: [PATCH 398/624] Fix math tests --- src/systemcmds/tests/CMakeLists.txt | 2 +- src/systemcmds/tests/test_hrt.c | 2 +- src/systemcmds/tests/test_time.c | 3 ++- src/systemcmds/tests/tests_main.c | 8 +++++--- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 25cad8505c..e9cb903477 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -58,7 +58,6 @@ set(srcs test_rc.c test_conv.cpp test_mount.c - test_eigen.cpp ) if(${OS} STREQUAL "nuttx") @@ -73,6 +72,7 @@ px4_add_module( STACK 60000 COMPILE_FLAGS -Wframe-larger-than=6000 + -Wno-float-equal -O0 SRCS ${srcs} DEPENDS diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index 8bc0888122..fe3c58af6a 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -37,7 +37,7 @@ ****************************************************************************/ #include - +#include #include #include diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index d89f90c683..c67f11f467 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -37,7 +37,8 @@ ****************************************************************************/ #include - +#include +#include #include #include diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index e16e0adb56..7c7cbeb28f 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -50,11 +50,11 @@ #include #include - -//#include - #include +// Not using Eigen at the moment +#define TESTS_EIGEN_DISABLE + #include "tests.h" /**************************************************************************** @@ -113,7 +113,9 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif +#ifndef TESTS_EIGEN_DISABLE {"eigen", test_eigen, OPT_NOJIGTEST}, +#endif {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From bb2d3a36b7626d91fb2a72503921c99043a05fe5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 14:15:04 +0100 Subject: [PATCH 399/624] Remove old build system artifact --- src/systemcmds/tests/module.mk | 51 ---------------------------------- 1 file changed, 51 deletions(-) delete mode 100644 src/systemcmds/tests/module.mk diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk deleted file mode 100644 index 7e31442525..0000000000 --- a/src/systemcmds/tests/module.mk +++ /dev/null @@ -1,51 +0,0 @@ -# -# Assorted tests and the like -# - -MODULE_COMMAND = tests -MODULE_STACKSIZE = 60000 -MAXOPTIMIZATION = -O0 - -SRCS = test_adc.c \ - test_bson.c \ - test_float.c \ - test_gpio.c \ - test_hott_telemetry.c \ - test_hrt.c \ - test_int.c \ - test_jig_voltages.c \ - test_led.c \ - test_sensors.c \ - test_servo.c \ - test_sleep.c \ - test_uart_baudchange.c \ - test_uart_console.c \ - test_uart_loopback.c \ - test_uart_send.c \ - test_mixer.cpp \ - test_mathlib.cpp \ - test_file.c \ - test_file2.c \ - tests_main.c \ - test_param.c \ - test_ppm_loopback.c \ - test_rc.c \ - test_conv.cpp \ - test_mount.c \ - test_eigen.cpp - -ifeq ($(PX4_TARGET_OS), nuttx) -SRCS += test_time.c - -EXTRACXXFLAGS = -Wframe-larger-than=6000 -else -EXTRACXXFLAGS = -endif - -EXTRACXXFLAGS += -Wno-float-equal - -# Flag is only valid for GCC, not clang -ifneq ($(USE_GCC), 0) -EXTRACXXFLAGS += -Wno-double-promotion -Wno-error=logical-op -endif - From 5500ad16d8119b1713620cc3fdb098cbaa36ee0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 20:25:30 +0100 Subject: [PATCH 400/624] Fix RC --- src/lib/rc/CMakeLists.txt | 1 + src/lib/rc/dsm.c | 14 ++++++++------ src/lib/rc/dsm.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index dd253a3f0a..15bbd71bb9 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( st24.c sumd.c sbus.c + dsm.c DEPENDS platforms__common ) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index c068cdae0c..5d7186ec1b 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -68,6 +68,9 @@ static unsigned dsm_partial_frame_count; /**< Count of bytes received for curren static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */ +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values); + /** * Attempt to decode a single channel raw channel datum * @@ -265,7 +268,6 @@ void dsm_bind(uint16_t cmd, int pulses) { #if !defined(GPIO_USART1_RX_SPEKTRUM) -#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else if (dsm_fd < 0) { @@ -332,8 +334,8 @@ dsm_bind(uint16_t cmd, int pulses) * @param[out] num_values pointer to number of raw channel values returned * @return true=DSM frame successfully decoded, false=no update */ -static bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values) { /* debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", @@ -379,7 +381,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) { + if (channel >= max_values) { continue; } @@ -478,7 +480,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes) +dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values) { ssize_t ret; hrt_abstime now; @@ -528,5 +530,5 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by * decode it. */ dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values); + return dsm_decode(now, values, num_values, max_values); } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 9129a389d8..80614d066e 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -46,7 +46,7 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); -__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); __EXPORT void dsm_bind(uint16_t cmd, int pulses); __END_DECLS From 6bc6eda2951d52bbe3ba8d2ac3455fb81968431f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 20:28:01 +0100 Subject: [PATCH 401/624] Fixed drivers --- src/drivers/boards/px4fmu-v4/board_config.h | 2 + src/drivers/px4fmu/fmu.cpp | 51 ++++++++++++++++++++- src/modules/px4iofirmware/controls.c | 2 +- 3 files changed, 53 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 3500d35e61..9a89a7e962 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,6 +214,8 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define SBUS_SERIAL_PORT "/dev/ttyS4" /* XXX not vetted */ + /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 #define PWMIN_TIMER_CHANNEL 2 diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9e1ea99861..452cf1fc2c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -71,6 +71,11 @@ #include #include +#include +#include +#include +#include + #include #include #include @@ -799,6 +804,45 @@ PX4FMU::cycle() update_pwm_rev_mask(); } + bool rc_updated = false; + +#ifdef SBUS_SERIAL_PORT + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (sbus_updated) { + // we have a new PPM frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + for (uint8_t i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0; + _rc_in.rc_failsafe = false; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_total_frame_count = 0; + + rc_updated = true; + } +#endif + +#ifdef DSM_SERIAL_PORT + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data @@ -824,6 +868,12 @@ PX4FMU::cycle() _rc_in.rc_lost_frame_count = 0; _rc_in.rc_total_frame_count = 0; + rc_updated = true; + } + +#endif + + if (rc_updated) { /* lazily advertise on first publication */ if (_to_input_rc == nullptr) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in); @@ -833,7 +883,6 @@ PX4FMU::cycle() } } -#endif work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 25874868b1..fa8d5ad48a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -77,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; From 193cf888f587292706a0e7578011fe2bc0f9a5c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 20:28:18 +0100 Subject: [PATCH 402/624] FMUv4: Enable RC lib --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 850b1a55ea..ecfeaa36a9 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -109,6 +109,7 @@ set(config_module_list #lib/mathlib/CMSIS lib/mathlib lib/mathlib/math/filter + lib/rc lib/ecl lib/external_lgpl lib/geo From a3eeafebeb3821806b88c08e0c9957e05a53f575 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Nov 2015 21:31:00 +0100 Subject: [PATCH 403/624] Enable UART6 and enable S.BUS. Tested to work. --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + nuttx-configs/px4fmu-v4/include/board.h | 4 +++ nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- src/drivers/boards/px4fmu-v4/board_config.h | 4 +-- src/drivers/px4fmu/fmu.cpp | 25 +++++++++++-------- .../attitude_estimator_q_main.cpp | 4 +++ src/modules/sensors/sensors.cpp | 2 +- 7 files changed, 27 insertions(+), 15 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index ecfeaa36a9..858c95dd05 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -58,6 +58,7 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/tests # # General system control diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h index 54e21bbfe7..2f7c1189f3 100755 --- a/nuttx-configs/px4fmu-v4/include/board.h +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -215,6 +215,9 @@ #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + #define GPIO_UART7_RX GPIO_UART7_RX_1 #define GPIO_UART7_TX GPIO_UART7_TX_1 @@ -222,6 +225,7 @@ /* UART RX DMA configurations */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* * CAN diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 96cd833d45..cfe3de4403 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -258,7 +258,7 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_UART4=y # CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set +CONFIG_STM32_USART6=y CONFIG_STM32_UART7=y CONFIG_STM32_UART8=y # CONFIG_STM32_IWDG is not set diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 9a89a7e962..ab94498514 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,7 +214,7 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) -#define SBUS_SERIAL_PORT "/dev/ttyS4" /* XXX not vetted */ +#define SBUS_SERIAL_PORT "/dev/ttyS4" /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -225,7 +225,7 @@ __BEGIN_DECLS #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) -#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 452cf1fc2c..fbf97c9b68 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -327,12 +327,9 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif -#ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); -#endif - -#ifdef DSM_SERIAL_PORT - _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#ifdef GPIO_SBUS_INV + // this board has a GPIO to control SBUS inversion + stm32_configgpio(GPIO_SBUS_INV); #endif /* only enable this during development */ @@ -648,6 +645,15 @@ PX4FMU::cycle() update_pwm_rev_mask(); +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + + #ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); + #endif + _initialized = true; } @@ -839,14 +845,11 @@ PX4FMU::cycle() } #endif -#ifdef DSM_SERIAL_PORT - //_dsm_fd = dsm_init(DSM_SERIAL_PORT); -#endif - #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) { + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d069786699..8ca0c74e32 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -206,6 +206,8 @@ private: AttitudeEstimatorQ::AttitudeEstimatorQ() : + _vel_prev(0, 0, 0), + _pos_acc(0, 0, 0), _voter_gyro(3), _voter_accel(3), _voter_mag(3), @@ -725,6 +727,8 @@ bool AttitudeEstimatorQ::update(float dt) corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; } + _q.normalize(); + // Accelerometer correction // Project 'k' unit vector of earth frame to body frame // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a1..12f882ed99 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -846,7 +846,7 @@ Sensors::parameters_update() } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) _parameters.battery_voltage_scaling = 0.0082f; #elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; From a953f83a45afcad5211f981b3b732024eed332b9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 27 Nov 2015 15:14:24 +0100 Subject: [PATCH 404/624] XRacer: start telemetry on telem2 --- ROMFS/px4fmu_common/init.d/rcS | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 44b85c35bc..a1105ca2a4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -542,8 +542,12 @@ then # Exit from nsh to free port for mavlink set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 set MAVLINK_F "-r 1200" + # Avoid using ttyS1 for MAVLink on FMUv4 + if ver hwcmp PX4FMU_V4 + then + set MAVLINK_F "-r 1200 -d /dev/ttyS2" + fi fi fi From b196d29662f9257e442efde18467dcd95a30820f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 00:58:51 +0100 Subject: [PATCH 405/624] FMU driver: Output and read at 400 Hz --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index fbf97c9b68..bdd7f4d212 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define CONTROL_INPUT_DROP_LIMIT_MS 2 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev From d407f0dfe7271a3214e14ef808c69c894cc14c8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 09:12:57 +0100 Subject: [PATCH 406/624] Fixed FMU code style --- src/drivers/px4fmu/fmu.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bdd7f4d212..cf62ca337f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -649,10 +649,10 @@ PX4FMU::cycle() _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); #endif - #ifdef DSM_SERIAL_PORT - // XXX rather than opening it we need to cycle between protocols until one is locked in - //_dsm_fd = dsm_init(DSM_SERIAL_PORT); - #endif +#ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif _initialized = true; } @@ -843,13 +843,14 @@ PX4FMU::cycle() rc_updated = true; } + #endif #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && - ppm_decoded_channels > 3) { + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; From d07c69d3298206b10836a4e57d5155858875f13b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:16:21 +0100 Subject: [PATCH 407/624] MAVLink: Output RC inputs faster --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b04eea1a0..e4fe7ef473 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1750,7 +1750,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VFR_HUD", 20.0f); configure_stream("ATTITUDE", 100.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); - configure_stream("RC_CHANNELS", 5.0f); + configure_stream("RC_CHANNELS", 10.0f); configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); From 05d248480adade0dc78678821e7968e4fbba213a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:37:59 +0100 Subject: [PATCH 408/624] Initialize FMU actuator control feedback struct --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index cf62ca337f..42f4fbf433 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -594,7 +594,7 @@ PX4FMU::update_pwm_rev_mask() void PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; outputs.noutputs = numvalues; outputs.timestamp = hrt_absolute_time(); From 6cc1bb7ec818b857b4d4cccaaf5a2a25a63ec087 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:39:56 +0100 Subject: [PATCH 409/624] Enable RX DMA for UART6 --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index cfe3de4403..0b653b9eb5 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -306,7 +306,7 @@ CONFIG_USART3_RXDMA=y CONFIG_UART4_RXDMA=y CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set CONFIG_UART7_RXDMA=y # CONFIG_UART8_RS485 is not set From 44d32a31d195e4b862783718e7d3925c82a561d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 13:41:02 +0100 Subject: [PATCH 410/624] MPU6K: Remove unused perf counter --- src/drivers/mpu6000/mpu6000.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ab2eede349..921a0d6f97 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -272,7 +272,6 @@ private: perf_counter_t _good_transfers; perf_counter_t _reset_retries; perf_counter_t _duplicates; - perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; uint8_t _register_wait; @@ -533,7 +532,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _duplicates(perf_alloc(PC_COUNT, "mpu6000_duplicates")), - _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), _reset_wait(0), @@ -1923,7 +1921,6 @@ MPU6000::measure() if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); - perf_begin(_system_latency_perf); /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } From f4f143927f1fc79c1e672e2a53eff3321fc2f532 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 13:41:17 +0100 Subject: [PATCH 411/624] MPU9K: Remove unused perf counter --- src/drivers/mpu9250/mpu9250.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 4308149bb3..d34b8ba651 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -272,7 +272,6 @@ private: perf_counter_t _good_transfers; perf_counter_t _reset_retries; perf_counter_t _duplicates; - perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; uint8_t _register_wait; @@ -533,7 +532,6 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset_retries")), _duplicates(perf_alloc(PC_COUNT, "mpu9250_duplicates")), - _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), _reset_wait(0), @@ -1743,7 +1741,6 @@ MPU9250::measure() if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); - perf_begin(_system_latency_perf); /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } From c28f394c3cca4781be24c5c2b8d635c15d474b15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 13:42:42 +0100 Subject: [PATCH 412/624] Gyro sim: Remove unused perf counter --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 1eef3f88eb..c6e7184d58 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -171,7 +171,6 @@ private: perf_counter_t _sample_perf; perf_counter_t _good_transfers; perf_counter_t _reset_retries; - perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; enum Rotation _rotation; @@ -327,7 +326,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")), _good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), - _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _rotation(rotation), _last_temperature(0) @@ -1121,7 +1119,6 @@ GYROSIM::_measure() if (!(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); - perf_begin(_system_latency_perf); /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } From 151402748e037012947f0f233f30ca320e2f9560 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 14:55:03 +0100 Subject: [PATCH 413/624] S.BUS input: Be less sensitive on timing --- src/lib/rc/sbus.c | 5 ++--- src/lib/rc/sbus.h | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 1ba1ff298e..f5330b720d 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -174,8 +174,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails * The minimum frame spacing is 7ms; with 25 bytes at 100000bps * frame transmission time is ~2ms. * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, + * If an interval of more than 4ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -184,7 +183,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); - if ((now - last_rx_time) > 3000) { + if ((now - last_rx_time) > 4000) { if (partial_frame_count > 0) { sbus_frame_drops++; partial_frame_count = 0; diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 1404a9eabd..04b91dc126 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -47,6 +47,24 @@ __BEGIN_DECLS __EXPORT int sbus_init(const char *device, bool singlewire); + +/** + * Parse serial bytes on the S.BUS bus + * + * The S.bus protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * If an interval of more than 4ms (7 ms frame spacing plus margin) + * passes between calls, the first byte we read will be the first + * byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); From fa197ee490297b39c6ccff47233ffa515306785d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 14:55:25 +0100 Subject: [PATCH 414/624] FMU driver: Run slightly faster to accomodate S.BUS --- src/drivers/px4fmu/fmu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 42f4fbf433..a5997f861c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_MS 2 +#define CONTROL_INPUT_DROP_LIMIT_US 1500 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev @@ -887,7 +887,7 @@ PX4FMU::cycle() } } - work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US)); } void PX4FMU::work_stop() From f8ccac69bc4c6dd18c15e6edbb14501f2cd7444f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:03:06 +0100 Subject: [PATCH 415/624] FMUv1: Remove unneeded code --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index dea7678014..4e969692bc 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -145,20 +145,6 @@ static struct spi_dev_s *spi3; #include -#if 0 -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif -#endif - __EXPORT int nsh_archinitialize(void) { int result; From 57ce3cfd0107279b91bde03ac00911ba7ec43a2f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:03:19 +0100 Subject: [PATCH 416/624] FMUv2: Remove unneeded code --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 116a6ff556..687110fa40 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -212,19 +212,6 @@ static struct sdio_dev_s *sdio; #include -/* TODO XXX commented this out to get cmake build working */ -/*#ifdef __cplusplus*/ -/*__EXPORT int matherr(struct __exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#else*/ -/*__EXPORT int matherr(struct exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#endif*/ - __EXPORT int nsh_archinitialize(void) { From cb286d0ae688d9182b9e68020e8eb05f8644de99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:03:50 +0100 Subject: [PATCH 417/624] FMUv4: Initialize complete board correctly --- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 23 +++++++--------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index a1e53d41c5..7966f57165 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -211,19 +211,6 @@ static struct sdio_dev_s *sdio; #include -/* TODO XXX commented this out to get cmake build working */ -/*#ifdef __cplusplus*/ -/*__EXPORT int matherr(struct __exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#else*/ -/*__EXPORT int matherr(struct exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#endif*/ - __EXPORT int nsh_archinitialize(void) { @@ -231,9 +218,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ - // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ - // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ @@ -243,6 +228,12 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_GPIO5_OUTPUT); + stm32_configgpio(GPIO_SBUS_INV); + stm32_configgpio(GPIO_8266_GPIO0); + stm32_configgpio(GPIO_SPEKTRUM_POWER); + stm32_configgpio(GPIO_8266_PD); + stm32_configgpio(GPIO_8266_RST); + /* configure the high-resolution time/callout interface */ hrt_init(); From 94eff8d9a857c209ca81e46626561e58e5117517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:05:01 +0100 Subject: [PATCH 418/624] Commander: Fix scheduling so its not running at higher prio than control apps --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20efb4db3c..ec47196763 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -300,7 +300,7 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT + 40, 3600, commander_thread_main, (char * const *)&argv[0]); From 00e9804b5ad5dd5d5d5c7ab714aeafa7c2300494 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:05:39 +0100 Subject: [PATCH 419/624] Mixer: Fix dependencies --- src/modules/systemlib/mixer/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/CMakeLists.txt b/src/modules/systemlib/mixer/CMakeLists.txt index 20187eb97f..e9abd7a2a0 100644 --- a/src/modules/systemlib/mixer/CMakeLists.txt +++ b/src/modules/systemlib/mixer/CMakeLists.txt @@ -38,7 +38,7 @@ add_custom_command(OUTPUT mixer_multirotor.generated.h > mixer_multirotor.generated.h) add_custom_target(mixer_gen - DEPENDS mixer_multirotor.generated.h) + DEPENDS mixer_multirotor.generated.h multi_tables.py) px4_add_module( MODULE modules__systemlib__mixer @@ -51,5 +51,6 @@ px4_add_module( mixer_multirotor.generated.h DEPENDS platforms__common + mixer_gen ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From a4aa844151b07c993d814ab9024ffabe40a7a52f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:06:54 +0100 Subject: [PATCH 420/624] FMU driver: Slightly increase run interval to save load --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a5997f861c..28abf37ff2 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_US 1500 +#define CONTROL_INPUT_DROP_LIMIT_US 1800 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev From 22c7f72a1c48b72be4bb32eec65a897348466b83 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:12:26 +0100 Subject: [PATCH 421/624] FMUv4 config: Fix periph 3.3V en signal, fix typo on 8266 RST signal --- src/drivers/boards/px4fmu-v4/board_config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index ab94498514..a29c961f5a 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -224,13 +224,13 @@ __BEGIN_DECLS #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) -#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) /**************************************************************************************************** * Public Types From f99e14144e1abae33650817fbc84c702520a56bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:38:08 +0100 Subject: [PATCH 422/624] Q estimator: Increase stack size as needed --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 8ca0c74e32..8c0d601bdf 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -264,7 +264,7 @@ int AttitudeEstimatorQ::start() _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2100, + 2400, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); From 3515e6ae918d1e29a656ead9e99ba79f9704957d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:38:26 +0100 Subject: [PATCH 423/624] INAV: Increase stack size as needed --- .../position_estimator_inav/position_estimator_inav_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 72b250ebe9..cc2c73f05b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -154,7 +154,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300, position_estimator_inav_thread_main, (argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL); return 0; From 66e9abc7741c567766a3ffd3719f3709fd699b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:38:48 +0100 Subject: [PATCH 424/624] SDLOG2: increase stack size as needed --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f15ec8969a..afc96bacc0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -311,7 +311,7 @@ int sdlog2_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, - 3000, + 3300, sdlog2_thread_main, (char * const *)argv); From ce1b35d024e71b63398bb73c6ebc970a572db59a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 18:41:18 +0100 Subject: [PATCH 425/624] Q estimator: Fix scope of sensor voting scheme --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 8c0d601bdf..d48b92f0ab 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -333,6 +333,10 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; + int best_gyro = 0; + int best_accel = 0; + int best_mag = 0; + if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data @@ -369,8 +373,6 @@ void AttitudeEstimatorQ::task_main() } } - int best_gyro, best_accel, best_mag; - // Get best measurement values hrt_abstime curr_time = hrt_absolute_time(); _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); From 3c26ca99a07eb6957d894295133dc9cb0f4999a5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 18:06:39 +0100 Subject: [PATCH 426/624] Q estimator: Increase phase margin --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 64994a7956..a8d506611f 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -205,9 +205,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 18.0f), - _lp_pitch_rate(250.0f, 18.0f), - _lp_yaw_rate(250.0f, 10.0f) + _lp_roll_rate(250.0f, 30.0f), + _lp_pitch_rate(250.0f, 30.0f), + _lp_yaw_rate(250.0f, 20.0f) { _voter_mag.set_timeout(200000); From 65002d279f4f672a19885bbad08db23e00f245f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 18:49:58 +0100 Subject: [PATCH 427/624] Commander: Allow setting home position faster --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b9e6817c14..270b7f4416 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,7 +153,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 -#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000 +#define INAIR_RESTART_HOLDOFF_INTERVAL 500000 #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 @@ -573,7 +573,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { @@ -2393,7 +2393,7 @@ int commander_thread_main(int argc, char *argv[]) commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - /* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { commander_set_home_position(home_pub, _home, local_position, global_position, attitude); From a9fb11829fdfdaeb45f4fe52d9ee42b0de6f7710 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:05:01 +0100 Subject: [PATCH 428/624] Commander: Fix scheduling so its not running at higher prio than control apps --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 270b7f4416..a19486acec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -300,7 +300,7 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT + 40, 3600, commander_thread_main, (char * const *)&argv[0]); From 989b65912bcf7bde040d12aea6c7e1e0058f7f73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Nov 2015 00:40:41 +0100 Subject: [PATCH 429/624] jMAVSim: Drop sim rate to 400 Hz --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index a313e80266..4267771753 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit a313e802663332e405dee24e732a82c79884dc3a +Subproject commit 4267771753c4bb5d0696a678856620229523162e From 9ee4760fe0a9bf5e64661d8e30f5406d54d5b032 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Nov 2015 00:41:46 +0100 Subject: [PATCH 430/624] Barosim: Simplify to a plain loop without reschedules or sub interfaces. Behaves now. The DriverFramework needs closer inspection for busy-running threads. --- .../posix/drivers/barosim/CMakeLists.txt | 1 - src/platforms/posix/drivers/barosim/baro.cpp | 88 +++++++--- .../posix/drivers/barosim/baro_sim.cpp | 157 ------------------ src/platforms/posix/drivers/barosim/barosim.h | 35 ---- src/platforms/posix/drivers/gpssim/gpssim.cpp | 2 +- 5 files changed, 69 insertions(+), 214 deletions(-) delete mode 100644 src/platforms/posix/drivers/barosim/baro_sim.cpp diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt index 15963f87eb..6e3a105204 100644 --- a/src/platforms/posix/drivers/barosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( -Os SRCS baro.cpp - baro_sim.cpp DEPENDS platforms__common ) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index ac5d31b6b9..be38915595 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include @@ -62,6 +61,8 @@ #include #include +#include + #include #include @@ -83,6 +84,8 @@ #define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ #define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +using namespace DriverFramework; + class BAROSIM : public VirtDevObj { public: @@ -100,9 +103,6 @@ public: void print_info(); protected: - BAROSIM_DEV *_interface; - - struct work_s _work; ringbuffer::RingBuffer *_reports; @@ -155,6 +155,8 @@ protected: */ void cycle(); + int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + /** * Get the internal / external state * @@ -195,7 +197,6 @@ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); BAROSIM::BAROSIM(const char *path) : VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100), - _interface(nullptr), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -212,10 +213,7 @@ BAROSIM::BAROSIM(const char *path) : _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "barosim_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - _interface = new BAROSIM_DEV; } BAROSIM::~BAROSIM() @@ -234,7 +232,6 @@ BAROSIM::~BAROSIM() perf_free(_comms_errors); perf_free(_buffer_overflows); - delete _interface; } int @@ -392,6 +389,8 @@ BAROSIM::devRead(void *buffer, size_t buflen) int BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) { + PX4_WARN("baro IOCTL %llu", hrt_absolute_time()); + switch (cmd) { case SENSORIOCSPOLLRATE: @@ -510,6 +509,7 @@ BAROSIM::start_cycle() void BAROSIM::stop_cycle() { + stop(); setSampleInterval(0); } @@ -523,7 +523,8 @@ void BAROSIM::cycle() { int ret; - unsigned long dummy = 0; + + //PX4_WARN("baro cycle %llu", hrt_absolute_time()); /* collection phase? */ if (_collect_phase) { @@ -532,11 +533,14 @@ BAROSIM::cycle() ret = collect(); if (ret != OK) { - /* issue a reset command to the sensor */ - _interface->devIOCTL(IOCTL_RESET, dummy); + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + result = transfer(&cmd, 1, nullptr, 0); /* reset the collection state machine and try again */ - start_cycle(); + //start_cycle(); return; } @@ -551,7 +555,7 @@ BAROSIM::cycle() */ if (_measure_phase != 0) { - setSampleInterval(BAROSIM_CONVERSION_INTERVAL); + //setSampleInterval(BAROSIM_CONVERSION_INTERVAL); return; } @@ -563,17 +567,17 @@ BAROSIM::cycle() if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ - _interface->devIOCTL(IOCTL_RESET, dummy); + //_interface->devIOCTL(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ - start_cycle(); + //start_cycle(); return; } /* next phase is collection */ _collect_phase = true; - setSampleInterval(BAROSIM_CONVERSION_INTERVAL); + //setSampleInterval(BAROSIM_CONVERSION_INTERVAL); } int @@ -581,6 +585,8 @@ BAROSIM::measure() { int ret; + //PX4_WARN("baro measure %llu", hrt_absolute_time()); + perf_begin(_measure_perf); /* @@ -591,7 +597,8 @@ BAROSIM::measure() /* * Send the command to begin measuring. */ - ret = _interface->devIOCTL(IOCTL_MEASURE, addr); + uint8_t cmd = addr; + ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -602,9 +609,48 @@ BAROSIM::measure() return ret; } +int +BAROSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (send_len == 1 && send[0] == ADDR_RESET_CMD) { + /* reset command */ + return 0; + + } else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) { + /* measure command */ + if (send[0] == ADDR_CMD_CONVERT_D2) { + } else { + } + + return 0; + + } else if (send[0] == 0 && send_len == 1) { + /* read requested */ + Simulator *sim = Simulator::getInstance(); + + if (sim == NULL) { + PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); + return -ENODEV; + } + + PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); + sim->getBaroSample(recv, recv_len); + return recv_len; + + } else { + PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len); + return 1; + } + + + return 0; +} + int BAROSIM::collect() { + //PX4_WARN("baro collect %llu", hrt_absolute_time()); + int ret; #pragma pack(push, 1) @@ -623,7 +669,8 @@ BAROSIM::collect() report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->devRead((void *)&baro_report, sizeof(baro_report)); + uint8_t cmd = 0; + ret = transfer(&cmd, 1, (uint8_t*)&report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); @@ -660,7 +707,8 @@ BAROSIM::collect() } /* notify anyone waiting for data */ - DevMgr::updateNotify(*this); + //DevMgr::updateNotify(*this); + updateNotify(); } /* update the measurement state machine */ diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp deleted file mode 100644 index 224025bc71..0000000000 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ - -/* XXX trim includes */ -#include -#include - -#include -#include -#include - -#include -#include -#include "barosim.h" -#include "board_config.h" - -BAROSIM_DEV::BAROSIM_DEV() : - VirtDevObj("BAROSIM_DEV", "/dev/BAROSIM_DEV", BARO_BASE_DEVICE_PATH, 0) -{ -} - -BAROSIM_DEV::~BAROSIM_DEV() -{ -} - -int -BAROSIM_DEV::init() -{ - return VirtDevObj::init(); -} - -ssize_t -BAROSIM_DEV::devRead(void *data, size_t count) -{ - /* read the most recent measurement */ - uint8_t cmd = 0; - int ret = transfer(&cmd, 1, static_cast(data), count); - - return ret; -} - -int -BAROSIM_DEV::devIOCTL(unsigned long operation, unsigned long arg) -{ - int ret; - - switch (operation) { - case IOCTL_RESET: - ret = _reset(); - break; - - case IOCTL_MEASURE: - ret = _doMeasurement(arg); - break; - - default: - ret = EINVAL; - } - - return ret; -} - -int -BAROSIM_DEV::_reset() -{ - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - result = transfer(&cmd, 1, nullptr, 0); - - return result; -} - -int -BAROSIM_DEV::_doMeasurement(unsigned addr) -{ - uint8_t cmd = addr; - return transfer(&cmd, 1, nullptr, 0); -} - -int -BAROSIM_DEV::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) -{ - if (send_len == 1 && send[0] == ADDR_RESET_CMD) { - /* reset command */ - return 0; - - } else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) { - /* measure command */ - if (send[0] == ADDR_CMD_CONVERT_D2) { - } else { - } - - return 0; - - } else if (send[0] == 0 && send_len == 1) { - /* read requested */ - Simulator *sim = Simulator::getInstance(); - - if (sim == NULL) { - PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); - return -ENODEV; - } - - PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); - sim->getBaroSample(recv, recv_len); - return recv_len; - - } else { - PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len); - return 1; - } - - - return 0; -} - -void BAROSIM_DEV::_measure() -{ -} diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index 92d959753f..b029dd9d09 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -79,38 +79,3 @@ union prom_u { extern bool crc4(uint16_t *n_prom); } /* namespace */ - -using namespace DriverFramework; - -class BAROSIM_DEV : public VirtDevObj -{ -public: - BAROSIM_DEV(); - virtual ~BAROSIM_DEV(); - - virtual int init(); - virtual ssize_t devRead(void *data, size_t count); - virtual int devIOCTL(unsigned long operation, unsigned long arg); - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); - -protected: - virtual void _measure(); - -private: - /** - * Send a reset command to the barometer simulator. - * - * This is required after any bus reset. - */ - int _reset(); - - /** - * Send a measure command to the barometer simulator. - * - * @param addr Which address to use for the measure operation. - */ - int _doMeasurement(unsigned addr); - -}; diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 3241c6c30d..afc9d94fb4 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -169,7 +169,7 @@ GPSSIM *g_dev = nullptr; GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : - VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 0), + VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10), _task_should_exit(false), //_healthy(false), //_mode_changed(false), From 1fc774bbf881c49138500073c28197dedb036b08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 18:06:39 +0100 Subject: [PATCH 431/624] Q estimator: Increase phase margin --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d48b92f0ab..09ccfc5afa 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -211,9 +211,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 18.0f), - _lp_pitch_rate(250.0f, 18.0f), - _lp_yaw_rate(250.0f, 10.0f) + _lp_roll_rate(250.0f, 30.0f), + _lp_pitch_rate(250.0f, 30.0f), + _lp_yaw_rate(250.0f, 20.0f) { _voter_mag.set_timeout(200000); From 3ce6ee57e41d28fc5bb47dbd97c65a53c56e9117 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 30 Nov 2015 15:44:36 -1000 Subject: [PATCH 432/624] Set nunmber of UAVCAN interfaces on HW from top level cmake file --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 ++ cmake/configs/nuttx_px4fmu-v4_default.cmake | 2 ++ src/modules/uavcan/CMakeLists.txt | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index c3a3586f66..62f96b12d4 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 2) + set(config_module_list # # Board support modules diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 858c95dd05..720b9e74b8 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 1) + set(config_module_list # # Board support modules diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index ce348ec910..18ddc0f261 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -41,7 +41,7 @@ add_definitions( -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 - -DUAVCAN_STM32_NUM_IFACES=2 + -DUAVCAN_STM32_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_STM32_TIMER_NUMBER=5 ) From 0c2b6bf9d11f1c733cd647fb51ec79bcb78bb00f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 10:27:39 +0100 Subject: [PATCH 433/624] POSIX: Set the thread name for better debugging --- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 729185691b..0422692024 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -75,6 +75,7 @@ static task_entry taskmap[PX4_MAX_TASKS] = {}; typedef struct { px4_main_t entry; + const char* name; int argc; char *argv[]; // strings are allocated after the @@ -83,6 +84,19 @@ typedef struct { static void *entry_adapter(void *ptr) { pthdata_t *data = (pthdata_t *) ptr; + + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np(data->name); + #else + rv = pthread_setname_np(pthread_self(), data->name); + #endif + + if (rv) { + PX4_ERR("px4_task_spawn_cmd: failed to set name of thread %d %d\n", rv, errno); + } + data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); @@ -132,6 +146,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int memset(taskdata, 0, structsize + len); offset = ((unsigned long)taskdata) + structsize; + taskdata->name = name; taskdata->entry = entry; taskdata->argc = argc; From cf17ef4d41aea95a56c8aa4941190aaecfff2276 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 10:28:05 +0100 Subject: [PATCH 434/624] PX4 semaphores: Add excellent debugging output on failure for Mac OS --- src/platforms/posix/px4_layer/px4_sem.cpp | 87 ++++++++++++++++++----- 1 file changed, 71 insertions(+), 16 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 84795eb16a..9abd25bc59 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #ifdef __PX4_DARWIN @@ -61,53 +62,107 @@ int px4_sem_init(px4_sem_t *s, int pshared, unsigned value) int px4_sem_wait(px4_sem_t *s) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value--; if (s->value < 0) { - pthread_cond_wait(&(s->wait), &(s->lock)); + ret = pthread_cond_wait(&(s->wait), &(s->lock)); + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + if (ret) { + PX4_WARN("px4_sem_wait failure"); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + return (ret) ? ret : mret; } int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value--; if (s->value < 0) { - pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + ret = pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + int err = errno; + #ifdef __PX4_DARWIN + err = ret; + #endif - return 0; + if (err != 0 && err != ETIMEDOUT) { + setbuf(stdout, NULL); + setbuf(stderr, NULL); + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + (void)pthread_getname_np(pthread_self(), thread_name, NAMELEN); + PX4_WARN("%s: px4_sem_timedwait failure: ret: %d, %s", thread_name, ret, strerror(err)); + } + + int mret = pthread_mutex_unlock(&(s->lock)); + + return (ret) ? ret : mret; } int px4_sem_post(px4_sem_t *s) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value++; if (s->value <= 0) { - pthread_cond_signal(&(s->wait)); + ret = pthread_cond_signal(&(s->wait)); + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + if (ret) { + PX4_WARN("px4_sem_post failure"); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + // return the cond signal failure if present, + // else return the mutex status + return (ret) ? ret : mret; } int px4_sem_getvalue(px4_sem_t *s, int *sval) { - pthread_mutex_lock(&(s->lock)); - *sval = s->value; - pthread_mutex_unlock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); - return 0; + if (ret) { + PX4_WARN("px4_sem_getvalue failure"); + } + + if (ret) { + return ret; + } + + *sval = s->value; + ret = pthread_mutex_unlock(&(s->lock)); + + return ret; } int px4_sem_destroy(px4_sem_t *s) From 4b319d2fc6044cfc245f8431a81bc8f19399e148 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 10:28:24 +0100 Subject: [PATCH 435/624] PWM out sim: Fix corner cases in startup and timeout handling --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 5d81944245..d7d4eeeeb9 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -187,6 +187,7 @@ PWMSim::PWMSim() : _update_rate(50), _current_update_rate(0), _control_subs { -1}, + _poll_fds{}, _poll_fds_num(0), _armed_sub(-1), _outputs_pub(0), @@ -366,7 +367,6 @@ PWMSim::subscribe() } if (_control_subs[i] > 0) { - printf("valid\n"); _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num++; @@ -417,7 +417,16 @@ PWMSim::task_main() } /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + int ret = 0; + if (_poll_fds_num == 0) { + usleep(1000 * 1000); + /* this can happen during boot, but after the sleep its likely resolved */ + if (_poll_fds_num == 0) { + PX4_WARN("pwm_out_sim: No valid fds"); + } + } else { + ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + } /* this would be bad... */ if (ret < 0) { From 5126f81aed2cf9aa17278fb55a6fa9e277910287 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 10:28:49 +0100 Subject: [PATCH 436/624] Fix corner cases in timing and in handling poll list iteration in VDEV posix --- src/drivers/device/vdev_posix.cpp | 54 +++++++++++++++++++++++++++---- 1 file changed, 47 insertions(+), 7 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 4d0500c486..406ecf70fd 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -233,6 +233,11 @@ extern "C" { int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { + if (nfds == 0) { + PX4_WARN("px4_poll with no fds"); + return -1; + } + px4_sem_t sem; int count = 0; int ret = -1; @@ -241,7 +246,8 @@ extern "C" { PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); - // For each fd + // Go through all fds and check them for a pollable state + bool fd_pollable = false; for (i = 0; i < nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; @@ -255,26 +261,52 @@ extern "C" { ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) { + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + (void)pthread_getname_np(pthread_self(), thread_name, NAMELEN); + PX4_WARN("%s: px4_poll() error: %s", + thread_name, strerror(errno)); break; } + + if (ret >= 0) { + fd_pollable = true; + } } } - if (ret >= 0) { + // If any FD can be polled, lock the semaphore and + // check for new data + if (fd_pollable) { if (timeout > 0) { + // Get the current time struct timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_sec += timeout / 1000; - ts.tv_nsec += (1000 * 1000 * timeout) % (1000 * 1000 * 1000); - px4_sem_timedwait(&sem, &ts); + // Calculate an absolute time in the future + const unsigned billion = (1000 * 1000 * 1000); + unsigned tdiff = timeout; + uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000); + ts.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + ts.tv_nsec = nsecs; + + // Execute a blocking wait for that time in the future + errno = 0; + ret = px4_sem_timedwait(&sem, &ts); + + // Ensure ret is negative on failure + if (ret > 0) { + ret = -ret; + } } else if (timeout < 0) { px4_sem_wait(&sem); } - // For each fd + // We have waited now (or not, depending on timeout), + // go through all fds and count how many have data for (i = 0; i < nfds; ++i) { VDev *dev = get_vdev(fds[i].fd); @@ -285,6 +317,7 @@ extern "C" { ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) { + PX4_WARN("px4_poll() 2nd poll fail"); break; } @@ -293,11 +326,18 @@ extern "C" { } } } + + // If nothing is ready + if (count == 0) { + usleep(timeout * 1000); + } } px4_sem_destroy(&sem); - return count; + // Return the positive count if present, + // return the negative error number if failed + return (count) ? count : ret; } int px4_fsync(int fd) From aa36e54c933642007be7f310abce821176aadf63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 11:53:00 +0100 Subject: [PATCH 437/624] VDEV: More active reporting --- src/drivers/device/vdev_posix.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 406ecf70fd..921e055693 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -243,6 +243,14 @@ extern "C" { int ret = -1; unsigned int i; + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); + + if (nret || thread_name[0] == 0) { + PX4_WARN("failed getting thread name"); + } + PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); @@ -257,13 +265,10 @@ extern "C" { // If fd is valid if (dev) { - PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); + PX4_DEBUG("%s: px4_poll: VDev->poll(setup) %d", thread_name, fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) { - const unsigned NAMELEN = 32; - char thread_name[NAMELEN] = {}; - (void)pthread_getname_np(pthread_self(), thread_name, NAMELEN); PX4_WARN("%s: px4_poll() error: %s", thread_name, strerror(errno)); break; @@ -301,6 +306,10 @@ extern "C" { ret = -ret; } + if (ret && ret != -ETIMEDOUT) { + PX4_WARN("%s: px4_poll() sem error", thread_name); + } + } else if (timeout < 0) { px4_sem_wait(&sem); } @@ -313,11 +322,11 @@ extern "C" { // If fd is valid if (dev) { - PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); + PX4_DEBUG("%s: px4_poll: VDev->poll(teardown) %d", thread_name, fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) { - PX4_WARN("px4_poll() 2nd poll fail"); + PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name); break; } @@ -326,11 +335,6 @@ extern "C" { } } } - - // If nothing is ready - if (count == 0) { - usleep(timeout * 1000); - } } px4_sem_destroy(&sem); From 1feb30fd399c87c5b26978b63a0838e8854a1a62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 11:53:59 +0100 Subject: [PATCH 438/624] commander: Set process name on all OS --- src/modules/commander/commander.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a19486acec..c6b71a762a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3114,10 +3114,8 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result) void *commander_low_prio_loop(void *arg) { -#if defined(__PX4_LINUX) || defined(__PX4_NUTTX) /* Set thread name */ - prctl(PR_SET_NAME, "commander_low_prio", getpid()); -#endif + px4_prctl(PR_SET_NAME, "commander_low_prio", getpid()); /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); From 05d5292641aa745e55c5f5aeb2ec5c5010fbdc36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 11:56:49 +0100 Subject: [PATCH 439/624] MAVLink receiver: Set thread name --- src/modules/mavlink/mavlink_receiver.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 37d070718d..016ea8142a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1750,10 +1750,15 @@ void * MavlinkReceiver::receive_thread(void *arg) { + /* set thread name */ + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); + px4_prctl(PR_SET_NAME, thread_name, getpid()); + const int timeout = 500; #ifdef __PX4_POSIX /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ - uint8_t buf[1600]; + uint8_t buf[1600 * 5]; #else /* the serial port buffers internally as well, we just need to fit a small chunk */ uint8_t buf[64]; @@ -1766,12 +1771,6 @@ MavlinkReceiver::receive_thread(void *arg) if (_mavlink->get_protocol() == SERIAL) { uart_fd = _mavlink->get_uart_fd(); -#ifndef __PX4_POSIX - /* set thread name */ - char thread_name[24]; - sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); - prctl(PR_SET_NAME, thread_name, getpid()); -#endif fds[0].fd = uart_fd; fds[0].events = POLLIN; @@ -1873,6 +1872,7 @@ void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) void *MavlinkReceiver::start_helper(void *context) { + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); rcv->receive_thread(NULL); From eb64a6813f887ad1660e3bb8c4c244fd435d31ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:01:10 +0100 Subject: [PATCH 440/624] Sim MAVLink: Return correct type, remove very uninformed comment --- src/modules/simulator/simulator_mavlink.cpp | 22 +++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2d625a21ac..57b7d167f8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -348,15 +348,23 @@ void Simulator::poll_topics() void *Simulator::sending_trampoline(void *) { _instance->send(); - return 0; // why do I have to put this??? + return nullptr; } void Simulator::send() { - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np("sim_send"); + #else + rv = pthread_setname_np(pthread_self(), "sim_send"); + #endif + int pret; while (true) { @@ -371,8 +379,6 @@ void Simulator::send() // this is undesirable but not much we can do if (pret < 0) { PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); continue; } @@ -516,6 +522,14 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np("sim_rcv"); + #else + rv = pthread_setname_np(pthread_self(), "sim_rcv"); + #endif + // wait for new mavlink messages to arrive while (true) { From 58d5fdc8531a62b19f45cc2474ab5a775713f6c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:01:31 +0100 Subject: [PATCH 441/624] Simulator: All app names are lowercase! --- src/modules/simulator/simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 44b3bcf0e1..b732945f7b 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -180,7 +180,7 @@ extern "C" { return 0; } - g_sim_task = px4_task_spawn_cmd("Simulator", + g_sim_task = px4_task_spawn_cmd("simulator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, From d5541ee2a74dc3104ab7eacf0bc1d5d858a2f392 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:01:52 +0100 Subject: [PATCH 442/624] sdlog2 writer: Use new prctl syntax --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f15ec8969a..e4d6341e22 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -555,7 +555,7 @@ int open_perf_file(const char* str) static void *logwriter_thread(void *arg) { /* set name */ - prctl(PR_SET_NAME, "sdlog2_writer", 0); + px4_prctl(PR_SET_NAME, "sdlog2_writer", 0); int log_fd = open_log_file(); From 72287aab3fdc959b6140e910056b915350d50f3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:02:43 +0100 Subject: [PATCH 443/624] Main app: set thread name --- src/platforms/posix/px4_layer/px4_posix_impl.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 19747556a9..a3ab78df55 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -49,6 +49,7 @@ #include "hrt_work.h" #include #include "px4_time.h" +#include extern pthread_t _shell_task_id; @@ -86,6 +87,13 @@ void init(int argc, char *argv[], const char *app_name) printf("Ready to fly.\n"); printf("\n"); printf("\n"); + + // set the threads name + #ifdef __PX4_DARWIN + (void)pthread_setname_np(app_name); + #else + (void)pthread_setname_np(pthread_self(), app_name); + #endif } uint64_t get_time_micros() From 85079125f4a6a8349881698f0308bbbc0031c779 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:03:21 +0100 Subject: [PATCH 444/624] posix tasks: Add px4_prctl() call --- .../posix/px4_layer/px4_posix_tasks.cpp | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 0422692024..623e0f123a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -116,6 +116,7 @@ px4_systemreset(bool to_bootloader) px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char *const argv[]) { + int rv; int argc = 0; int i; @@ -365,4 +366,27 @@ const char *getprogname() return "Unknown App"; } + +int px4_prctl(int option, const char* arg2, unsigned pid) +{ + int rv; + + switch (option) { + case PR_SET_NAME: + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np(arg2); + #else + rv = pthread_setname_np(pthread_self(), arg2); + #endif + break; + default: + rv = -1; + PX4_WARN("FAILED SETTING TASK NAME"); + break; + } + + return rv; +} + __END_DECLS From 79a030ec5033443aecc9f4b097341de7fbdff518 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:04:34 +0100 Subject: [PATCH 445/624] Work threads: Ensure they have proper names --- src/platforms/posix/work_queue/work_thread.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c index ec565bcfcc..406e350341 100644 --- a/src/platforms/posix/work_queue/work_thread.c +++ b/src/platforms/posix/work_queue/work_thread.c @@ -195,7 +195,7 @@ void work_queues_init(void) #endif // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + g_work[HPWORK].pid = px4_task_spawn_cmd("hpwork", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 1, 2000, @@ -203,7 +203,7 @@ void work_queues_init(void) (char *const *)NULL); // Create low priority worker thread - g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + g_work[LPWORK].pid = px4_task_spawn_cmd("lpwork", SCHED_DEFAULT, SCHED_PRIORITY_MIN, 2000, @@ -306,6 +306,14 @@ int work_usrthread(int argc, char *argv[]) { /* Loop forever */ + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np("USR"); + #else + rv = pthread_setname_np(pthread_self(), "USR"); + #endif + for (;;) { /* Then process queued work. We need to keep interrupts disabled while * we process items in the work list. From b170467a1dc084cf8ead4f8e53fdfb182552b027 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:04:53 +0100 Subject: [PATCH 446/624] HRT thread: Set thread name --- src/platforms/posix/work_queue/hrt_thread.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 5711aa7d6d..c7f5e7c22b 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -119,6 +119,14 @@ static void hrt_work_process() uint32_t remaining; uint32_t next; + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np("HRT"); + #else + rv = pthread_setname_np(pthread_self(), "HRT"); + #endif + /* Then process queued work. We need to keep interrupts disabled while * we process items in the work list. */ From ffdf5076dde6b6388a248ad3fba6c6a4739e089e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:05:15 +0100 Subject: [PATCH 447/624] PX4 tasks: Add px4_prctl API --- src/platforms/px4_tasks.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 04585aaced..bf8c5eeeff 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -80,10 +80,13 @@ typedef int px4_task_t; #error "No target OS defined" #endif -#if defined (__PX4_LINUX) || defined(__PX4_NUTTX) +#if defined (__PX4_NUTTX) +#include +#define px4_prctl prctl +#elif defined (__PX4_LINUX) #include #else -#define prctl(_action, _string, _pid) +#define PR_SET_NAME 1 #endif typedef int px4_task_t; @@ -126,5 +129,10 @@ __EXPORT void px4_show_tasks(void); /** See if a task is running **/ __EXPORT bool px4_task_is_running(const char *taskname); +#ifdef __PX4_POSIX +/** set process (and thread) options */ +__EXPORT int px4_prctl(int option, const char* arg2, unsigned pid); +#endif + __END_DECLS From 4beef623373af449925440852b141ac5da4cdbc1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:18:40 +0100 Subject: [PATCH 448/624] Update sitl_gazebo repo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 1fb483cc15..b6ea4c7d2a 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 1fb483cc15ec7d71e08c4058e0a79534751b4933 +Subproject commit b6ea4c7d2a1f73b92746185afb1c9e7c3a4b0219 From da190f31fe18da1eb39233a822c1affe0e5eb37d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:30:13 +0100 Subject: [PATCH 449/624] SITL: Use stable version for gazebo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index b6ea4c7d2a..03a749a3b3 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit b6ea4c7d2a1f73b92746185afb1c9e7c3a4b0219 +Subproject commit 03a749a3b3238c014329e9e8da57d102ede6f4f0 From 1e99b04d9cce0206cec47f2cad3640faf3963a74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:32:22 +0100 Subject: [PATCH 450/624] Fixes in simulator interface --- src/modules/simulator/simulator_mavlink.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 57b7d167f8..31292a8b6b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -500,7 +500,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed.."); while (pret <= 0) { - pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100); + pret = ::poll(&fds[0], fd_count, 100); } PX4_INFO("Found initial message, pret = %d", pret); @@ -537,6 +537,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) //timed out if (pret == 0) { + PX4_WARN("mavlink sim timeout for 100 ms"); continue; } @@ -556,7 +557,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) mavlink_message_t msg; mavlink_status_t status; - for (int i = 0; i < len; ++i) { + for (int i = 0; i < len; i++) { if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it handle_message(&msg, publish); @@ -566,7 +567,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) } // got data from PIXHAWK - if (fds[1].revents & POLLIN) { + if (fd_count > 1 && fds[1].revents & POLLIN) { len = ::read(serial_fd, serial_buf, sizeof(serial_buf)); if (len > 0) { From 0cc74205a041dbf0ff385f9b8988ad00f1eedbab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:42:57 +0100 Subject: [PATCH 451/624] jMAVSim: Set stiffer gains --- posix-configs/SITL/init/rcS_jmavsim_iris | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 1661d61c4f..6d8b575ac8 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -3,8 +3,10 @@ simulator start -s param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.0 +param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 From f035f11e30f6a2a8687d6ade3090c95b38358da5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:43:12 +0100 Subject: [PATCH 452/624] vdev posix: Code style --- src/drivers/device/vdev_posix.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 921e055693..3e140e907c 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -256,6 +256,7 @@ extern "C" { // Go through all fds and check them for a pollable state bool fd_pollable = false; + for (i = 0; i < nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; @@ -270,7 +271,7 @@ extern "C" { if (ret < 0) { PX4_WARN("%s: px4_poll() error: %s", - thread_name, strerror(errno)); + thread_name, strerror(errno)); break; } From b80e0a658bc276fc5136f57109974be9437af8ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:43:26 +0100 Subject: [PATCH 453/624] pwm out: Code style --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index d7d4eeeeb9..b67ba077df 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -418,12 +418,15 @@ PWMSim::task_main() /* sleep waiting for data, but no more than a second */ int ret = 0; + if (_poll_fds_num == 0) { usleep(1000 * 1000); + /* this can happen during boot, but after the sleep its likely resolved */ if (_poll_fds_num == 0) { PX4_WARN("pwm_out_sim: No valid fds"); } + } else { ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); } From 247d7960b9817d21dfb53a2936ac3676bad91f0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:43:38 +0100 Subject: [PATCH 454/624] MAVLink sim: Code style --- src/modules/simulator/simulator_mavlink.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 31292a8b6b..4ea56156ac 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -359,11 +359,11 @@ void Simulator::send() int rv; // set the threads name - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN rv = pthread_setname_np("sim_send"); - #else +#else rv = pthread_setname_np(pthread_self(), "sim_send"); - #endif +#endif int pret; @@ -524,11 +524,11 @@ void Simulator::pollForMAVLinkMessages(bool publish) int rv; // set the threads name - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN rv = pthread_setname_np("sim_rcv"); - #else +#else rv = pthread_setname_np(pthread_self(), "sim_rcv"); - #endif +#endif // wait for new mavlink messages to arrive while (true) { From 9fb2a408bfff1a84a84dd3c74c85678571c0ad1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:43:49 +0100 Subject: [PATCH 455/624] baro sim: Code style --- src/platforms/posix/drivers/barosim/baro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index be38915595..960e14c1a3 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -670,7 +670,7 @@ BAROSIM::collect() /* read the most recent measurement - read offset/size are hardcoded in the interface */ uint8_t cmd = 0; - ret = transfer(&cmd, 1, (uint8_t*)&report, sizeof(baro_report)); + ret = transfer(&cmd, 1, (uint8_t *)&report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); From 069ebebb2784101e1baae420dba1de9a3fe91147 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:44:06 +0100 Subject: [PATCH 456/624] PX4 layer: Code style fixes --- .../posix/px4_layer/px4_posix_impl.cpp | 6 ++-- .../posix/px4_layer/px4_posix_tasks.cpp | 35 ++++++++++--------- src/platforms/posix/px4_layer/px4_sem.cpp | 7 ++-- 3 files changed, 26 insertions(+), 22 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index a3ab78df55..30f4cf0f0c 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -89,11 +89,11 @@ void init(int argc, char *argv[], const char *app_name) printf("\n"); // set the threads name - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN (void)pthread_setname_np(app_name); - #else +#else (void)pthread_setname_np(pthread_self(), app_name); - #endif +#endif } uint64_t get_time_micros() diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 623e0f123a..e05cc5432b 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -75,7 +75,7 @@ static task_entry taskmap[PX4_MAX_TASKS] = {}; typedef struct { px4_main_t entry; - const char* name; + const char *name; int argc; char *argv[]; // strings are allocated after the @@ -87,11 +87,11 @@ static void *entry_adapter(void *ptr) int rv; // set the threads name - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN rv = pthread_setname_np(data->name); - #else +#else rv = pthread_setname_np(pthread_self(), data->name); - #endif +#endif if (rv) { PX4_ERR("px4_task_spawn_cmd: failed to set name of thread %d %d\n", rv, errno); @@ -367,23 +367,24 @@ const char *getprogname() return "Unknown App"; } -int px4_prctl(int option, const char* arg2, unsigned pid) +int px4_prctl(int option, const char *arg2, unsigned pid) { int rv; switch (option) { - case PR_SET_NAME: - // set the threads name - #ifdef __PX4_DARWIN - rv = pthread_setname_np(arg2); - #else - rv = pthread_setname_np(pthread_self(), arg2); - #endif - break; - default: - rv = -1; - PX4_WARN("FAILED SETTING TASK NAME"); - break; + case PR_SET_NAME: + // set the threads name +#ifdef __PX4_DARWIN + rv = pthread_setname_np(arg2); +#else + rv = pthread_setname_np(pthread_self(), arg2); +#endif + break; + + default: + rv = -1; + PX4_WARN("FAILED SETTING TASK NAME"); + break; } return rv; diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 9abd25bc59..777741ef06 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -72,6 +72,7 @@ int px4_sem_wait(px4_sem_t *s) if (s->value < 0) { ret = pthread_cond_wait(&(s->wait), &(s->lock)); + } else { ret = 0; } @@ -97,14 +98,15 @@ int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) if (s->value < 0) { ret = pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + } else { ret = 0; } int err = errno; - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN err = ret; - #endif +#endif if (err != 0 && err != ETIMEDOUT) { setbuf(stdout, NULL); @@ -132,6 +134,7 @@ int px4_sem_post(px4_sem_t *s) if (s->value <= 0) { ret = pthread_cond_signal(&(s->wait)); + } else { ret = 0; } From 1b65bba08345d6bdaaffe0aa7ff420681fdbdb76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:44:22 +0100 Subject: [PATCH 457/624] Work queue: Code style fixes --- src/platforms/posix/work_queue/hrt_thread.c | 6 +++--- src/platforms/posix/work_queue/work_thread.c | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index c7f5e7c22b..67a6efe440 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -121,11 +121,11 @@ static void hrt_work_process() int rv; // set the threads name - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN rv = pthread_setname_np("HRT"); - #else +#else rv = pthread_setname_np(pthread_self(), "HRT"); - #endif +#endif /* Then process queued work. We need to keep interrupts disabled while * we process items in the work list. diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c index 406e350341..8a88bdc505 100644 --- a/src/platforms/posix/work_queue/work_thread.c +++ b/src/platforms/posix/work_queue/work_thread.c @@ -308,11 +308,11 @@ int work_usrthread(int argc, char *argv[]) int rv; // set the threads name - #ifdef __PX4_DARWIN +#ifdef __PX4_DARWIN rv = pthread_setname_np("USR"); - #else +#else rv = pthread_setname_np(pthread_self(), "USR"); - #endif +#endif for (;;) { /* Then process queued work. We need to keep interrupts disabled while From f32569211dbce0e5f8df43793983457e15c29850 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:44:33 +0100 Subject: [PATCH 458/624] PX4 tasks: Code style fixes --- src/platforms/px4_tasks.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index bf8c5eeeff..e71e97f880 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -131,7 +131,7 @@ __EXPORT bool px4_task_is_running(const char *taskname); #ifdef __PX4_POSIX /** set process (and thread) options */ -__EXPORT int px4_prctl(int option, const char* arg2, unsigned pid); +__EXPORT int px4_prctl(int option, const char *arg2, unsigned pid); #endif __END_DECLS From 8cc54ac661136d930dbece40e46fee4b0225e5f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:01:23 +0100 Subject: [PATCH 459/624] SITL run: Request best scheduling for SITL processes - next up is lockstep --- Tools/sitl_run.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 2b535c5e12..dcfd21930b 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -48,7 +48,7 @@ if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ] then cd Tools/jMAVSim ant - java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & + nice -n -10 java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & SIM_PID=`echo $!` elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ] then @@ -65,7 +65,7 @@ then cd Tools/sitl_gazebo/Build cmake -Wno-dev .. make -j4 - gzserver --verbose ../worlds/${model}.world & + nice -n -10 gzserver --verbose ../worlds/${model}.world & SIM_PID=`echo $!` gzclient --verbose & GUI_PID=`echo $!` @@ -96,7 +96,7 @@ elif [ "$debugger" == "valgrind" ] then valgrind ./mainapp ../../../../${rc_script}_${program}_${model} else - ./mainapp ../../../../${rc_script}_${program}_${model} + nice -n -10 ./mainapp ../../../../${rc_script}_${program}_${model} fi if [ "$program" == "jmavsim" ] From f57eff5433da6d917603750d1462684e885b87cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:02:17 +0100 Subject: [PATCH 460/624] Bump internal sim thread priority. Output warning if sim packets are spaced more than 10 ms apart, giving some hints what is at miss with the system --- src/modules/simulator/simulator.cpp | 2 +- src/modules/simulator/simulator_mavlink.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index b732945f7b..bcd188b0f0 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -182,7 +182,7 @@ extern "C" { g_sim_task = px4_task_spawn_cmd("simulator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX, 1500, Simulator::start, argv); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 4ea56156ac..a10c89792d 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -457,7 +457,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT; + param.sched_priority = SCHED_PRIORITY_DEFAULT + 40; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); // setup serial connection to autopilot (used to get manual controls) @@ -533,11 +533,11 @@ void Simulator::pollForMAVLinkMessages(bool publish) // wait for new mavlink messages to arrive while (true) { - pret = ::poll(&fds[0], fd_count, 100); + pret = ::poll(&fds[0], fd_count, 10); //timed out if (pret == 0) { - PX4_WARN("mavlink sim timeout for 100 ms"); + PX4_WARN("mavlink sim timeout for 10 ms"); continue; } From 012a66787f8363ec1e9f07b8cab278561d3bc50c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:10:41 +0100 Subject: [PATCH 461/624] Fix includes in px4_tasks.h --- src/platforms/px4_tasks.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index e71e97f880..33c2fb4705 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -49,6 +49,9 @@ #elif defined(__PX4_NUTTX) typedef int px4_task_t; +#include +#define px4_prctl prctl + /** Default scheduler type */ #if CONFIG_RR_INTERVAL > 0 # define SCHED_DEFAULT SCHED_RR @@ -80,10 +83,7 @@ typedef int px4_task_t; #error "No target OS defined" #endif -#if defined (__PX4_NUTTX) -#include -#define px4_prctl prctl -#elif defined (__PX4_LINUX) +#if defined (__PX4_LINUX) #include #else #define PR_SET_NAME 1 From 895747825cf2ee431d126b64f9cb2c08c294b6cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:10:57 +0100 Subject: [PATCH 462/624] MAVLink app: Include task header --- src/modules/mavlink/mavlink_receiver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5609e849ff..459473d436 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -43,6 +43,7 @@ /* XXX trim includes */ #include #include +#include #include #include #include From bd0fcfb07ac4f0db46b4588c7cb47903691698d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:26:26 +0100 Subject: [PATCH 463/624] Work queue: Fix for Linux build --- src/platforms/posix/work_queue/hrt_thread.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 67a6efe440..68d980caff 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include From 88caa9bc936f84ca05cdfe8e68cb5e3ccec01704 Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Tue, 1 Dec 2015 12:47:11 +0100 Subject: [PATCH 464/624] MPU6000: Correct temperature scaling for use with ICM20608 --- src/drivers/mpu6000/mpu6000.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 235814c341..3d3bf24ada 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1913,7 +1913,14 @@ MPU6000::measure() arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; - _last_temperature = (report.temp) / 361.0f + 35.0f; + if(is_icm_device()) // if it is an ICM20608 + { + _last_temperature = (report.temp) / 326.8f + 25.0f; + } + else // If it is an MPU6000 + { + _last_temperature = (report.temp) / 361.0f + 35.0f; + } arb.temperature_raw = report.temp; arb.temperature = _last_temperature; From a048832d55939a52378239dd9c8550da58f18f64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:35:54 +0100 Subject: [PATCH 465/624] HRT thread: Linux: Add missing macro --- src/platforms/posix/work_queue/hrt_thread.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 68d980caff..dc3297b006 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -41,6 +41,9 @@ #include #include #include +#ifdef __PX4_LINUX +#define _GNU_SOURCE +#endif #include #include #include From b240b3fc3ca4dfb46ff086ab90e93e1955c128e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:36:52 +0100 Subject: [PATCH 466/624] MPU6K: Fix formatting --- src/drivers/mpu6000/mpu6000.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 3d3bf24ada..1d948bb7aa 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1913,14 +1913,12 @@ MPU6000::measure() arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; - if(is_icm_device()) // if it is an ICM20608 - { - _last_temperature = (report.temp) / 326.8f + 25.0f; - } - else // If it is an MPU6000 - { - _last_temperature = (report.temp) / 361.0f + 35.0f; - } + if (is_icm_device()) { // if it is an ICM20608 + _last_temperature = (report.temp) / 326.8f + 25.0f; + + } else { // If it is an MPU6000 + _last_temperature = (report.temp) / 361.0f + 35.0f; + } arb.temperature_raw = report.temp; arb.temperature = _last_temperature; From 360e3c03b6732fd2f557711455617bf3dd0dab7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:41:24 +0100 Subject: [PATCH 467/624] Krait: Add missing header --- src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 6e53f0bbfd..736bb3eefe 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -36,6 +36,10 @@ #include "px4_tasks.h" #include #include +#ifdef __PX4_LINUX +#define _GNU_SOURCE +#endif +#include #define LOG_TAG "uORBKraitFastRpcChannel.cpp" From 72e0664f1e816ad21aa410abb8faf72bf0108971 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:41:33 +0100 Subject: [PATCH 468/624] Simulator: Add missing header --- src/modules/simulator/simulator_mavlink.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a10c89792d..c2f7aa2f47 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -39,6 +39,10 @@ #include #include #include +#ifdef __PX4_LINUX +#define _GNU_SOURCE +#endif +#include extern "C" __EXPORT hrt_abstime hrt_reset(void); From 855dcf70693ddf7efaf9455ffb8566292a0b4151 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:41:48 +0100 Subject: [PATCH 469/624] Add missing header for Linux --- src/platforms/posix/px4_layer/px4_posix_impl.cpp | 3 +++ src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 30f4cf0f0c..f12fd44d68 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -49,6 +49,9 @@ #include "hrt_work.h" #include #include "px4_time.h" +#ifdef __PX4_LINUX +#define _GNU_SOURCE +#endif #include extern pthread_t _shell_task_id; diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index e05cc5432b..f4a291341d 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -48,6 +48,10 @@ #include #include #include +#ifdef __PX4_LINUX +#define _GNU_SOURCE +#endif +#include #include #include From 754aa6e94f6633e5a4695f3db064c9ebedd47b0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:42:06 +0100 Subject: [PATCH 470/624] Work thread: Add missing header for Linux --- src/platforms/posix/work_queue/work_thread.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c index 8a88bdc505..f8ff2a52fd 100644 --- a/src/platforms/posix/work_queue/work_thread.c +++ b/src/platforms/posix/work_queue/work_thread.c @@ -45,6 +45,10 @@ #include #include #include +#ifdef __PX4_LINUX +#define _GNU_SOURCE +#endif +#include #include #include #include "work_lock.h" From 5f9da470bba0f20cbc516cd52ed2990793e82d3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:49:55 +0100 Subject: [PATCH 471/624] Do not redefine GNU source in modules --- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp | 3 --- src/modules/simulator/simulator_mavlink.cpp | 3 --- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 459473d436..3ac659608a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 736bb3eefe..71d052b4c6 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -36,9 +36,6 @@ #include "px4_tasks.h" #include #include -#ifdef __PX4_LINUX -#define _GNU_SOURCE -#endif #include #define LOG_TAG "uORBKraitFastRpcChannel.cpp" diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c2f7aa2f47..be4be9711e 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -39,9 +39,6 @@ #include #include #include -#ifdef __PX4_LINUX -#define _GNU_SOURCE -#endif #include extern "C" __EXPORT hrt_abstime hrt_reset(void); From a54b40c4572e71cb841de2ecb2e7a5e3d809b575 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:50:12 +0100 Subject: [PATCH 472/624] Do not redefine GNU source in platform for POSIX --- src/platforms/posix/px4_layer/px4_posix_impl.cpp | 4 +--- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 4 +--- src/platforms/posix/work_queue/hrt_thread.c | 3 --- src/platforms/posix/work_queue/work_thread.c | 3 --- 4 files changed, 2 insertions(+), 12 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index f12fd44d68..c77fe760b8 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -49,9 +50,6 @@ #include "hrt_work.h" #include #include "px4_time.h" -#ifdef __PX4_LINUX -#define _GNU_SOURCE -#endif #include extern pthread_t _shell_task_id; diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index f4a291341d..a220b88c67 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -48,9 +49,6 @@ #include #include #include -#ifdef __PX4_LINUX -#define _GNU_SOURCE -#endif #include #include diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index dc3297b006..68d980caff 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -41,9 +41,6 @@ #include #include #include -#ifdef __PX4_LINUX -#define _GNU_SOURCE -#endif #include #include #include diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c index f8ff2a52fd..2050ad51e6 100644 --- a/src/platforms/posix/work_queue/work_thread.c +++ b/src/platforms/posix/work_queue/work_thread.c @@ -45,9 +45,6 @@ #include #include #include -#ifdef __PX4_LINUX -#define _GNU_SOURCE -#endif #include #include #include From bc1639386bf274789a187fe39e889ee5630e5bd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:54:37 +0100 Subject: [PATCH 473/624] Fix compilation of baro sim on Linux --- src/platforms/posix/drivers/barosim/baro.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 960e14c1a3..b538f399b1 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -534,10 +534,9 @@ BAROSIM::cycle() if (ret != OK) { uint8_t cmd = ADDR_RESET_CMD; - int result; /* bump the retry count */ - result = transfer(&cmd, 1, nullptr, 0); + (void)transfer(&cmd, 1, nullptr, 0); /* reset the collection state machine and try again */ //start_cycle(); From c140c2b59c9f1e3eeacb9c4bcdd421572a0feca3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 13:58:28 +0100 Subject: [PATCH 474/624] Baro sim: Ensure struct is initialized to zero --- src/platforms/posix/drivers/barosim/baro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index b538f399b1..fbcd28ae6e 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -662,7 +662,7 @@ BAROSIM::collect() perf_begin(_sample_perf); - struct baro_report report; + struct baro_report report = {}; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); From a880725bd76b2294808740893302f542c27ec4ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 14:07:11 +0100 Subject: [PATCH 475/624] Baro sim: Fix up struct use --- src/platforms/posix/drivers/barosim/baro.cpp | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index fbcd28ae6e..f80e42cad8 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -653,11 +653,11 @@ BAROSIM::collect() int ret; #pragma pack(push, 1) - struct { + struct raw_baro_s { float pressure; float altitude; float temperature; - } baro_report; + } raw_baro; #pragma pack(pop) perf_begin(_sample_perf); @@ -669,7 +669,7 @@ BAROSIM::collect() /* read the most recent measurement - read offset/size are hardcoded in the interface */ uint8_t cmd = 0; - ret = transfer(&cmd, 1, (uint8_t *)&report, sizeof(baro_report)); + ret = transfer(&cmd, 1, (uint8_t*)(&raw_baro), sizeof(raw_baro)); if (ret < 0) { perf_count(_comms_errors); @@ -679,16 +679,14 @@ BAROSIM::collect() /* handle a measurement */ if (_measure_phase == 0) { - report.pressure = baro_report.pressure; - report.altitude = baro_report.altitude; - report.temperature = baro_report.temperature; - report.timestamp = hrt_absolute_time(); + report.pressure = raw_baro.pressure; + report.altitude = raw_baro.altitude; + report.temperature = raw_baro.temperature; } else { - report.pressure = baro_report.pressure; - report.altitude = baro_report.altitude; - report.temperature = baro_report.temperature; - report.timestamp = hrt_absolute_time(); + report.pressure = raw_baro.pressure; + report.altitude = raw_baro.altitude; + report.temperature = raw_baro.temperature; /* publish it */ if (!(m_pub_blocked)) { From 2db915080c625659c473704f2f7f57df5ac35e97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 14:14:17 +0100 Subject: [PATCH 476/624] Linux compile fix --- src/platforms/posix/work_queue/hrt_thread.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 68d980caff..d9f8055519 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -41,6 +41,11 @@ #include #include #include +#ifdef __PX4_LINUX +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif +#endif #include #include #include From 119c82de4e1def0be9dd73357df486ab92c2e124 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 14:14:47 +0100 Subject: [PATCH 477/624] Baro sim: Code style fix --- src/platforms/posix/drivers/barosim/baro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index f80e42cad8..2485189487 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -669,7 +669,7 @@ BAROSIM::collect() /* read the most recent measurement - read offset/size are hardcoded in the interface */ uint8_t cmd = 0; - ret = transfer(&cmd, 1, (uint8_t*)(&raw_baro), sizeof(raw_baro)); + ret = transfer(&cmd, 1, (uint8_t *)(&raw_baro), sizeof(raw_baro)); if (ret < 0) { perf_count(_comms_errors); From 905edfd9c993d85d6d09f65582896f37373cbd55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 14:24:43 +0100 Subject: [PATCH 478/624] HRT: Do not set thread name on Linux --- src/platforms/posix/work_queue/hrt_thread.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index d9f8055519..ea185525ac 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -41,12 +41,6 @@ #include #include #include -#ifdef __PX4_LINUX -#ifndef _GNU_SOURCE -#define _GNU_SOURCE -#endif -#endif -#include #include #include #include @@ -130,7 +124,8 @@ static void hrt_work_process() #ifdef __PX4_DARWIN rv = pthread_setname_np("HRT"); #else - rv = pthread_setname_np(pthread_self(), "HRT"); + // The Linux headers do not actually contain this + //rv = pthread_setname_np(pthread_self(), "HRT"); #endif /* Then process queued work. We need to keep interrupts disabled while From 33f85a50243aed61919e11c4807ff2c323413c54 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 1 Dec 2015 15:01:15 +0100 Subject: [PATCH 479/624] fixed unused variable --- src/modules/simulator/simulator_mavlink.cpp | 12 ++++++------ src/platforms/posix/work_queue/hrt_thread.c | 3 +-- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index be4be9711e..12402b4632 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -358,12 +358,12 @@ void Simulator::send() fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; - int rv; + // set the threads name #ifdef __PX4_DARWIN - rv = pthread_setname_np("sim_send"); + pthread_setname_np("sim_send"); #else - rv = pthread_setname_np(pthread_self(), "sim_send"); + pthread_setname_np(pthread_self(), "sim_send"); #endif int pret; @@ -523,12 +523,12 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); - int rv; + // set the threads name #ifdef __PX4_DARWIN - rv = pthread_setname_np("sim_rcv"); + pthread_setname_np("sim_rcv"); #else - rv = pthread_setname_np(pthread_self(), "sim_rcv"); + pthread_setname_np(pthread_self(), "sim_rcv"); #endif // wait for new mavlink messages to arrive diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index ea185525ac..5199785d78 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -119,10 +119,9 @@ static void hrt_work_process() uint32_t remaining; uint32_t next; - int rv; // set the threads name #ifdef __PX4_DARWIN - rv = pthread_setname_np("HRT"); + pthread_setname_np("HRT"); #else // The Linux headers do not actually contain this //rv = pthread_setname_np(pthread_self(), "HRT"); From f91af2221fc9a00f5ef174ca6c23c69e5ef7a46b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 1 Dec 2015 15:55:21 +0100 Subject: [PATCH 480/624] fix sem error for linux --- src/drivers/device/vdev_posix.cpp | 3 +++ src/platforms/posix/px4_layer/px4_sem.cpp | 8 +++----- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 3e140e907c..b07b6c1f4b 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -301,6 +301,9 @@ extern "C" { // Execute a blocking wait for that time in the future errno = 0; ret = px4_sem_timedwait(&sem, &ts); +#ifndef __PX4_DARWIN + ret = errno; +#endif // Ensure ret is negative on failure if (ret > 0) { diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 777741ef06..67e59bc8fd 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -95,6 +95,7 @@ int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) } s->value--; + errno = 0; if (s->value < 0) { ret = pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); @@ -103,10 +104,7 @@ int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) ret = 0; } - int err = errno; -#ifdef __PX4_DARWIN - err = ret; -#endif + int err = ret; if (err != 0 && err != ETIMEDOUT) { setbuf(stdout, NULL); @@ -119,7 +117,7 @@ int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) int mret = pthread_mutex_unlock(&(s->lock)); - return (ret) ? ret : mret; + return (err) ? err : mret; } int px4_sem_post(px4_sem_t *s) From c0f45ed8a7ab7a8ad1a272086237a2f169841011 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 1 Dec 2015 16:55:46 +0100 Subject: [PATCH 481/624] use tilt compensation also for position control --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a631336273..7adc51392e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1370,9 +1370,10 @@ MulticopterPositionControl::task_main() } } } + } - } else { - /* thrust compensation for altitude only control mode */ + if (_control_mode.flag_control_altitude_enabled) { + /* thrust compensation for altitude only control modes */ float att_comp; if (_R(2, 2) > TILT_COS_MAX) { From 425d707112cf8dd80f58f45ea5fc793d31be9afa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 20:45:57 +0100 Subject: [PATCH 482/624] SITL: Fixed wing uses joystick --- posix-configs/SITL/init/rc.fixed_wing | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index 635772d727..7aa521daf2 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -21,7 +21,7 @@ param set CAL_MAG0_XOFF 0.01 param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbled start tone_alarm start gyrosim start From 3886e33f8567eae4470065be0b51e2590834f848 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 20:46:12 +0100 Subject: [PATCH 483/624] SITL: Gazebo uses joystick --- posix-configs/SITL/init/rcS_gazebo_iris | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index 00fdb610da..3d38f80da6 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -27,7 +27,7 @@ param set MPC_XY_VEL_P 0.05 param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbledsim start tone_alarm start gyrosim start From bc4bbce1e389388ec45be3683ad817b9d7c36e3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 20:46:26 +0100 Subject: [PATCH 484/624] SITL: Tailsitter uses joystick --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index 09b361d8c5..c88948cf55 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -28,7 +28,7 @@ param set MPC_XY_VEL_P 0.05 param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbledsim start tone_alarm start gyrosim start From c5c08645467f70b033860a096cb5eb7680e325f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 20:46:51 +0100 Subject: [PATCH 485/624] SITL: jMAVSim and Iris use joystick --- posix-configs/SITL/init/rcS_jmavsim_iris | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 6d8b575ac8..7120d63ac6 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -28,7 +28,7 @@ param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbledsim start tone_alarm start gyrosim start From a78afc818f9cb3245a5b15e3aef4aa0582df515a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 20:47:09 +0100 Subject: [PATCH 486/624] SITL: IRIS / ROS: Use Joystick --- posix-configs/SITL/init/rc_iris_ros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 3293166bad..af2db4d57b 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -40,7 +40,7 @@ param set MP_ROLLRATE_I 0.001 param set MP_ROLLRATE_D 0.001 param set MP_PITCH_P 4 param set MP_PITCHRATE_P 0.3 -param set COM_RC_IN_MODE 2 +param set COM_RC_IN_MODE 1 rgbled start tone_alarm start gyrosim start From 6828e4db2cb70af0b5aa1a323f6b2a576130dec7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Dec 2015 09:14:08 +0100 Subject: [PATCH 487/624] Geofence: Use machine data types --- src/modules/navigator/geofence.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index a19e13ca2f..deaf2dadb8 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -119,7 +119,7 @@ private: float _altitude_min; float _altitude_max; - uint8_t _vertices_count; + unsigned _vertices_count; /* Params */ control::BlockParamInt _param_action; @@ -129,7 +129,7 @@ private: control::BlockParamInt _param_max_hor_distance; control::BlockParamInt _param_max_ver_distance; - uint8_t _outside_counter; + unsigned _outside_counter; int _mavlinkFd; From 557fd3d99bc02acb53e3716f1fafb6ccb16a705f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Dec 2015 09:15:11 +0100 Subject: [PATCH 488/624] Navigation: Turn mission item into packed struct and make nav_cmd compiler-independent --- src/modules/navigator/navigation.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index d23a20c5ff..e6b8a68281 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -81,6 +81,7 @@ enum ORIGIN { * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ +#pragma pack(push, 1) struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees */ @@ -89,19 +90,19 @@ struct mission_item_s { float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< navigation command */ + unsigned nav_cmd; /**< navigation command */ float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ - enum ORIGIN origin; /**< where the waypoint has been generated */ + unsigned origin; /**< where the waypoint has been generated */ int do_jump_mission_index; /**< index where the do jump will go to */ unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ unsigned do_jump_current_count; /**< count how many times the jump has been done */ int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ int actuator_value; /**< new value for selected actuator in ms 900...2000 */ }; - +#pragma pack(pop) #include /** From b6c4501d0e221fb2d610c028a2c5f801502d5fad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Dec 2015 09:15:37 +0100 Subject: [PATCH 489/624] Adjust data size of dataman to force reset of all existing user data --- src/modules/dataman/dataman.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index a29a29c735..d5e39dbfd3 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -89,7 +89,7 @@ typedef enum { } dm_reset_reason; /** Maximum size in bytes of a single item instance */ -#define DM_MAX_DATA_SIZE 124 +#define DM_MAX_DATA_SIZE 126 /** Retrieve from the data manager store */ __EXPORT ssize_t From f87407eb5423d909fb2c581ab526f096020cd50b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Dec 2015 11:55:32 +0100 Subject: [PATCH 490/624] PWMin: Fix test cmd --- src/drivers/pwm_input/pwm_input.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 4d73f6204d..45b493a426 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -599,6 +599,9 @@ static void pwmin_test(void) (unsigned)buf.period, (unsigned)buf.pulse_width, (unsigned)buf.error_count); + } else { + /* no data, retry in 2 ms */ + ::usleep(2000); } } From 7b9d49f6b6c12a5c7eeac58a9f2ce285b071224f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Dec 2015 12:23:59 +0100 Subject: [PATCH 491/624] PWM input driver: Fix code style --- src/drivers/pwm_input/pwm_input.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 45b493a426..1a609a91f0 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -599,6 +599,7 @@ static void pwmin_test(void) (unsigned)buf.period, (unsigned)buf.pulse_width, (unsigned)buf.error_count); + } else { /* no data, retry in 2 ms */ ::usleep(2000); From 644ce51956f69dd5846112b76d4dfd712925e521 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 2 Dec 2015 12:56:20 +0100 Subject: [PATCH 492/624] gyrosim: calculate delta angles and delta velocities --- .../posix/drivers/gyrosim/gyrosim.cpp | 51 +++++++++++++++---- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index c6e7184d58..3b42951cc9 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -71,6 +71,7 @@ #include #include +#include #include #include #include @@ -173,6 +174,9 @@ private: perf_counter_t _reset_retries; perf_counter_t _controller_latency_perf; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // last temperature reading for print_info() @@ -327,6 +331,8 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _accel_int(1000000 / GYROSIM_ACCEL_DEFAULT_RATE, true), + _gyro_int(1000000 / GYROSIM_GYRO_DEFAULT_RATE, true), _rotation(rotation), _last_temperature(0) { @@ -1094,6 +1100,14 @@ GYROSIM::_measure() arb.y = mpu_report.accel_y; arb.z = mpu_report.accel_z; + math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + grb.x_raw = (int16_t)(mpu_report.gyro_x / _gyro_range_scale); grb.y_raw = (int16_t)(mpu_report.gyro_y / _gyro_range_scale); grb.z_raw = (int16_t)(mpu_report.gyro_z / _gyro_range_scale); @@ -1108,24 +1122,39 @@ GYROSIM::_measure() grb.y = mpu_report.gyro_y; grb.z = mpu_report.gyro_z; + math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + _accel_reports->force(&arb); _gyro_reports->force(&grb); - /* notify anyone waiting for data */ - updateNotify(); - _gyro->parent_poll_notify(); + if (accel_notify) { + /* notify anyone waiting for data */ + updateNotify(); + _gyro->parent_poll_notify(); - if (!(_pub_blocked)) { - /* log the time of this report */ - perf_begin(_controller_latency_perf); - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + if (!(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } } - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + if (gyro_notify) { + updateNotify(); + _gyro->parent_poll_notify(); + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } } /* stop measuring */ From 4e4f780ecc7f873d7398e6b6ea0e6fc4dbde3e05 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Wed, 25 Nov 2015 16:22:23 -0500 Subject: [PATCH 493/624] Add support for switching to auto modes via SET_MODE --- src/modules/commander/commander.cpp | 18 +++++++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc4f2feec7..fd1826a0c0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -559,6 +559,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t)cmd->param1; uint8_t custom_main_mode = (uint8_t)cmd->param2; + uint8_t custom_sub_mode = (uint8_t)cmd->param3; transition_result_t arming_ret = TRANSITION_NOT_CHANGED; @@ -596,7 +597,22 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + switch(custom_sub_mode) { + case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); + break; + + default: + main_ret = TRANSITION_DENIED; + mavlink_log_critical(mavlink_fd, "Unsupported auto command"); + break; + } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ac659608a..508a1d8c95 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -539,7 +539,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; vcmd.param2 = custom_mode.main_mode; - vcmd.param3 = 0; + vcmd.param3 = custom_mode.sub_mode; vcmd.param4 = 0; vcmd.param5 = 0; vcmd.param6 = 0; From df6fe7f99372b1a9194d811daca385d88452b838 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 29 Nov 2015 19:26:45 +0100 Subject: [PATCH 494/624] transition to auto mission if custom mode param is not set --- src/modules/commander/commander.cpp | 33 +++++++++++++++++------------ 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd1826a0c0..62366f2981 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -597,21 +597,26 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - switch(custom_sub_mode) { - case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); - break; - case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); - break; - case PX4_CUSTOM_SUB_MODE_AUTO_RTL: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); - break; + if (custom_sub_mode > 0) { + switch(custom_sub_mode) { + case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); + break; + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); + break; - default: - main_ret = TRANSITION_DENIED; - mavlink_log_critical(mavlink_fd, "Unsupported auto command"); - break; + default: + main_ret = TRANSITION_DENIED; + mavlink_log_critical(mavlink_fd, "Unsupported auto mode"); + break; + } + + } else { + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { From 756282b195473844b08b632e5be9665f9807f862 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 2 Dec 2015 12:04:34 +0100 Subject: [PATCH 495/624] added auto takeoff sub mode --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 62366f2981..14aa97d375 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -608,6 +608,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_RTL: main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); break; + case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF); + break; default: main_ret = TRANSITION_DENIED; From c4ed598dff0ff3f54a803f708b1cb4b9be98a9bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Dec 2015 23:00:53 +0100 Subject: [PATCH 496/624] MAVLink app: Do not decode buttons when they are not true mode switches --- src/modules/mavlink/mavlink_receiver.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 508a1d8c95..6b5f62fcca 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1149,13 +1149,6 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - manual.mode_switch = decode_switch_pos(man.buttons, 0); - manual.return_switch = decode_switch_pos(man.buttons, 1); - manual.posctl_switch = decode_switch_pos(man.buttons, 2); - manual.loiter_switch = decode_switch_pos(man.buttons, 3); - manual.acro_switch = decode_switch_pos(man.buttons, 4); - manual.offboard_switch = decode_switch_pos(man.buttons, 5); - if (_manual_pub == nullptr) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); From e1a5efb9a5c8c55b32b36b126828e05ab14c7c17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 13:49:35 +0100 Subject: [PATCH 497/624] INAV: Be less chatty --- .../position_estimator_inav/position_estimator_inav_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index a14cc82630..97e476e5d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -428,8 +428,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offset: %d m", (int)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } From 672a7b7b5cb1934c30d073288fdbc86e43b19ef9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 14:28:05 +0100 Subject: [PATCH 498/624] SDLOG2: Initialize thread attrs correctly --- src/modules/sdlog2/sdlog2.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fb123208ac..f1648037c0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -686,9 +686,12 @@ void sdlog2_start_log() pthread_attr_init(&logwriter_attr); struct sched_param param; + (void)pthread_attr_getschedparam(&logwriter_attr, ¶m); /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); + if (pthread_attr_setschedparam(&logwriter_attr, ¶m)) { + warnx("sdlog2: failed setting sched params"); + } pthread_attr_setstacksize(&logwriter_attr, 2048); From 80439989a9783e685a75e1dba0fc4b475550edf5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 14:28:17 +0100 Subject: [PATCH 499/624] UAVCAN: Initialize thread attributes correctly --- src/modules/uavcan/uavcan_servers.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index d2c149724b..318940eb64 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -188,9 +188,12 @@ int UavcanServers::start(uavcan::INode &main_node) struct sched_param param; pthread_attr_init(&tattr); + (void)pthread_attr_getschedparam(&tattr, ¶m); tattr.stacksize = StackSize; param.sched_priority = Priority; - pthread_attr_setschedparam(&tattr, ¶m); + if (pthread_attr_setschedparam(&tattr, ¶m)) { + warnx("setting sched params failed"); + } static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; From a003013fd98c5d6f4341fedfd7646d768fdc5178 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 14:28:37 +0100 Subject: [PATCH 500/624] QuRT: Set thread attributes correctly --- src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 4216bbf599..19319d1736 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -161,6 +161,13 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } + rv = pthread_attr_getschedparam(&attr, ¶m); + + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param"); + return (rv < 0) ? rv : -rv; + } + #if 0 rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); From 08a1797eec52aa1609f0f7aa152e9f1118222ea6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 3 Dec 2015 15:19:26 +0100 Subject: [PATCH 501/624] changed isnan checks and cmake compiler condition for apple machines --- src/drivers/device/vdev_posix.cpp | 2 +- src/firmware/posix/CMakeLists.txt | 2 +- src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index b07b6c1f4b..d99c7cfc74 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -99,7 +99,7 @@ extern "C" { strncmp(path, "/dev/", 5) != 0) { va_list p; va_start(p, flags); - mode = va_arg(p, mode_t); + mode = va_arg(p, int); va_end(p); // Create the file diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index c8b9febe39..2ed7c4b441 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -10,7 +10,7 @@ add_executable(mainapp apps.h ) -if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) +if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index c3d44588fd..fb929c9906 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -74,7 +74,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - if (isnan(delay_element_0) || isinf(delay_element_0)) { + if (!PX4_ISFINITE(delay_element_0)) { // don't allow bad values to propagate via the filter delay_element_0 = sample; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6b5f62fcca..f4a3fbd208 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -721,7 +721,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) { + if (!offboard_control_mode.ignore_attitude && PX4_ISFINITE(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -730,7 +730,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) { + if (!offboard_control_mode.ignore_bodyrate && PX4_ISFINITE(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; From 35d387792ce835b7525c790b2f7d1214e4173aa5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 16:08:38 +0100 Subject: [PATCH 502/624] Fix product string for FMUv4 --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 0b653b9eb5..a5b79ebf48 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -671,7 +671,7 @@ CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" # CONFIG_USBMSC is not set # CONFIG_USBHOST is not set # CONFIG_WIRELESS is not set From c38341a263d99bf39fae20461dd44fdae9324960 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 16:10:14 +0100 Subject: [PATCH 503/624] Enable FMUv4 in Travis --- .travis.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index e7c3119377..b0857fd4b2 100644 --- a/.travis.yml +++ b/.travis.yml @@ -107,14 +107,16 @@ script: - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default + - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default - echo 'Running Tests..' && make px4fmu-v2_default test after_success: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 - && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 - && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html From 9bfa933a1b4c3e5da6fcb002481244192784d886 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:23:47 +0100 Subject: [PATCH 504/624] Travis: Fix typo --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index b0857fd4b2..f2a72b9ff5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -114,7 +114,7 @@ after_success: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 - && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4 + && cp build_px4fmu-v4_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4 && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ From daee5eeefa6f3768dd11efa3768a2b1c8711cf5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 21:24:58 +0100 Subject: [PATCH 505/624] Mission handling: Use default acceptance radius in case its set to zero --- src/modules/navigator/mission_block.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b230bcab3f..80d8b3df3d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -143,7 +143,14 @@ MissionBlock::is_mission_item_reached() } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { + float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); + + /* if set to zero use the default instead */ + if (mission_acceptance_radius < 0.001f) { + mission_acceptance_radius = _navigator->get_acceptance_radius(); + } + + if (dist >= 0.0f && dist <= mission_acceptance_radius) { _waypoint_position_reached = true; } } From 2a1b1fe11db3406ff9e6027f858c7a994adee15d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 21:25:46 +0100 Subject: [PATCH 506/624] SITL: Better Navigation defaults for multicopters --- posix-configs/SITL/init/rcS_gazebo_iris | 3 +++ posix-configs/SITL/init/rcS_jmavsim_iris | 3 +++ 2 files changed, 6 insertions(+) diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index 3d38f80da6..95be984fb4 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -28,6 +28,9 @@ param set MPC_XY_VEL_D 0.005 param set MPC_XY_FF 0.1 param set SENS_BOARD_ROT 8 param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 rgbledsim start tone_alarm start gyrosim start diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 7120d63ac6..88a3f3978e 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -29,6 +29,9 @@ param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 rgbledsim start tone_alarm start gyrosim start From c070d326e015bb37fad364e4d05d5f768b4a5b6e Mon Sep 17 00:00:00 2001 From: Peter Duerr Date: Fri, 4 Dec 2015 14:14:31 +0900 Subject: [PATCH 507/624] Avoid violation of acos' domain in map_projection * There are pathological cases (e.g., setpoint very close to reference for certain reference latitudes), where numerical errors lead to a sum larger than 1.0 passed to acos, resulting in NaN values. This should fix issue #2813 --- src/lib/geo/geo.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index fe0224d508..448fd97986 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -148,7 +148,13 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double cos_lat = cos(lat_rad); double cos_d_lon = cos(lon_rad - ref->lon_rad); - double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon; + if (arg > 1.0) { + arg = 1.0; + } else if (arg < -1.0) { + arg = -1.0; + } + double c = acos(arg); double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; From 6b78b72ab2e1e6bdbfdf3f65ab41e3e3e758b97c Mon Sep 17 00:00:00 2001 From: Peter Duerr Date: Fri, 4 Dec 2015 14:53:59 +0900 Subject: [PATCH 508/624] Fix code style according to astyle --- src/lib/geo/geo.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 448fd97986..6912f19070 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -149,11 +149,14 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double cos_d_lon = cos(lon_rad - ref->lon_rad); double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon; + if (arg > 1.0) { arg = 1.0; + } else if (arg < -1.0) { arg = -1.0; } + double c = acos(arg); double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); From abbd560a903b5391bc28d23f2a121ea2cb034f36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 09:51:39 +0100 Subject: [PATCH 509/624] Feasibility checker: Do not accept ROI --- src/modules/navigator/mission_feasibility_checker.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index a1c166bbbf..ece75c89f7 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -197,7 +197,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && missionitem.nav_cmd != NAV_CMD_LAND && missionitem.nav_cmd != NAV_CMD_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_ROI && missionitem.nav_cmd != NAV_CMD_PATHPLANNING && missionitem.nav_cmd != NAV_CMD_DO_JUMP && missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO) { From 590f00f56089cf6afa62b7f4df07527c014da559 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 10:50:38 +0100 Subject: [PATCH 510/624] Navigator: Set sane defaults for navigation and loiter waypoints with zero acceptance radius or orbit --- src/modules/navigator/mission_block.cpp | 5 +++-- src/modules/navigator/navigation.h | 2 ++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 80d8b3df3d..e2f454d4cf 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -146,7 +146,7 @@ MissionBlock::is_mission_item_reached() float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ - if (mission_acceptance_radius < 0.001f) { + if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } @@ -210,7 +210,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; + sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius : + _navigator->get_loiter_radius(); sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index e6b8a68281..b0fabc08f5 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -47,6 +47,8 @@ #define NUM_MISSIONS_SUPPORTED 256 +#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ + /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_IDLE = 0, From 835208e070dd5f7ab39f15861d920392f559506a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 10:51:15 +0100 Subject: [PATCH 511/624] Commander: Fix dev handle init --- src/modules/commander/commander_helper.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a7ca5b231f..e1a517538c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -96,12 +96,15 @@ bool is_vtol(const struct vehicle_status_s * current_status) { current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR); } -static DevHandle h_buzzer; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; +static DevHandle h_leds; +static DevHandle h_rgbleds; +static DevHandle h_buzzer; + static param_t bat_v_empty_h; static param_t bat_v_full_h; static param_t bat_n_cells_h; @@ -136,7 +139,6 @@ int buzzer_init() tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; - DevHandle h_buzzer; DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h_buzzer); if (!h_buzzer.isValid()) { @@ -268,9 +270,6 @@ int blink_msg_state() } } -static DevHandle h_leds; -static DevHandle h_rgbleds; - int led_init() { blink_msg_end = 0; From a5a33579285b426c3b474092b73f5ff91a93ef98 Mon Sep 17 00:00:00 2001 From: Anthony Kenga Date: Wed, 25 Nov 2015 21:11:21 +0200 Subject: [PATCH 512/624] Attitude estimator Q: fix magnetic declination inducing gyro bias growth --- src/lib/mathlib/math/Quaternion.hpp | 10 +++++++ .../attitude_estimator_q_main.cpp | 27 +++++++++++++++++-- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index b7cb068dd4..4f5ccc623b 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -211,6 +211,16 @@ public: data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } + /** + * simplified version of the above method to create quaternion representing rotation only by yaw + */ + void from_yaw(float yaw) { + data[0] = cosf(yaw / 2.0f); + data[1] = 0.0f; + data[2] = 0.0f; + data[3] = sinf(yaw / 2.0f); + } + /** * create Euler angles vector from the quaternion */ diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 2a61e18046..f456e73120 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -202,6 +202,9 @@ private: bool init(); bool update(float dt); + + // Update magnetic declination (in rads) immediately changing yaw rotation + void update_mag_declination(float new_declination); }; @@ -522,7 +525,7 @@ void AttitudeEstimatorQ::task_main() if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) { /* set magnetic declination automatically */ - _mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon)); + update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon))); } } @@ -650,7 +653,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); - _mag_decl = math::radians(mag_decl_deg); + update_mag_declination(math::radians(mag_decl_deg)); int32_t mag_decl_auto_int; param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int); _mag_decl_auto = mag_decl_auto_int != 0; @@ -685,6 +688,12 @@ bool AttitudeEstimatorQ::init() // Convert to quaternion _q.from_dcm(R); + + // Compensate for magnetic declination + Quaternion decl_rotation; + decl_rotation.from_yaw(_mag_decl); + _q = decl_rotation * _q; + _q.normalize(); if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && @@ -788,6 +797,20 @@ bool AttitudeEstimatorQ::update(float dt) return true; } +void AttitudeEstimatorQ::update_mag_declination(float new_declination) +{ + // Apply initial declination or trivial rotations without changing estimation + if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) { + _mag_decl = new_declination; + + } else { + // Immediately rotate current estimation to avoid gyro bias growth + Quaternion decl_rotation; + decl_rotation.from_yaw(new_declination - _mag_decl); + _q = decl_rotation * _q; + _mag_decl = new_declination; + } +} int attitude_estimator_q_main(int argc, char *argv[]) { From 5f18182accf83acad2fab87f92226fa2571f3ef7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 23 Aug 2015 23:01:53 +0200 Subject: [PATCH 513/624] bst (Black Sheep Telemetery) module added --- src/drivers/bst/bst.cpp | 362 ++++++++++++++++++++++++++++++++++++++ src/drivers/bst/module.mk | 9 + 2 files changed, 371 insertions(+) create mode 100644 src/drivers/bst/bst.cpp create mode 100644 src/drivers/bst/module.mk diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp new file mode 100644 index 0000000000..e21e268e04 --- /dev/null +++ b/src/drivers/bst/bst.cpp @@ -0,0 +1,362 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 bst.cpp + * + * Black Sheep Telemetry driver + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define DEFAULT_DEVICE 1 // TODO use PX4 define for external I2C +#define BST_DEVICE_PATH "/dev/bst0" + +static const char commandline_usage[] = "usage: bst start|status|stop"; + +extern "C" __EXPORT int bst_main(int argc, char *argv[]); + +#define BST_ADDR 0x76 + +namespace px4 { +namespace bst { + +#pragma pack(push, 1) + +template +struct BSTPacket { + uint8_t length; + uint8_t type; + T payload; + uint8_t crc; +}; + +struct BSTDeviceInfoRequest { + uint8_t cmd; +}; + +struct BSTDeviceInfoReply { + uint32_t hw_id; + uint16_t fw_id; + uint8_t dev_name_len; + char dev_name[32]; +}; + +struct BSTGPSPosition { + int32_t lat; + int32_t lon; + uint16_t gs; + uint16_t heading; + uint16_t alt; + uint8_t sats; +}; + +struct BSTAttitude { + int16_t pitch; + int16_t roll; + int16_t yaw; +}; + +#pragma pack(pop) + +class BST : public device::I2C { +public: + BST(int bus); + + virtual ~BST(); + + virtual int init(); + + virtual int probe(); + + virtual int info() { return 0; } + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; } + + work_s *work_ptr() { return &_work; } + + void stop(); + + static void start_trampoline(void *arg); + +private: + work_s _work = {}; + bool _should_run = false; + unsigned _interval = 100; + int _gps_sub; + int _attitude_sub; + int _battery_sub; + + void start(); + + static void cycle_trampoline(void *arg); + + void cycle(); + + template + void send_packet(BSTPacket &packet) { + packet.length = sizeof(packet) - 1; // Length + packet.crc = crc8(reinterpret_cast(&packet.type), sizeof(packet) - 2); + + transfer(reinterpret_cast(&packet), sizeof(packet), nullptr, 0); + } + + template + void send_packet(BSTPacket &packet_send, BSTPacket &packet_recv) { + packet_send.length = sizeof(packet_send) - 1; // Length + packet_send.crc = crc8(reinterpret_cast(&packet_send.type), sizeof(packet_send) - 2); + transfer(reinterpret_cast(&packet_send), sizeof(packet_send), reinterpret_cast(&packet_recv), sizeof(packet_recv)); + } + + static uint8_t crc8(uint8_t *data, size_t len); + + //! Byte swap unsigned short + uint16_t swap_uint16( uint16_t val ) + { + return (val << 8) | (val >> 8 ); + } + + //! Byte swap short + int16_t swap_int16( int16_t val ) + { + return (val << 8) | ((val >> 8) & 0xFF); + } + + //! Byte swap unsigned int + uint32_t swap_uint32( uint32_t val ) + { + val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF ); + return (val << 16) | (val >> 16); + } + + //! Byte swap int + int32_t swap_int32( int32_t val ) + { + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF ); + return (val << 16) | ((val >> 16) & 0xFFFF); + } +}; + +static BST *g_bst = nullptr; + +BST::BST(int bus) : + I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR + #ifdef __PX4_NUTTX + ,100000 /* maximum speed supported */ + #endif + ) { +} + +BST::~BST() { + _should_run = false; + + work_cancel(LPWORK, &_work); +} + +int BST::probe() { + int retries_prev = _retries; + _retries = 3; + + BSTPacket dev_info_req = {}; + dev_info_req.type = 0x0A; + dev_info_req.payload.cmd = 0x04; + BSTPacket dev_info_reply = {}; + send_packet(dev_info_req, dev_info_reply); + if (dev_info_reply.type != 0x05) { + warnx("no devices found"); + return -EIO; + } + uint8_t *reply_raw = reinterpret_cast(&dev_info_reply); + uint8_t crc_calc = crc8(reinterpret_cast(&dev_info_reply.type), dev_info_reply.length - 1); + uint8_t crc_recv = reply_raw[dev_info_reply.length]; + if (crc_recv != crc_calc) { + warnx("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc); + return -EIO; + } + dev_info_reply.payload.dev_name[dev_info_reply.payload.dev_name_len] = '\0'; + warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s", (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id), dev_info_reply.payload.dev_name); + + _retries = retries_prev; + + return OK; +} + +int BST::init() { + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + work_queue(LPWORK, &_work, BST::start_trampoline, g_bst, 0); + + return OK; +} + +void BST::start_trampoline(void *arg) { + reinterpret_cast(arg)->start(); +} + +void BST::start() { + _should_run = true; + + _attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _battery_sub = orb_subscribe(ORB_ID(battery_status)); + + set_address(0x00); // General call address + + work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0); +} + +void BST::cycle_trampoline(void *arg) { + reinterpret_cast(arg)->cycle(); +} + +void BST::cycle() { + if (_should_run) { + bool updated = false; + + orb_check(_attitude_sub, &updated); + if (updated) { + vehicle_attitude_s att; + orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att); + BSTPacket bst_att = {}; + bst_att.type = 0x1E; + bst_att.payload.roll = swap_int32(att.roll * 10000); + bst_att.payload.pitch = swap_int32(att.pitch * 10000); + bst_att.payload.yaw = swap_int32(att.yaw * 10000); + send_packet(bst_att); + } + + updated = false; + orb_check(_gps_sub, &updated); + if (updated) { + vehicle_gps_position_s gps; + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); + if (gps.fix_type >= 3 && gps.eph < 50.0f) { + BSTPacket gps_pos = {}; + gps_pos.type = 0x02; + gps_pos.payload.lat = swap_int32(gps.lat); + gps_pos.payload.lon = swap_int32(gps.lon); + gps_pos.payload.alt = swap_int16(gps.alt / 1000 + 1000); + gps_pos.payload.gs = swap_int16(gps.vel_m_s * 360.0f); + gps_pos.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F); + gps_pos.payload.sats = gps.satellites_used; + send_packet(gps_pos); + } + } + + work_queue(LPWORK, &_work, BST::cycle_trampoline, this, _interval); + } +} + +uint8_t BST::crc8(uint8_t *data, size_t len) { + uint8_t crc = 0x00; + + while (len--) { + crc ^= *data++; + + for (int i = 0; i < 8; i++) { + crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1; + } + } + + return crc; +} + +} +} + +using namespace px4::bst; + +int bst_main(int argc, char *argv[]) { + if (argc < 2) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (g_bst) { + warnx("already running"); + exit(0); + } + + g_bst = new BST(DEFAULT_DEVICE); + + if (g_bst != nullptr && OK != g_bst->init()) { + delete g_bst; + g_bst = nullptr; + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (!g_bst) { + warnx("not running"); + exit(0); + } + + delete g_bst; + g_bst = nullptr; + warnx("stopped"); + + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (g_bst) { + warnx("is running"); + + } else { + warnx("is not running"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} diff --git a/src/drivers/bst/module.mk b/src/drivers/bst/module.mk new file mode 100644 index 0000000000..2e1f81720a --- /dev/null +++ b/src/drivers/bst/module.mk @@ -0,0 +1,9 @@ +# +# Black Sheep Telemetry driver +# + +MODULE_COMMAND = bst + +SRCS = bst.cpp + +MAXOPTIMIZATION = -Os From 2ae84ca171d1fe6edbbd679dc7368f44a1109357 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 23 Aug 2015 23:02:23 +0200 Subject: [PATCH 514/624] rc.sensors: run ms5611 driver on internal SPI bus only --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a7bc6b0815..39a1df70b0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -3,7 +3,7 @@ # Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers. # -if ms5611 start +if ms5611 -s start then fi From c08ab251ef17f5320634b4b6a7eb421609412932 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Aug 2015 17:25:58 +0200 Subject: [PATCH 515/624] bst: battery packet implemented --- src/drivers/bst/bst.cpp | 40 +++++++++++++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp index e21e268e04..f80ceabc63 100644 --- a/src/drivers/bst/bst.cpp +++ b/src/drivers/bst/bst.cpp @@ -98,6 +98,12 @@ struct BSTAttitude { int16_t yaw; }; +struct BSTBattery { + uint16_t voltage; + uint16_t current; + uint8_t capacity[3]; +}; + #pragma pack(pop) class BST : public device::I2C { @@ -271,21 +277,37 @@ void BST::cycle() { send_packet(bst_att); } + updated = false; + orb_check(_battery_sub, &updated); + if (updated) { + battery_status_s batt; + orb_copy(ORB_ID(battery_status), _battery_sub, &batt); + BSTPacket bst_batt = {}; + bst_batt.type = 0x08; + bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f); + bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f); + uint32_t discharged = batt.discharged_mah; + bst_batt.payload.capacity[0] = static_cast(discharged >> 16); + bst_batt.payload.capacity[1] = static_cast(discharged >> 8); + bst_batt.payload.capacity[2] = static_cast(discharged); + send_packet(bst_batt); + } + updated = false; orb_check(_gps_sub, &updated); if (updated) { vehicle_gps_position_s gps; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); if (gps.fix_type >= 3 && gps.eph < 50.0f) { - BSTPacket gps_pos = {}; - gps_pos.type = 0x02; - gps_pos.payload.lat = swap_int32(gps.lat); - gps_pos.payload.lon = swap_int32(gps.lon); - gps_pos.payload.alt = swap_int16(gps.alt / 1000 + 1000); - gps_pos.payload.gs = swap_int16(gps.vel_m_s * 360.0f); - gps_pos.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F); - gps_pos.payload.sats = gps.satellites_used; - send_packet(gps_pos); + BSTPacket bst_gps = {}; + bst_gps.type = 0x02; + bst_gps.payload.lat = swap_int32(gps.lat); + bst_gps.payload.lon = swap_int32(gps.lon); + bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000); + bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f); + bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F); + bst_gps.payload.sats = gps.satellites_used; + send_packet(bst_gps); } } From ec55a56191d19f52993365faa344bb8cb6e256e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Nov 2015 09:01:50 +0100 Subject: [PATCH 516/624] bst: move to cmake --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + src/drivers/bst/CMakeLists.txt | 43 +++++++++++++++++++++ src/drivers/bst/module.mk | 9 ----- 3 files changed, 44 insertions(+), 9 deletions(-) create mode 100644 src/drivers/bst/CMakeLists.txt delete mode 100644 src/drivers/bst/module.mk diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 62f96b12d4..9fafe851be 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -45,6 +45,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands diff --git a/src/drivers/bst/CMakeLists.txt b/src/drivers/bst/CMakeLists.txt new file mode 100644 index 0000000000..146f6f6ee0 --- /dev/null +++ b/src/drivers/bst/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 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 +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__bst + MAIN bst + COMPILE_FLAGS + -Os + SRCS + bst.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/bst/module.mk b/src/drivers/bst/module.mk deleted file mode 100644 index 2e1f81720a..0000000000 --- a/src/drivers/bst/module.mk +++ /dev/null @@ -1,9 +0,0 @@ -# -# Black Sheep Telemetry driver -# - -MODULE_COMMAND = bst - -SRCS = bst.cpp - -MAXOPTIMIZATION = -Os From 45c9ca5eb71aad1d4a3a294356a490493e456b3f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:33:01 +0100 Subject: [PATCH 517/624] Start BlackSheep telemetry by default --- ROMFS/px4fmu_common/init.d/rc.sensors | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 39a1df70b0..f02fb5c650 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -3,8 +3,20 @@ # Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers. # -if ms5611 -s start +if ver hwcmp PX4FMU_V1 then + if ms5611 start + then + fi +else + if ms5611 -s start + then + fi + + # Blacksheep telemetry + if bst start + then + fi fi if adc start From 02e7144d9fe3f6ba95b1c7071c883fc7aa6f7778 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:34:02 +0100 Subject: [PATCH 518/624] BST driver: Clean up I2C bus and stack --- src/drivers/bst/CMakeLists.txt | 1 + src/drivers/bst/bst.cpp | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/bst/CMakeLists.txt b/src/drivers/bst/CMakeLists.txt index 146f6f6ee0..7c312fc2e9 100644 --- a/src/drivers/bst/CMakeLists.txt +++ b/src/drivers/bst/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__bst MAIN bst + STACK 1200 COMPILE_FLAGS -Os SRCS diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp index f80ceabc63..c6bf008e50 100644 --- a/src/drivers/bst/bst.cpp +++ b/src/drivers/bst/bst.cpp @@ -50,7 +50,6 @@ #include #include -#define DEFAULT_DEVICE 1 // TODO use PX4 define for external I2C #define BST_DEVICE_PATH "/dev/bst0" static const char commandline_usage[] = "usage: bst start|status|stop"; @@ -346,7 +345,7 @@ int bst_main(int argc, char *argv[]) { exit(0); } - g_bst = new BST(DEFAULT_DEVICE); + g_bst = new BST(PX4_I2C_BUS_EXPANSION); if (g_bst != nullptr && OK != g_bst->init()) { delete g_bst; From 3f8507f61930e4656837b5474c169dbc571fdee5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 12:40:39 +0100 Subject: [PATCH 519/624] Add missing board config to px4_config.h --- src/platforms/px4_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 4417c5b894..e44fd4afce 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,6 +42,7 @@ #if defined(__PX4_NUTTX) #include +#include #elif defined (__PX4_POSIX) From a210b8ddbebc6a51087ac4b3df09af5242c9c4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Nov 2015 13:15:01 +0100 Subject: [PATCH 520/624] BST: Fix code style --- src/drivers/bst/bst.cpp | 104 +++++++++++++++++++++++++--------------- 1 file changed, 65 insertions(+), 39 deletions(-) diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp index c6bf008e50..9a8d55ac40 100644 --- a/src/drivers/bst/bst.cpp +++ b/src/drivers/bst/bst.cpp @@ -58,8 +58,10 @@ extern "C" __EXPORT int bst_main(int argc, char *argv[]); #define BST_ADDR 0x76 -namespace px4 { -namespace bst { +namespace px4 +{ +namespace bst +{ #pragma pack(push, 1) @@ -105,7 +107,8 @@ struct BSTBattery { #pragma pack(pop) -class BST : public device::I2C { +class BST : public device::I2C +{ public: BST(int bus); @@ -140,7 +143,8 @@ private: void cycle(); template - void send_packet(BSTPacket &packet) { + void send_packet(BSTPacket &packet) + { packet.length = sizeof(packet) - 1; // Length packet.crc = crc8(reinterpret_cast(&packet.type), sizeof(packet) - 2); @@ -148,58 +152,63 @@ private: } template - void send_packet(BSTPacket &packet_send, BSTPacket &packet_recv) { + void send_packet(BSTPacket &packet_send, BSTPacket &packet_recv) + { packet_send.length = sizeof(packet_send) - 1; // Length packet_send.crc = crc8(reinterpret_cast(&packet_send.type), sizeof(packet_send) - 2); - transfer(reinterpret_cast(&packet_send), sizeof(packet_send), reinterpret_cast(&packet_recv), sizeof(packet_recv)); + transfer(reinterpret_cast(&packet_send), sizeof(packet_send), reinterpret_cast(&packet_recv), + sizeof(packet_recv)); } static uint8_t crc8(uint8_t *data, size_t len); //! Byte swap unsigned short - uint16_t swap_uint16( uint16_t val ) + uint16_t swap_uint16(uint16_t val) { - return (val << 8) | (val >> 8 ); + return (val << 8) | (val >> 8); } //! Byte swap short - int16_t swap_int16( int16_t val ) + int16_t swap_int16(int16_t val) { - return (val << 8) | ((val >> 8) & 0xFF); + return (val << 8) | ((val >> 8) & 0xFF); } //! Byte swap unsigned int - uint32_t swap_uint32( uint32_t val ) + uint32_t swap_uint32(uint32_t val) { - val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF ); - return (val << 16) | (val >> 16); + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | (val >> 16); } //! Byte swap int - int32_t swap_int32( int32_t val ) + int32_t swap_int32(int32_t val) { - val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF ); - return (val << 16) | ((val >> 16) & 0xFFFF); + val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF); + return (val << 16) | ((val >> 16) & 0xFFFF); } }; static BST *g_bst = nullptr; BST::BST(int bus) : - I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR - #ifdef __PX4_NUTTX - ,100000 /* maximum speed supported */ - #endif - ) { + I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR +#ifdef __PX4_NUTTX + , 100000 /* maximum speed supported */ +#endif + ) +{ } -BST::~BST() { +BST::~BST() +{ _should_run = false; work_cancel(LPWORK, &_work); } -int BST::probe() { +int BST::probe() +{ int retries_prev = _retries; _retries = 3; @@ -208,26 +217,33 @@ int BST::probe() { dev_info_req.payload.cmd = 0x04; BSTPacket dev_info_reply = {}; send_packet(dev_info_req, dev_info_reply); + if (dev_info_reply.type != 0x05) { warnx("no devices found"); return -EIO; } + uint8_t *reply_raw = reinterpret_cast(&dev_info_reply); uint8_t crc_calc = crc8(reinterpret_cast(&dev_info_reply.type), dev_info_reply.length - 1); uint8_t crc_recv = reply_raw[dev_info_reply.length]; + if (crc_recv != crc_calc) { warnx("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc); return -EIO; } + dev_info_reply.payload.dev_name[dev_info_reply.payload.dev_name_len] = '\0'; - warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s", (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id), dev_info_reply.payload.dev_name); + warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s", + (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id), + dev_info_reply.payload.dev_name); _retries = retries_prev; return OK; } -int BST::init() { +int BST::init() +{ int ret; ret = I2C::init(); @@ -240,11 +256,13 @@ int BST::init() { return OK; } -void BST::start_trampoline(void *arg) { +void BST::start_trampoline(void *arg) +{ reinterpret_cast(arg)->start(); } -void BST::start() { +void BST::start() +{ _should_run = true; _attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -256,15 +274,18 @@ void BST::start() { work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0); } -void BST::cycle_trampoline(void *arg) { +void BST::cycle_trampoline(void *arg) +{ reinterpret_cast(arg)->cycle(); } -void BST::cycle() { +void BST::cycle() +{ if (_should_run) { bool updated = false; orb_check(_attitude_sub, &updated); + if (updated) { vehicle_attitude_s att; orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att); @@ -278,6 +299,7 @@ void BST::cycle() { updated = false; orb_check(_battery_sub, &updated); + if (updated) { battery_status_s batt; orb_copy(ORB_ID(battery_status), _battery_sub, &batt); @@ -294,9 +316,11 @@ void BST::cycle() { updated = false; orb_check(_gps_sub, &updated); + if (updated) { vehicle_gps_position_s gps; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); + if (gps.fix_type >= 3 && gps.eph < 50.0f) { BSTPacket bst_gps = {}; bst_gps.type = 0x02; @@ -314,18 +338,19 @@ void BST::cycle() { } } -uint8_t BST::crc8(uint8_t *data, size_t len) { - uint8_t crc = 0x00; +uint8_t BST::crc8(uint8_t *data, size_t len) +{ + uint8_t crc = 0x00; - while (len--) { - crc ^= *data++; + while (len--) { + crc ^= *data++; - for (int i = 0; i < 8; i++) { - crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1; - } - } + for (int i = 0; i < 8; i++) { + crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1; + } + } - return crc; + return crc; } } @@ -333,7 +358,8 @@ uint8_t BST::crc8(uint8_t *data, size_t len) { using namespace px4::bst; -int bst_main(int argc, char *argv[]) { +int bst_main(int argc, char *argv[]) +{ if (argc < 2) { errx(1, "missing command\n%s", commandline_usage); } From 1d05ad2cf179fb9a739f17a1e7db87c1a5809fea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 20:06:42 +0100 Subject: [PATCH 521/624] Allow OneShot PWM pulse --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index bb04245333..64699f8696 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -76,7 +76,7 @@ __BEGIN_DECLS /** * Lowest minimum PWM in us */ -#define PWM_LOWEST_MIN 900 +#define PWM_LOWEST_MIN 90 /** * Default minimum PWM in us From d97592e853e67105ac677cd9e602cd8b3253e8ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:09:47 +0100 Subject: [PATCH 522/624] Sensors: Use ADC handle correctly --- src/modules/sensors/sensors.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 935f612ea3..afb5930984 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -65,7 +65,6 @@ #include #include -//#include #include #include @@ -199,7 +198,7 @@ private: void rc_poll(); /* XXX should not be here - should be own driver */ - int _fd_adc; /**< ADC driver handle */ + DevHandle _h_adc; /**< ADC driver handle */ hrt_abstime _last_adc; /**< last time we took input from the ADC */ bool _task_should_exit; /**< if true, sensor task should exit */ @@ -489,7 +488,7 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _fd_adc(-1), + _h_adc(), _last_adc(0), _task_should_exit(true), @@ -1025,11 +1024,10 @@ int Sensors::adc_init() { - DevHandle h_adc; - DevMgr::getHandle(ADC0_DEVICE_PATH, h_adc); + DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc); - if (!h_adc.isValid()) { - warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, h_adc.getError()); + if (!_h_adc.isValid()) { + warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError()); return ERROR; } @@ -1619,7 +1617,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ struct adc_msg_s buf_adc[12]; /* read all channels available */ - int ret = px4_read(_fd_adc, &buf_adc, sizeof(buf_adc)); + int ret = _h_adc.read(&buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { @@ -2058,10 +2056,7 @@ Sensors::task_main() warnx("sensor initialization failed"); _sensors_task = -1; - if (_fd_adc >= 0) { - px4_close(_fd_adc); - _fd_adc = -1; - } + DevMgr::releaseHandle(_h_adc); return; } From 3c349236e11be7a428c1b4cc62f59496a5e9d8a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:19:31 +0100 Subject: [PATCH 523/624] Fix S.BUS compile error --- src/lib/rc/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index f5330b720d..b307fb6bfa 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -101,7 +101,7 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ #ifdef SBUS_SERIAL_PORT - ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + //ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); #endif } From d290487382bc838af2d01c0a627c291249edcf4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Dec 2015 06:55:52 +1100 Subject: [PATCH 524/624] circuit_breaker: prevent param fetch failure from disabling safety switch if the param get failed then an uninitialised stack variable was used for the safety disable on boot. In ArduPilot that value happened to equal the correct magic due to stack passing from caller. This forced safety off on boot --- src/modules/systemlib/circuit_breaker.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ba9e9f6219..c5a94a43bd 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -48,9 +48,10 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) { - int32_t val; - (void)PX4_PARAM_GET_BYNAME(breaker, &val); - - return (val == magic); + int32_t val = -1; + if (PX4_PARAM_GET_BYNAME(breaker, &val) == 0 && val == magic) { + return true; + } + return false; } From cb5e384105354b95d577f662500df479a73df639 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 4 Dec 2015 11:28:26 -1000 Subject: [PATCH 525/624] Ran Astyle --- src/modules/systemlib/circuit_breaker.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index c5a94a43bd..c980ee757c 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -49,9 +49,11 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) { int32_t val = -1; + if (PX4_PARAM_GET_BYNAME(breaker, &val) == 0 && val == magic) { return true; } + return false; } From 73e26804daa58e0e35d002372d02c7ce5253e19f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:42:31 +0100 Subject: [PATCH 526/624] Fix HRT tests --- src/systemcmds/tests/test_hrt.c | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index fe3c58af6a..c88603e745 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -88,8 +88,8 @@ extern uint16_t ppm_buffer[]; extern unsigned ppm_decoded_channels; -extern uint16_t ppm_edge_history[]; -extern uint16_t ppm_pulse_history[]; +// extern uint16_t ppm_edge_history[]; +// extern uint16_t ppm_pulse_history[]; int test_ppm(int argc, char *argv[]) { @@ -102,17 +102,17 @@ int test_ppm(int argc, char *argv[]) printf(" %u\n", ppm_buffer[i]); } - printf("edges\n"); + // printf("edges\n"); - for (i = 0; i < 32; i++) { - printf(" %u\n", ppm_edge_history[i]); - } + // for (i = 0; i < 32; i++) { + // printf(" %u\n", ppm_edge_history[i]); + // } - printf("pulses\n"); + // printf("pulses\n"); - for (i = 0; i < 32; i++) { - printf(" %u\n", ppm_pulse_history[i]); - } + // for (i = 0; i < 32; i++) { + // printf(" %u\n", ppm_pulse_history[i]); + // } fflush(stdout); #else From c0ef1a9dcb0816a071195c2fcbefbccc126fa81d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 21 Nov 2015 04:01:55 -1000 Subject: [PATCH 527/624] Move uavcan bin files to ROMFS based on naming convention --- .travis.yml | 3 +-- Tools/uavcan_copy.sh | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+), 2 deletions(-) create mode 100755 Tools/uavcan_copy.sh diff --git a/.travis.yml b/.travis.yml index f2a72b9ff5..48e674881c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -102,9 +102,8 @@ script: - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make + - ../Tools/uavcan_copy.sh - cd .. - - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default diff --git a/Tools/uavcan_copy.sh b/Tools/uavcan_copy.sh new file mode 100755 index 0000000000..34969cf8fe --- /dev/null +++ b/Tools/uavcan_copy.sh @@ -0,0 +1,33 @@ +#!/bin/bash +UAVCANFW=../ROMFS/px4fmu_common/uavcan/fw +echo Removing : $UAVCANFW +rm -fr $UAVCANFW +for f in $(find firmware -type f -name "*.*.bin") +do + UAVCAN_NAME=$(echo $f | cut -d"/" -f2 | cut -d. -f1-4 | cut -d- -f1-2) + UAVCAN_HW=$(echo $f cut -d/ -f2 | cut -d. -f1-4 | cut -d- -f3) + DST=$(echo $f | cut -d. -f3-7 | cut -d- -f1,2,3).$(echo $f | cut -d. -f6,7) + # deal with legacy non conforming naming + if [[ ${DST:(-7)} == bin.bin ]] + then + echo " WARNING: Improper name format!!!!!!!!! $f see should be -...bin" + DST=${DST%???} + fi + if [ ${#DST} -le 28 ] + echo Processing file: $f + then + if [ -d "${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}" ] + then + echo " ERROR: name colision directory ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW} exits!" + exit 2 + fi + echo " Creating Directory ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}" + mkdir -p ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW} + echo " Copying $f to ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}/${DST}" + cp $f ${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}/${DST} + else + echo " ERROR: $DST is ${#DST} charaters and needs to be less than or equal to 28" + exit 1 + fi +done +exit 0 From 31a773e3cef4d618ddb9ea70309a3f8338cd6e1e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 21 Nov 2015 06:17:27 -1000 Subject: [PATCH 528/624] Use _ prefix o uavcan files in ROMFS , simplify upgrade --- Tools/uavcan_copy.sh | 7 +++--- src/modules/uavcan/uavcan_servers.cpp | 33 +++++++++++++-------------- src/modules/uavcan/uavcan_servers.hpp | 2 +- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/Tools/uavcan_copy.sh b/Tools/uavcan_copy.sh index 34969cf8fe..9d21e0b562 100755 --- a/Tools/uavcan_copy.sh +++ b/Tools/uavcan_copy.sh @@ -1,20 +1,21 @@ #!/bin/bash UAVCANFW=../ROMFS/px4fmu_common/uavcan/fw +ROMFS_REFIX=_ echo Removing : $UAVCANFW rm -fr $UAVCANFW for f in $(find firmware -type f -name "*.*.bin") do UAVCAN_NAME=$(echo $f | cut -d"/" -f2 | cut -d. -f1-4 | cut -d- -f1-2) UAVCAN_HW=$(echo $f cut -d/ -f2 | cut -d. -f1-4 | cut -d- -f3) - DST=$(echo $f | cut -d. -f3-7 | cut -d- -f1,2,3).$(echo $f | cut -d. -f6,7) + DST=${ROMFS_REFIX}$(echo $f | cut -d. -f3-7 | cut -d- -f1,2,3).$(echo $f | cut -d. -f6,7) # deal with legacy non conforming naming if [[ ${DST:(-7)} == bin.bin ]] then echo " WARNING: Improper name format!!!!!!!!! $f see should be -...bin" - DST=${DST%???} + DST=${DST%????} fi + echo Processing file: $f Length:${#DST} if [ ${#DST} -le 28 ] - echo Processing file: $f then if [ -d "${UAVCANFW}/${UAVCAN_NAME}/${UAVCAN_HW}" ] then diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 318940eb64..2da32688b0 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -886,14 +886,15 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat /fs/microsd/fw - /c - - - /1a2b3c4d.bin cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/1a2b3c4d.bin) - - /org.pixhawk.nodename-v1 device directory for org.pixhawk.nodename-v1 - - - /1.0 version directory for hardware 1.0 - - - /1.1 version directory for hardware 1.1 - - - - /1a2b3c4d.bin firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1 - - /com.example.othernode-v3 device directory for com.example.othernode-v3 - - - /1.0 version directory for hardawre 1.0 - - - - /deadbeef.bin firmware file for com.example.othernode-v3, hardware version 1.0 + - /nodename-v1-1.0.25d0137d.bin cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/nodename-v1-1.0.25d0137d.bin) + - /othernode-v3-1.6.25d0137d.bin cache file (copy of /fs/microsd/fw/com.example.othernode-v3/1.6/othernode-v3-1.6.25d0137d.bin) + - /org.pixhawk.nodename-v1 device directory for org.pixhawk.nodename-v1 + - - /1.0 version directory for hardware 1.0 + - - /1.1 version directory for hardware 1.1 + - - - /nodename-v1-1.0.25d0137d.bin firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1 + - /com.example.othernode-v3 device directory for com.example.othernode-v3 + - - /1.0 version directory for hardawre 1.0 + - - - /othernode-v3-1.6.25d0137d.bin firmware file for com.example.othernode-v3, hardware version 1.6 The ROMFS directory structure is the same, but located at /etc/uavcan/fw @@ -1037,7 +1038,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat continue; } - if (!memcmp(&fw_dirent->d_name[sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1], src_fw_dirent->d_name, fw_len)) { + if (!memcmp(&fw_dirent->d_name, src_fw_dirent->d_name, fw_len)) { /* * Exact match between SD card filename and ROMFS filename; must be the same version * so don't bother deleting and rewriting it. @@ -1048,14 +1049,13 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; if (dstpath_fw_len > maxlen) { // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator - warnx("unlink: path '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name); } else { - // File name starts with "romfs_", delete it. + // File name starts with "_", delete it. dstpath[dstpath_ver_len] = '/'; memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1); unlink(dstpath); - - warnx("unlink: removed '%s/%s%s'", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + warnx("unlink: removed '%s/%s'", dstpath, fw_dirent->d_name); } } else { // User file, don't copy firmware @@ -1068,20 +1068,19 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat // If we need to, copy the file from ROMFS to the SD card if (copy_fw) { size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len; - size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + size_t dstpath_fw_len = dstpath_ver_len + fw_len; if (srcpath_fw_len > maxlen) { warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); } else if (dstpath_fw_len > maxlen) { - warnx("copy: dstpath '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, src_fw_dirent->d_name); + warnx("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name); } else { // All OK, make the paths and copy the file srcpath[srcpath_ver_len] = '/'; memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); dstpath[dstpath_ver_len] = '/'; - memcpy(&dstpath[dstpath_ver_len + 1], UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX)); - memcpy(&dstpath[dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX)], src_fw_dirent->d_name, fw_len + 1); + memcpy(&dstpath[dstpath_ver_len], src_fw_dirent->d_name, fw_len + 1); rv = copyFw(dstpath, srcpath); if (rv != 0) { diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 3cf4cbb4da..30fb3f5255 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -70,7 +70,7 @@ #define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" #define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" #define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw" -#define UAVCAN_ROMFS_FW_PREFIX "romfs_" +#define UAVCAN_ROMFS_FW_PREFIX "_" #define UAVCAN_MAX_PATH_LENGTH (128 + 40) #define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" From 31241c9b49372509ad8aee326ebeac1ad36bd2fb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 23 Nov 2015 13:56:07 -1000 Subject: [PATCH 529/624] Fixed off-by-one errors --- src/modules/uavcan/uavcan_servers.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 2da32688b0..701390de86 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -1046,7 +1046,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat copy_fw = false; } else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) { size_t fw_len = strlen(fw_dirent->d_name); - size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + size_t dstpath_fw_len = dstpath_ver_len + fw_len; if (dstpath_fw_len > maxlen) { // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name); @@ -1080,7 +1080,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); dstpath[dstpath_ver_len] = '/'; - memcpy(&dstpath[dstpath_ver_len], src_fw_dirent->d_name, fw_len + 1); + memcpy(&dstpath[dstpath_ver_len +1], src_fw_dirent->d_name, fw_len + 1); rv = copyFw(dstpath, srcpath); if (rv != 0) { From daccfc6c40ecbdbe99c9cddfbc750f59c9580d9a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 23 Nov 2015 13:56:39 -1000 Subject: [PATCH 530/624] Fixed repeated output --- src/modules/uavcan/uavcan_servers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 701390de86..ce80a88fab 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -1055,7 +1055,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat dstpath[dstpath_ver_len] = '/'; memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1); unlink(dstpath); - warnx("unlink: removed '%s/%s'", dstpath, fw_dirent->d_name); + warnx("unlink: removed '%s'", dstpath); } } else { // User file, don't copy firmware From bc89f6faefd5eaa6321b3de9ac18d27963bac67f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 23 Nov 2015 13:58:55 -1000 Subject: [PATCH 531/624] todo:Added todo for using strcat --- src/modules/uavcan/uavcan_servers.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ce80a88fab..1b4a7e1056 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -907,6 +907,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat In each version directory, we remove any files with a name starting with "romfs_" in the corresponding directory on the SD card that don't match the bundled firmware filename; if the directory is empty after that process, we copy the bundled firmware. + todo:This code would benefit from the use of strcat. */ const size_t maxlen = UAVCAN_MAX_PATH_LENGTH; const size_t sd_path_len = strlen(sd_path); From 5d422f5c300e4b74642f614ed5e8597b88dc11c1 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 23 Nov 2015 14:10:23 -1000 Subject: [PATCH 532/624] Updated comments --- src/modules/uavcan/uavcan_servers.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 1b4a7e1056..5e51317737 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -897,6 +897,8 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat - - - /othernode-v3-1.6.25d0137d.bin firmware file for com.example.othernode-v3, hardware version 1.6 The ROMFS directory structure is the same, but located at /etc/uavcan/fw + Files located there are prefixed with _ to identify them a comming from the rom + file system. We iterate over all device directories in the ROMFS base directory, and create corresponding device directories on the SD card if they don't already exist. @@ -904,10 +906,12 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat In each device directory, we iterate over each version directory and create a corresponding version directory on the SD card if it doesn't already exist. - In each version directory, we remove any files with a name starting with "romfs_" + In each version directory, we remove any files with a name starting with "_" in the corresponding directory on the SD card that don't match the bundled firmware filename; if the directory is empty after that process, we copy the bundled firmware. + todo:This code would benefit from the use of strcat. + */ const size_t maxlen = UAVCAN_MAX_PATH_LENGTH; const size_t sd_path_len = strlen(sd_path); From 923a43b67fa1f3af99e2d95c2ecf4b1328467beb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 3 Dec 2015 15:45:50 -1000 Subject: [PATCH 533/624] Support for Rev 5 of Bootloader --- Tools/px_uploader.py | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 3cdd9d1a6f..4755eb24ff 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -148,6 +148,7 @@ class uploader(object): OK = b'\x10' FAILED = b'\x11' INVALID = b'\x13' # rev3+ + BAD_SILICON_REV = b'\x14' # rev5+ # command bytes NOP = b'\x00' # guaranteed to be discarded by the bootloader @@ -162,11 +163,14 @@ class uploader(object): GET_SN = b'\x2b' # rev4+ , get a word from SN area GET_CHIP = b'\x2c' # rev5+ , get chip version SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay + GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII + MAX_DES_LENGTH = 20 + REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision - BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 4 # maximum supported bootloader protocol + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 5 # maximum supported bootloader protocol INFO_BOARD_ID = b'\x02' # board type INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes @@ -235,12 +239,16 @@ class uploader(object): if (self.__recv() != self.INSYNC): #print("unexpected 0x%x instead of INSYNC" % ord(c)) return False; - - if (self.__recv() != self.OK): + c = self.__recv() + if (c == self.BAD_SILICON_REV): + raise NotImplementedError() + if (c != self.OK): #print("unexpected 0x%x instead of OK" % ord(c)) return False return True + except NotImplementedError: + raise RuntimeError("Programing not supported for this version of silicon!\n See https://pixhawk.org/help/errata") except RuntimeError: #timeout, no response yet return False @@ -274,6 +282,14 @@ class uploader(object): value = self.__recv_int() self.__getSync() return value + # send the GET_CHIP command + def __getCHIPDes(self): + self.__send(uploader.GET_CHIP_DES + uploader.EOC) + length = self.__recv_int() + value = self.__recv(length) + self.__getSync() + peices = value.split(",") + return peices def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: @@ -467,6 +483,12 @@ class uploader(object): print(binascii.hexlify(x).decode('Latin-1'), end='') # show user print('') print("chip: %08x" % self.__getCHIP()) + if (self.bl_rev >= 5): + des = self.__getCHIPDes() + if (len(des) == 2): + print("family: %s" % des[0]) + print("revision: %s" % des[1]) + print("flash %d" % self.fw_maxsize) except Exception: # ignore bad character encodings pass From 452d724cfe6e0d3b52ef1502f0c261a96bc13d34 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 4 Dec 2015 09:07:37 -1000 Subject: [PATCH 534/624] Reved FMU Side px4pio uploader to accept Rev 5 and below bootloader --- src/drivers/px4io/uploader.h | 37 +++++++++++++++++++++--------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index def10c0392..8b5b83a0f0 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -54,22 +54,29 @@ public: private: enum { - PROTO_NOP = 0x00, - PROTO_OK = 0x10, - PROTO_FAILED = 0x11, - PROTO_INSYNC = 0x12, - PROTO_EOC = 0x20, - PROTO_GET_SYNC = 0x21, - PROTO_GET_DEVICE = 0x22, - PROTO_CHIP_ERASE = 0x23, - PROTO_CHIP_VERIFY = 0x24, - PROTO_PROG_MULTI = 0x27, - PROTO_READ_MULTI = 0x28, - PROTO_GET_CRC = 0x29, - PROTO_REBOOT = 0x30, + PROTO_NOP = 0x00, + PROTO_OK = 0x10, + PROTO_FAILED = 0x11, + PROTO_INSYNC = 0x12, + PROTO_INVALID = 0x13, + PROTO_BAD_SILICON_REV = 0x14, + PROTO_EOC = 0x20, + PROTO_GET_SYNC = 0x21, + PROTO_GET_DEVICE = 0x22, + PROTO_CHIP_ERASE = 0x23, + PROTO_CHIP_VERIFY = 0x24, + PROTO_PROG_MULTI = 0x27, + PROTO_READ_MULTI = 0x28, + PROTO_GET_CRC = 0x29, + PROTO_GET_OTP = 0x2a, + PROTO_GET_SN = 0x2b, + PROTO_GET_CHIP = 0x2c, + PROTO_SET_DELAY = 0x2d, + PROTO_GET_CHIP_DES = 0x2e, + PROTO_REBOOT = 0x30, - INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 4, /**< supported bootloader protocol */ + INFO_BL_REV = 1, /**< bootloader protocol revision */ + BL_REV = 5, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ From 9d4c7e9af62ea1fca90fd2d21110c33ae152eb48 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 4 Dec 2015 11:37:06 -1000 Subject: [PATCH 535/624] Ran Astyle --- src/drivers/px4io/uploader.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 8b5b83a0f0..2855ea4ad6 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -76,7 +76,7 @@ private: PROTO_REBOOT = 0x30, INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 5, /**< supported bootloader protocol */ + BL_REV = 5, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ From 4b9cad7129b63a8f32f12d5104d3184870eaaf88 Mon Sep 17 00:00:00 2001 From: Emmanuel Roussel Date: Thu, 3 Dec 2015 13:40:17 +0100 Subject: [PATCH 536/624] Added configuration file for coaxial helicopter --- ROMFS/px4fmu_common/init.d/15001_coax_heli | 65 ++++++++++++++++++++++ ROMFS/px4fmu_common/mixers/coax.main.mix | 33 +++++++++++ 2 files changed, 98 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/15001_coax_heli create mode 100644 ROMFS/px4fmu_common/mixers/coax.main.mix diff --git a/ROMFS/px4fmu_common/init.d/15001_coax_heli b/ROMFS/px4fmu_common/init.d/15001_coax_heli new file mode 100644 index 0000000000..97513b7049 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15001_coax_heli @@ -0,0 +1,65 @@ +#!nsh +# +# @name Coaxial Helicopter (such as Esky Lama v4 or Esky Big Lama) +# +# @type Coaxial Helicopter +# +# @output MAIN1 Left swashplate servomotor, pitch axis +# @output MAIN2 Right swashplate servomotor, roll axis +# @output MAIN3 Upper rotor (CCW) +# @output MAIN4 Lower rotor (CW) +# +# @maintainer Emmanuel Roussel +# + +set VEHICLE_TYPE mc + +if [ $AUTOCNF == yes ] +then + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.35 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.25 + + param set NAV_ACC_RAD 2.0 + param set RTL_RETURN_ALT 30.0 + param set RTL_DESCEND_ALT 10.0 + + param set PWM_DISARMED 900 + param set PWM_MIN 1075 + param set PWM_MAX 1950 + + param set MC_ROLL_P 6.5 + param set MC_ROLLRATE_P 0.17 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.005 + param set MC_ROLLRATE_FF 0 + param set MC_PITCH_P 6.5 + param set MC_PITCHRATE_P 0.17 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCHRATE_D 0.005 + param set MC_PITCHRATE_FF 0 + param set MC_YAW_P 2 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.1 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0 +fi + +set MIXER coax +# use PWM parameters for throttle channel +set PWM_OUT 34 +set PWM_RATE 400 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + diff --git a/ROMFS/px4fmu_common/mixers/coax.main.mix b/ROMFS/px4fmu_common/mixers/coax.main.mix new file mode 100644 index 0000000000..96d9afdf1a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/coax.main.mix @@ -0,0 +1,33 @@ +Coaxial helicopter mixer +- Two servomotors act on the swashplate (90° angle on the swashplate, decoupled effect on roll and pitch). +- No collective pitch. +- One motor per rotor. +=========================== + +Left swashplate servomotor, pitch axis +------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Right swashplate servomotor, roll axis +------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Upper rotor (CCW) +Mixing between yaw and thrust +------------- +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Lower rotor (CW) +Mixing between yaw and thrust +------------- +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 From 55cd24a4faeda1f273e15d80e12be2d3fe98ac66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:38:24 +0100 Subject: [PATCH 537/624] Runway lib: Do not attempt to build params file --- src/lib/runway_takeoff/CMakeLists.txt | 1 - src/lib/runway_takeoff/runway_takeoff_params.c | 4 ---- 2 files changed, 5 deletions(-) diff --git a/src/lib/runway_takeoff/CMakeLists.txt b/src/lib/runway_takeoff/CMakeLists.txt index 7938279a0e..f9f51dd543 100644 --- a/src/lib/runway_takeoff/CMakeLists.txt +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_module( -Os SRCS RunwayTakeoff.cpp - runway_takeoff_params.c DEPENDS platforms__common ) diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index f5a1017f96..62c5e54fac 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -39,10 +39,6 @@ * @author Andreas Antener */ -#include - -#include - /** * Enable or disable runway takeoff with landing gear * From 2e10351fcc84141ed2b26862bbe1082bd2ca48b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:40:22 +0100 Subject: [PATCH 538/624] Fix HRT abstraction for Mac OS --- src/platforms/posix/px4_layer/drv_hrt.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index e4839f0ac7..3bb8fa129f 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -86,6 +86,10 @@ static void hrt_unlock(void) #include #define CLOCK_REALTIME 0 +#ifndef CLOCK_MONOTONIC +#define CLOCK_MONOTONIC 1 +#endif + int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) { struct timeval now; From e7e714f6dfc8ffb4e0572bc80e492530bd7218a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:55:10 +0100 Subject: [PATCH 539/624] Unit tests: Ignore all test executables --- unittests/.gitignore | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/unittests/.gitignore b/unittests/.gitignore index aa316941f4..d5bf4e2412 100644 --- a/unittests/.gitignore +++ b/unittests/.gitignore @@ -4,5 +4,13 @@ mixer_test sf0x_test sbus2_test autodeclination_test -st24_test +rc_input_test +conversion_test +param_test +*.a sample_unittest +CMakeCache.txt +CMakeFiles +CTestTestfile.cmake +cmake_install.cmake +Makefile From 7158a154cc3f545d30fa59fb57110ac011682f51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:55:42 +0100 Subject: [PATCH 540/624] Unit tests: Fix build for current temporary build setup. Needs conversion to proper CMake proejct --- unittests/CMakeLists.txt | 46 +++++++++++++++++++++------------------- 1 file changed, 24 insertions(+), 22 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 4285520b0c..684ac9a6de 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -37,7 +37,7 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g") #set(CMAKE_INCLUDE_SYSTEM_FLAG_C "-isystem" ) #set(CMAKE_INCLUDE_SYSTEM_FLAG_CXX "-isystem" ) -set(GTEST_DIR gtest) +set(GTEST_DIR googletest) add_subdirectory(${GTEST_DIR}) include_directories(${GTEST_DIR}/include) @@ -48,10 +48,15 @@ include_directories(${PX_SRC}/modules) include_directories(${PX_SRC}/modules/uORB) include_directories(${PX_SRC}/lib) include_directories(${PX_SRC}/drivers) +include_directories(${PX_SRC}/lib/DriverFramework/framework/include) +include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules) +include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules/uORB) +include_directories(${PX_SRC}/../build_posix_sitl_default/src) +include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules/param) include_directories(${PX_SRC}/platforms) include_directories(${PX_SRC}/platforms/posix/include) include_directories(${PX_SRC}/platforms/posix/px4_layer) -include_directories(${PX_SRC}/drivers/device ) +include_directories(${PX_SRC}/drivers/device) add_definitions(-D__EXPORT=) @@ -101,13 +106,15 @@ add_library( px4_platform ${PX_SRC}/platforms/posix/work_queue/dq_addlast.c ${PX_SRC}/platforms/posix/px4_layer/lib_crc32.c ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c + ${PX_SRC}/platforms/posix/px4_layer/px4_sem.cpp ${PX_SRC}/drivers/device/device_posix.cpp ${PX_SRC}/drivers/device/vdev.cpp ${PX_SRC}/drivers/device/vfile.cpp ${PX_SRC}/drivers/device/vdev_posix.cpp ${PX_SRC}/drivers/device/i2c_posix.cpp ${PX_SRC}/drivers/device/sim.cpp - ${PX_SRC}/drivers/device/ringbuffer.cpp + ${PX_SRC}/drivers/device/ringbuffer.cpp + ${PX_SRC}/../build_posix_sitl_default/src/modules/param/px4_parameters.c ) #target_include_directories( px4_platform PUBLIC ${PX_SRC}/platforms ) @@ -146,14 +153,9 @@ target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) # st24_test -add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) -target_link_libraries( st24_test px4_platform ) -add_gtest(st24_test) - -# sumd_test -add_executable(sumd_test sumd_test.cpp hrt.cpp ${PX_SRC}/lib/rc/sumd.c) -target_link_libraries( sumd_test px4_platform ) -add_gtest(sumd_test) +add_executable(rc_input_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c sumd_test.cpp ${PX_SRC}/lib/rc/sumd.c) +target_link_libraries(rc_input_test px4_platform) +add_gtest(rc_input_test) # sf0x_test add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) @@ -172,15 +174,15 @@ target_link_libraries( param_test px4_platform ) add_gtest(param_test) # uorb test -add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp - uorb_unittests/uORBCommunicatorMock.cpp - uorb_unittests/uORBCommunicatorMockLoopback.cpp - ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp - ${PX_SRC}/modules/uORB/uORBManager_posix.cpp - ${PX_SRC}/modules/uORB/objects_common.cpp - ${PX_SRC}/modules/uORB/uORBUtils.cpp - ${PX_SRC}/modules/uORB/uORB.cpp - ) -target_link_libraries( uorb_tests px4_platform ) +#add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp +# uorb_unittests/uORBCommunicatorMock.cpp +# uorb_unittests/uORBCommunicatorMockLoopback.cpp +# ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp +# ${PX_SRC}/modules/uORB/uORBManager_posix.cpp +# ${PX_SRC}/modules/uORB/objects_common.cpp +# ${PX_SRC}/modules/uORB/uORBUtils.cpp +# ${PX_SRC}/modules/uORB/uORB.cpp +# ) +#target_link_libraries( uorb_tests px4_platform ) -add_gtest(uorb_tests) +#add_gtest(uorb_tests) From 29621ce0ed99473f921d75b96aac71fd4672bbc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:57:54 +0100 Subject: [PATCH 541/624] mixer test: Fix build warning --- unittests/mixer_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 5be4cb032b..171bad252f 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -7,6 +7,6 @@ TEST(MixerTest, Mixer) { - char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; - ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; + const char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; + ASSERT_EQ(test_mixer(3, (char **)args), 0) << "IO_pass.mix failed"; } From 981ca02001a320b7bba7e5cef54f06a5be03450d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:58:33 +0100 Subject: [PATCH 542/624] st24 test: Be less verbose --- unittests/st24_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index c7fded68d4..5b6a22fd27 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -12,12 +12,12 @@ TEST(ST24Test, ST24) { const char *filepath = "testdata/st24_data.txt"; - warnx("loading data from: %s", filepath); + //warnx("loading data from: %s", filepath); FILE *fp; fp = fopen(filepath, "rt"); - //ASSERT_TRUE(fp); + ASSERT_TRUE(fp); float f; unsigned x; From fb495b6594de3df473e20a6a2ded65fb381b1927 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:58:44 +0100 Subject: [PATCH 543/624] sumd test: Be less verbose --- unittests/sumd_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index 65c944676c..40be7a9d8f 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -12,7 +12,7 @@ TEST(SUMDTest, SUMD) { const char *filepath = "testdata/sumd_data.txt"; - warnx("loading data from: %s", filepath); + //warnx("loading data from: %s", filepath); FILE *fp; From acd9e7571f060b751ddc230888b6cf3ff38d7d57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 12:00:06 +0100 Subject: [PATCH 544/624] Update test run file --- unittests/run_tests.sh | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/unittests/run_tests.sh b/unittests/run_tests.sh index ff5ee509ac..85aba9bd88 100755 --- a/unittests/run_tests.sh +++ b/unittests/run_tests.sh @@ -1,6 +1,14 @@ #!/bin/sh +cmake . make clean -make all -./mixer_test -./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt \ No newline at end of file +make all -j4 + +set -e + +#./param_test +./conversion_test +./autodeclination_test +./mixer_test > /dev/null +./sbus2_test +./rc_input_test From 292688b3717a2f668e81f9a693437c8e7d353f4e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 12:00:22 +0100 Subject: [PATCH 545/624] Re-enable unit tests in Travis --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 48e674881c..209bcc8795 100644 --- a/.travis.yml +++ b/.travis.yml @@ -99,6 +99,8 @@ script: - arm-none-eabi-gcc --version - echo 'Building POSIX Firmware..' && make posix_sitl_default - echo 'Running Tests..' && make posix_sitl_default test + - echo 'Running Unittests..' && cd unittests && ./run_tests.sh + - cd .. - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make From 314bb49bcef6bd760ab08c4cdec71c9f320a28f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 12:19:24 +0100 Subject: [PATCH 546/624] Unit tests: Workaround for driver framework. Now unit tests are fully enabled --- src/drivers/device/vdev.cpp | 2 ++ unittests/CMakeLists.txt | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 330447086c..9e2c39062f 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -548,6 +548,7 @@ void VDev::showDevices() pthread_mutex_unlock(&devmutex); +#ifndef __PX4_UNIT_TESTS PX4_INFO("DF Devices:"); const char *dev_path; unsigned int index = 0; @@ -561,6 +562,7 @@ void VDev::showDevices() PX4_INFO(" %s", dev_path); } } while (i == 0); +#endif } void VDev::showTopics() diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 684ac9a6de..c7e62a3535 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -26,9 +26,9 @@ include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall") elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall") else() message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() From d9d90881a762846c2a7c5874c0c15da1e23537f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 13:42:38 +0100 Subject: [PATCH 547/624] VDev: Code style fix --- src/drivers/device/vdev.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 9e2c39062f..cdf01d0ea5 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -562,6 +562,7 @@ void VDev::showDevices() PX4_INFO(" %s", dev_path); } } while (i == 0); + #endif } From 03babedaf520325e8e374ef6316bc3449ff05a9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 23:27:55 +0100 Subject: [PATCH 548/624] sensors: Be less verbose --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index afb5930984..40a7fffd17 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1291,7 +1291,7 @@ Sensors::parameter_update_poll(bool forced) } int id = h.ioctl(DEVIOCGDEVICEID, 0); - PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + //PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { From d45634a308d39ec9c108955bb7249896e84069b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 23:51:04 +0100 Subject: [PATCH 549/624] Sensors: Do not run an IOCTL only used for printing --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 40a7fffd17..a5c4efb94e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1290,7 +1290,7 @@ Sensors::parameter_update_poll(bool forced) continue; } - int id = h.ioctl(DEVIOCGDEVICEID, 0); + //int id = h.ioctl(DEVIOCGDEVICEID, 0); //PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); /* if the calibration is for this device, apply it */ From b44cf06ace2bcae3e26fc55d566ceff831d96b4f Mon Sep 17 00:00:00 2001 From: Peter Duerr Date: Fri, 4 Dec 2015 14:05:00 +0900 Subject: [PATCH 550/624] Fix pointer arithmetic in EKF logging * sizeof returns byte, but buf.estimator_status.states and buf.estimator_status.covariances are float pointers --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f1648037c0..0fb8254a92 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1793,7 +1793,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_EST1_MSG; unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); - memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); + memcpy(&(log_msg.body.log_EST1.s), ((char*)buf.estimator_status.states) + maxcopy0, maxcopy1); LOGBUFFER_WRITE_AND_COUNT(EST1); log_msg.msg_type = LOG_EST2_MSG; @@ -1805,7 +1805,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_EST3_MSG; unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov); memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov)); - memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3); + memcpy(&(log_msg.body.log_EST3.cov), ((char*)buf.estimator_status.covariances) + maxcopy2, maxcopy3); LOGBUFFER_WRITE_AND_COUNT(EST3); } From 4b321dddc1880951fdc0de49a49a773f06f81b49 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 7 Dec 2015 08:45:36 +0100 Subject: [PATCH 551/624] Use a more specific (and shorter) airframe name for QGC. --- ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad index 4b8191f557..fabab37231 100644 --- a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad @@ -1,6 +1,6 @@ #!nsh # -# @name Generic AAERT tailplane airframe with Quad VTOL. +# @name Fun Cub Quad VTOL. # # @type Standard VTOL # From 38bebe2d6683f17e70a471ee2eacb4eefbd02711 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 09:37:46 +0100 Subject: [PATCH 552/624] Master: Less verbosity --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a5c4efb94e..5dba49879b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1362,8 +1362,8 @@ Sensors::parameter_update_poll(bool forced) continue; } - int id = h.ioctl(DEVIOCGDEVICEID, 0); - PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + // int id = h.ioctl(DEVIOCGDEVICEID, 0); + // PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { @@ -1443,8 +1443,8 @@ Sensors::parameter_update_poll(bool forced) continue; } - int id = h.ioctl(DEVIOCGDEVICEID, 0); - PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); + // int id = h.ioctl(DEVIOCGDEVICEID, 0); + // PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id); /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { From 24a176b135dfa3b335c860b7030820299d2325f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 11:38:28 +0100 Subject: [PATCH 553/624] XRacer: Force mixer out of the way --- ROMFS/px4fmu_common/init.d/rcS | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a1105ca2a4..1c04627982 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -238,6 +238,13 @@ then fi unset MODE + # + # Wipe incompatible settings for boards not having two outputs + if ver hwcmp PX4FMU_V4 + then + set MIXER_AUX none + fi + # # Override parameters from user configuration file # From ca32916d8e00c963cb7bcb2b60dbccc0791163f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 11:38:59 +0100 Subject: [PATCH 554/624] PWM out: Allow OneShot PWM --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 64699f8696..fd20814b7d 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -101,7 +101,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 950 +#define PWM_LOWEST_MAX 200 /** * Do not output a channel with this value From 87340f728bc54774c4433e76c1b070d0095395b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:18:15 +0100 Subject: [PATCH 555/624] RC: Better reporting --- msg/rc_channels.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index b2e08d864a..6b30794e00 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -26,3 +26,4 @@ uint8 channel_count # Number of valid channels int8[20] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout +uint32 frame_drop_count # Number of dropped frames From 3c55ece28ddd1be7217efb81baf62e5328c0cd3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:18:33 +0100 Subject: [PATCH 556/624] FMU driver: Better RC reporting --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 28abf37ff2..af80e66682 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -838,7 +838,7 @@ PX4FMU::cycle() _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0; _rc_in.rc_failsafe = false; _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_lost_frame_count = sbus_dropped_frames(); _rc_in.rc_total_frame_count = 0; rc_updated = true; From 1754816358f579ab342c73d7206add39a634dd26 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:18:59 +0100 Subject: [PATCH 557/624] IO driver: Better RC reporting --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2cabe98a23..9db91d28b8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1844,7 +1844,7 @@ int PX4IO::io_publish_pwm_outputs() { /* data we are going to fetch */ - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; multirotor_motor_limits_s motor_limits; outputs.timestamp = hrt_absolute_time(); From 976d541940d0d21533cadb65f24d6a18098176dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:19:56 +0100 Subject: [PATCH 558/624] sdlog2: Update RC logging --- src/modules/sdlog2/sdlog2.c | 3 ++- src/modules/sdlog2/sdlog2_messages.h | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0fb8254a92..57aec762e6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1677,11 +1677,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; - /* Copy only the first 8 channels of 14 */ + /* Copy only the first 12 channels of 18 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); log_msg.body.log_RC.rssi = buf.rc.rssi; log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; + log_msg.body.log_RC.frame_drop = buf.rc.frame_drop_count; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 6ccbec2fa9..86c2c58868 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -188,10 +188,11 @@ struct log_STAT_s { /* --- RC - RC INPUT CHANNELS --- */ #define LOG_RC_MSG 11 struct log_RC_s { - float channel[8]; + float channel[12]; uint8_t rssi; uint8_t channel_count; uint8_t signal_lost; + uint32_t frame_drop; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -533,7 +534,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), - LOG_FORMAT(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"), + LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From e41fa79b81e078c66f6a943d9b3d1c8114add326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:20:11 +0100 Subject: [PATCH 559/624] S.BUS: Expose frame drop variable --- src/lib/rc/sbus.c | 26 ++++++++++++++++++++++---- src/lib/rc/sbus.h | 5 +++++ 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index b307fb6bfa..5c1f0aeadc 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -53,6 +53,8 @@ #define SBUS_FRAMELOST_BIT 2 #define SBUS1_FRAME_DELAY 14000 +#define SBUS_SINGLE_CHAR_LEN_US ((1/((100000/10)) * 1000 * 1000) + /* Measured values with Futaba FX-30/R6108SB: -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) @@ -81,6 +83,12 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; +unsigned +sbus_dropped_frames() +{ + return sbus_frame_drops; +} + static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); @@ -108,7 +116,7 @@ sbus_init(const char *device, bool singlewire) /* initialise the decoder */ partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - + sbus_frame_drops = 0; } return sbus_fd; @@ -172,9 +180,9 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails * so we detect frame boundaries by the inter-frame delay. * * The minimum frame spacing is 7ms; with 25 bytes at 100000bps - * frame transmission time is ~2ms. + * frame transmission time is ~2500 us. * - * If an interval of more than 4ms passes between calls, + * If an interval of more than 2ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -183,6 +191,9 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); + /* + * If we timed out, reset the decoder + */ if ((now - last_rx_time) > 4000) { if (partial_frame_count > 0) { sbus_frame_drops++; @@ -201,6 +212,13 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails return false; } + /* if the first byte of the frame is not the start symbol, give up instantly */ + if (frame[0] != SBUS_START_SYMBOL) { + sbus_frame_drops++; + partial_frame_count = 0; + return false; + } + last_rx_time = now; /* @@ -264,7 +282,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f)) { + if ((frame[0] != SBUS_START_SYMBOL)) { sbus_frame_drops++; return false; } diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 04b91dc126..685c9d515f 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -71,4 +71,9 @@ __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bo __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); +/** + * The number of incomplete frames we encountered + */ +__EXPORT unsigned sbus_dropped_frames(void); + __END_DECLS From f8ea18909eb21a343b8c43714215923c0a5ae204 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 17:20:30 +0100 Subject: [PATCH 560/624] sensors: Carry frame drop counter from RAW input to normalized RC --- 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 5dba49879b..b7bfba8ffc 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1908,6 +1908,7 @@ Sensors::rc_poll() _rc.rssi = rc_input.rssi; _rc.signal_lost = signal_lost; _rc.timestamp = rc_input.timestamp_last_signal; + _rc.frame_drop_count = rc_input.rc_lost_frame_count; /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub != nullptr) { From ec3282a925d6352a9386c06dab8d6a32d81256f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Dec 2015 22:47:34 +0100 Subject: [PATCH 561/624] S.BUS parser: Empty complete buffer --- src/lib/rc/sbus.c | 78 ++++++++++++++++++++++++++--------------------- 1 file changed, 43 insertions(+), 35 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 5c1f0aeadc..ef8ba16fa0 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -46,6 +46,8 @@ #include "sbus.h" #include +#define SBUS_START_SYMBOL 0x0f + #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 @@ -194,51 +196,57 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails /* * If we timed out, reset the decoder */ - if ((now - last_rx_time) > 4000) { + if ((now - last_rx_time) > 6000) { if (partial_frame_count > 0) { sbus_frame_drops++; partial_frame_count = 0; } } - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + bool decode_success = false; - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - return false; - } + do { + /* + * Fetch bytes, but no more than we would need to complete + * the current frame. + */ + ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); - /* if the first byte of the frame is not the start symbol, give up instantly */ - if (frame[0] != SBUS_START_SYMBOL) { - sbus_frame_drops++; + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + continue; + } + + last_rx_time = now; + + /* if the first byte of the frame is not the start symbol, give up instantly */ + if (frame[0] != SBUS_START_SYMBOL) { + sbus_frame_drops++; + partial_frame_count = 0; + continue; + } + + /* + * Add bytes to the current frame + */ + partial_frame_count += ret; + + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < SBUS_FRAME_SIZE) { + continue; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ partial_frame_count = 0; - return false; - } + decode_success = decode_success || sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + } while (ret > 0); - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < SBUS_FRAME_SIZE) { - return false; - } - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + return decode_success; } /* From 66bc96f4b3dd50ac35693ca8401767f825c163dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:08:27 +0100 Subject: [PATCH 562/624] S.BUS: Adjust timeout interval --- src/lib/rc/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index ef8ba16fa0..662d09ef61 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -196,7 +196,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails /* * If we timed out, reset the decoder */ - if ((now - last_rx_time) > 6000) { + if ((now - last_rx_time) > 3600) { if (partial_frame_count > 0) { sbus_frame_drops++; partial_frame_count = 0; From bff0f225b19058efb4fde675a5d1cf3354280585 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:08:42 +0100 Subject: [PATCH 563/624] FMU driver: Update faster --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index af80e66682..e1e569f531 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_US 1800 +#define CONTROL_INPUT_DROP_LIMIT_US 1600 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev From 7b73e8f9ee5438e237caeb501a1f8bcecaff9b50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:16:08 +0100 Subject: [PATCH 564/624] S.BUS parsing improvements --- src/lib/rc/sbus.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 662d09ef61..cb0348f9b7 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -214,23 +214,23 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails /* if the read failed for any reason, just give up here */ if (ret < 1) { - continue; + break; } last_rx_time = now; - /* if the first byte of the frame is not the start symbol, give up instantly */ - if (frame[0] != SBUS_START_SYMBOL) { - sbus_frame_drops++; - partial_frame_count = 0; - continue; - } - /* * Add bytes to the current frame */ partial_frame_count += ret; + /* if the first byte of the frame is not the start symbol, give up instantly */ + if (partial_frame_count > 0 && (frame[0] != SBUS_START_SYMBOL)) { + sbus_frame_drops++; + partial_frame_count = 0; + continue; + } + /* * If we don't have a full frame, return */ From e715610887dac79998cb4a80b4028d2e04375dc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 21:42:49 +0100 Subject: [PATCH 565/624] Be more forgiving on avionics power voltage --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ba8720f9ee..1bde387c63 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -203,7 +203,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages - if (status->avionics_power_rail_voltage < 4.75f) { + if (status->avionics_power_rail_voltage < 4.5f) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; From 37f7460fea45f5173541aa6023e86174d7c57eb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Dec 2015 15:20:49 +0100 Subject: [PATCH 566/624] FMU driver: S.BUS fixes, scheduling fixes and preparations for One Shot --- src/drivers/px4fmu/fmu.cpp | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e1e569f531..70ed184d0c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_US 1600 +#define CONTROL_INPUT_DROP_LIMIT_US 2000 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev @@ -702,6 +702,9 @@ PX4FMU::cycle() /* check if anything updated */ int ret = ::poll(_poll_fds, _poll_fds_num, 0); + /* log if main actuator updated and sync */ + int main_out_latency = 0; + /* this would be bad... */ if (ret < 0) { DEVICE_LOG("poll error %d", errno); @@ -719,6 +722,20 @@ PX4FMU::cycle() if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + + /* main outputs */ + if (i == 0) { + //main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250; + + /* do only correct within the current phase */ + if (abs(main_out_latency) > CONTROL_INPUT_DROP_LIMIT_US) { + main_out_latency = CONTROL_INPUT_DROP_LIMIT_US; + } + + if (main_out_latency < 250) { + main_out_latency = 0; + } + } } poll_id++; @@ -887,7 +904,8 @@ PX4FMU::cycle() } } - work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US)); + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, + USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US - main_out_latency)); } void PX4FMU::work_stop() @@ -2172,7 +2190,6 @@ fmu_main(int argc, char *argv[]) (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]); } - if (fmu_start() != OK) { errx(1, "failed to start the FMU driver"); } @@ -2224,6 +2241,11 @@ fmu_main(int argc, char *argv[]) test(); } + if (!strcmp(verb, "info")) { + warnx("frame drops: %u", sbus_dropped_frames()); + return 0; + } + if (!strcmp(verb, "fake")) { fake(argc - 1, argv + 1); } From 990e8acaea9868090fd5689f4853c7d7c3413517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Dec 2015 15:21:48 +0100 Subject: [PATCH 567/624] S.BUS parser rewrite to address FrSky timing and S.BUS2 commands --- src/lib/rc/sbus.c | 160 +++++++++++++++++++++++++++++++--------------- 1 file changed, 107 insertions(+), 53 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index cb0348f9b7..e3d63e0606 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -49,13 +49,19 @@ #define SBUS_START_SYMBOL 0x0f #define SBUS_FRAME_SIZE 25 +#define SBUS_MAX_BUF_SIZE (SBUS_FRAME_SIZE + 15) #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 #define SBUS1_FRAME_DELAY 14000 -#define SBUS_SINGLE_CHAR_LEN_US ((1/((100000/10)) * 1000 * 1000) +#define SBUS_SINGLE_CHAR_LEN_US (1/((100000/10)) * 1000 * 1000) + +#define SBUS_FRAME_INTERVAL_US (SBUS_SINGLE_CHAR_LEN_US * SBUS_FRAME_SIZE) +#define SBUS_FRAME_GAP_US 6200 +#define SBUS_MIN_CALL_INTERVAL_US (SBUS_FRAME_GAP_US / 3) +#define SBUS_EPSILON_US 2500 /* Measured values with Futaba FX-30/R6108SB: @@ -71,6 +77,15 @@ #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f +#define SBUS_SYNC_PHASE0 0 +#define SBUS_SYNC_PHASE1 1 + +#define SBUS_DEBUG + +#ifdef SBUS_DEBUG +#include +#endif + /* pre-calculate the floating point stuff as far as possible at compile time */ #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) @@ -79,11 +94,12 @@ static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; -static uint8_t frame[SBUS_FRAME_SIZE]; +static uint8_t frame[SBUS_MAX_BUF_SIZE]; static unsigned partial_frame_count; -unsigned sbus_frame_drops; +static unsigned sbus_frame_drops; +static unsigned sbus_sync = SBUS_SYNC_PHASE0; unsigned sbus_dropped_frames() @@ -174,14 +190,14 @@ bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { - ssize_t ret; + int ret; hrt_abstime now; /* * The S.bus protocol doesn't provide reliable framing, * so we detect frame boundaries by the inter-frame delay. * - * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * The minimum frame spacing is 6ms; with 25 bytes at 100000bps * frame transmission time is ~2500 us. * * If an interval of more than 2ms passes between calls, @@ -193,60 +209,76 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); - /* - * If we timed out, reset the decoder - */ - if ((now - last_rx_time) > 3600) { - if (partial_frame_count > 0) { - sbus_frame_drops++; + if (sbus_sync == SBUS_SYNC_PHASE0) { + + /* empty the UART buffer */ + int ret = read(sbus_fd, &frame[0], sizeof(frame) / sizeof(frame[0])); + + /* establish the last data we got */ + if (ret > 0) { + last_rx_time = now; + + bool rc_control_packet = false; + + for (unsigned i = 0; i < ret; i++) { + if (frame[i] == SBUS_START_SYMBOL) { + rc_control_packet = true; + } + } + + if (rc_control_packet) { + /* we should not be reading anything in this state if synced */ + sbus_frame_drops++; + } + + /* There are two known valid S.BUS 2 commands: + * + * 0x3 0x84 0x0 + * 0x3 0x80 0x2f + */ + } + + /* wait until a frame gap has passed */ + if (now - last_rx_time > (SBUS_FRAME_INTERVAL_US + 500)) { + + /* ready to decode - reset buffer and start filling it */ + sbus_sync = SBUS_SYNC_PHASE1; partial_frame_count = 0; + + } else { + return false; } } - bool decode_success = false; + /* + * Get all data + */ + ret = read(sbus_fd, &frame[partial_frame_count], SBUS_MAX_BUF_SIZE); - do { - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + return false; + } - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - break; - } + last_rx_time = now; - last_rx_time = now; + /* + * Add bytes to the current frame + */ + partial_frame_count += ret; - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < SBUS_FRAME_SIZE) { + return false; + } - /* if the first byte of the frame is not the start symbol, give up instantly */ - if (partial_frame_count > 0 && (frame[0] != SBUS_START_SYMBOL)) { - sbus_frame_drops++; - partial_frame_count = 0; - continue; - } - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < SBUS_FRAME_SIZE) { - continue; - } - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - decode_success = decode_success || sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); - } while (ret > 0); - - return decode_success; + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -289,13 +321,33 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { + /* skip over valid S.BUS 2 command sequences */ + unsigned s = 0; + + if ((frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) || + (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) || + (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) || + (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f)) { + + + s = 3; + } + /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != SBUS_START_SYMBOL)) { + if ((frame[s + 0] != SBUS_START_SYMBOL)) { sbus_frame_drops++; + sbus_sync = SBUS_SYNC_PHASE0; + printf("decode fail:"); + + for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { + printf("0x%0x ", frame[i]); + } + + printf("\n"); return false; } - switch (frame[24]) { + switch (frame[s + 24]) { case 0x00: /* this is S.BUS 1 */ break; @@ -335,7 +387,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; if (decode->mask != 0) { - unsigned piece = frame[1 + decode->byte]; + unsigned piece = frame[s + 1 + decode->byte]; piece >>= decode->rshift; piece &= decode->mask; piece <<= decode->lshift; @@ -383,5 +435,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_frame_drop = false; } + sbus_sync = SBUS_SYNC_PHASE0; + return true; } From ac3bc34d15af5612c933ef307ce0d6e5272dc14b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Dec 2015 15:30:23 +0100 Subject: [PATCH 568/624] S.BUS decoder: Do not report initial sync as dropped frame --- src/lib/rc/sbus.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index e3d63e0606..84cac34185 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -134,6 +134,7 @@ sbus_init(const char *device, bool singlewire) /* initialise the decoder */ partial_frame_count = 0; last_rx_time = hrt_absolute_time(); + last_frame_time = last_rx_time; sbus_frame_drops = 0; } @@ -226,7 +227,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails } } - if (rc_control_packet) { + if (rc_control_packet && last_frame_time > 0) { /* we should not be reading anything in this state if synced */ sbus_frame_drops++; } From 98da35793d264d10545e2309dbb52c03d5a3988a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Dec 2015 15:59:47 +0100 Subject: [PATCH 569/624] Further S.BUS decoding work for S.BUS2 --- src/lib/rc/sbus.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 84cac34185..38c2373a9d 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -49,7 +49,7 @@ #define SBUS_START_SYMBOL 0x0f #define SBUS_FRAME_SIZE 25 -#define SBUS_MAX_BUF_SIZE (SBUS_FRAME_SIZE + 15) +#define SBUS_MAX_BUF_SIZE (SBUS_FRAME_SIZE + 10) #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 @@ -58,8 +58,7 @@ #define SBUS_SINGLE_CHAR_LEN_US (1/((100000/10)) * 1000 * 1000) -#define SBUS_FRAME_INTERVAL_US (SBUS_SINGLE_CHAR_LEN_US * SBUS_FRAME_SIZE) -#define SBUS_FRAME_GAP_US 6200 +#define SBUS_FRAME_INTERVAL_US 2500 #define SBUS_MIN_CALL_INTERVAL_US (SBUS_FRAME_GAP_US / 3) #define SBUS_EPSILON_US 2500 @@ -210,6 +209,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); + // XXX rewrite this using a proper protocol parser not relying on timing if (sbus_sync == SBUS_SYNC_PHASE0) { /* empty the UART buffer */ @@ -254,7 +254,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails /* * Get all data */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_MAX_BUF_SIZE); + ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ if (ret < 1) { @@ -367,9 +367,16 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool case 0xA3: case 0x63: case 0xE3: + case 0x04: + case 0x14: + case 0x24: + case 0x34: break; default: + sbus_frame_drops++; + sbus_sync = SBUS_SYNC_PHASE0; + return false; /* we expect one of the bits above, but there are some we don't know yet */ break; } From 475fec88fae2e0188c231d78ae0a0f25f494ca68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:35:00 +0100 Subject: [PATCH 570/624] Update SBUS2 test --- unittests/sbus2_test.cpp | 74 ++++++++++++++++++++++++++++++++-------- 1 file changed, 59 insertions(+), 15 deletions(-) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 54c7fc412b..ff8f14bd27 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -20,33 +20,67 @@ TEST(SBUS2Test, SBUS2) ASSERT_TRUE(fp); warnx("loading data from: %s", filepath); + // if (argc < 2) + // errx(1, "Need a filename for the input file"); + + int byte_offset = 7; + + // if (argc > 2) { + // char* end; + // byte_offset = strtol(argv[2],&end,10); + // } + + warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset); + float f; unsigned x; int ret; // Trash the first 20 lines for (unsigned i = 0; i < 20; i++) { - char buf[200]; - (void)fgets(buf, sizeof(buf), fp); + (void)fscanf(fp, "%f,%x,,", &f, &x); } // Init the parser - uint8_t frame[30]; + uint8_t frame[SBUS_BUFFER_SIZE]; unsigned partial_frame_count = 0; - //uint16_t rc_values[18]; - //uint16_t num_values; - //bool sbus_failsafe; - //bool sbus_frame_drop; - //uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + uint16_t rc_values[18]; + uint16_t num_values; + uint16_t sbus_frame_drops = 0; + unsigned sbus_frame_resets = 0; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); float last_time = 0; + int rate_limiter = 0; + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { - if (((f - last_time) * 1000 * 1000) > 3000) { + + unsigned last_drop = sbus_frame_drops + sbus_frame_resets; + + unsigned interval_us = ((f - last_time) * 1000 * 1000); + + if (interval_us > SBUS_INTER_FRAME_TIMEOUT) { + if (partial_frame_count != 0) { + warnx("[ %08.4fs ] INTERVAL: %u - FRAME RESET, DROPPED %d bytes", + interval_us, f, partial_frame_count); + + printf("\t\tdropped: "); + for (int i = 0; i < partial_frame_count; i++) { + printf("%02X ", frame[i]); + } + printf("\n"); + + sbus_frame_resets++; + } + partial_frame_count = 0; - //warnx("FRAME RESET\n\n"); } + last_time = f; + frame[partial_frame_count] = x; partial_frame_count++; @@ -54,15 +88,25 @@ TEST(SBUS2Test, SBUS2) if (partial_frame_count == sizeof(frame)) { partial_frame_count = 0; + warnx("FRAME SIZE OVERFLOW!\n\n\n"); } - last_time = f; - // Pipe the data into the parser - //hrt_abstime now = hrt_absolute_time(); + hrt_abstime now = hrt_absolute_time(); - //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + if (rate_limiter % byte_offset == 0) { + bool result = sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, max_channels); + sbus_frame_drops = sbus_dropped_frames(); + + if (result) + warnx("decoded packet"); + } + + if (last_drop != (sbus_frame_drops + sbus_frame_resets)) + warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); + + rate_limiter++; } ASSERT_EQ(ret, EOF); From 02bdc8005eccfd98d7d5e385f2773c2df9678cf6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 11:35:25 +0100 Subject: [PATCH 571/624] S.BUS: Use proper parser --- src/lib/rc/sbus.c | 367 ++++++++++++++++++++-------------------------- 1 file changed, 155 insertions(+), 212 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 38c2373a9d..4120bb56d3 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -93,7 +93,21 @@ static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; -static uint8_t frame[SBUS_MAX_BUF_SIZE]; +#define SBUS2_FRAME_SIZE_RX_VOLTAGE 3 +#define SBUS2_FRAME_SIZE_GPS_DIGIT 3 + +static enum SBUS2_DECODE_STATE { + SBUS2_DECODE_STATE_DESYNC = 0xFFF, + SBUS2_DECODE_STATE_SBUS_START = 0x2FF, + SBUS2_DECODE_STATE_SBUS1_SYNC = 0x00, + SBUS2_DECODE_STATE_SBUS2_SYNC = 0x1FF, + SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE = 0x04, + SBUS2_DECODE_STATE_SBUS2_GPS = 0x14, + SBUS2_DECODE_STATE_SBUS2_DATA1 = 0x24, + SBUS2_DECODE_STATE_SBUS2_DATA2 = 0x34 +} sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + +static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; static unsigned partial_frame_count; @@ -187,20 +201,21 @@ sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) } bool -sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { - int ret; + ssize_t ret; hrt_abstime now; /* - * The S.bus protocol doesn't provide reliable framing, + * The S.BUS protocol doesn't provide reliable framing, * so we detect frame boundaries by the inter-frame delay. * - * The minimum frame spacing is 6ms; with 25 bytes at 100000bps - * frame transmission time is ~2500 us. + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. * - * If an interval of more than 2ms passes between calls, + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -209,241 +224,169 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); - // XXX rewrite this using a proper protocol parser not relying on timing - if (sbus_sync == SBUS_SYNC_PHASE0) { - - /* empty the UART buffer */ - int ret = read(sbus_fd, &frame[0], sizeof(frame) / sizeof(frame[0])); - - /* establish the last data we got */ - if (ret > 0) { - last_rx_time = now; - - bool rc_control_packet = false; - - for (unsigned i = 0; i < ret; i++) { - if (frame[i] == SBUS_START_SYMBOL) { - rc_control_packet = true; - } - } - - if (rc_control_packet && last_frame_time > 0) { - /* we should not be reading anything in this state if synced */ - sbus_frame_drops++; - } - - /* There are two known valid S.BUS 2 commands: - * - * 0x3 0x84 0x0 - * 0x3 0x80 0x2f - */ - } - - /* wait until a frame gap has passed */ - if (now - last_rx_time > (SBUS_FRAME_INTERVAL_US + 500)) { - - /* ready to decode - reset buffer and start filling it */ - sbus_sync = SBUS_SYNC_PHASE1; + if ((now - last_rx_time) > SBUS_INTER_FRAME_TIMEOUT) { + if (partial_frame_count > 0) { + sbus_frame_drops++; partial_frame_count = 0; - - } else { - return false; } } /* - * Get all data + * Fetch bytes, but no more than we would need to complete + * a complete frame. */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + ret = read(sbus_fd, &sbus_frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ if (ret < 1) { return false; } - last_rx_time = now; - /* * Add bytes to the current frame */ partial_frame_count += ret; /* - * If we don't have a full frame, return + * Try to decode something with what we got */ - if (partial_frame_count < SBUS_FRAME_SIZE) { - return false; - } - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + return sbus_parse(now, sbus_frame, &partial_frame_count, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); } -/* - * S.bus decoder matrix. - * - * Each channel value can come from up to 3 input bytes. Each row in the - * matrix describes up to three bytes, and each entry gives: - * - * - byte offset in the data portion of the frame - * - right shift applied to the data byte - * - mask for the data byte - * - left shift applied to the result into the channel value - */ -struct sbus_bit_pick { - uint8_t byte; - uint8_t rshift; - uint8_t mask; - uint8_t lshift; -}; -static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { - /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, - /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, - /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, - /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, - /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, - /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } -}; - -static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, - uint16_t max_values) +bool +sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *frame_drops, uint16_t max_channels) { - /* skip over valid S.BUS 2 command sequences */ - unsigned s = 0; - if ((frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) || - (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) || - (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) || - (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f)) { + last_rx_time = now; + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. + */ + bool decode_ret = true; - s = 3; - } + /* keep decoding until we have consumed the buffer or have given up */ + for (unsigned d = 0; ((d < 10) && (*partial_count > 0) && (decode_ret)); d++) { + unsigned n_consumed = 0; - /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[s + 0] != SBUS_START_SYMBOL)) { - sbus_frame_drops++; - sbus_sync = SBUS_SYNC_PHASE0; - printf("decode fail:"); + switch (sbus_decode_state) { + case SBUS2_DECODE_STATE_DESYNC: + /* we are de-synced and only interested in the frame marker */ + { + unsigned i = 0; + while (i < *partial_count) { + + /* for the case where frame[i] is the start sign, the index + * is exactly the number of bytes to consume + */ + n_consumed = i; - for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { - printf("0x%0x ", frame[i]); + if (frame[i] == 0x0F) { + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + break; + } + + /* increment index to next position */ + i++; + } + } + break; + /* fall through */ + case SBUS2_DECODE_STATE_SBUS_START: + case SBUS2_DECODE_STATE_SBUS1_SYNC: + /* fall through */ + case SBUS2_DECODE_STATE_SBUS2_SYNC: + { + /* decode whatever we got and expect */ + if (*partial_count < SBUS_FRAME_SIZE) { + decode_ret = false; + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = sbus_decode(now, frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + + if (decode_ret) { + n_consumed = SBUS_FRAME_SIZE; + } + } + break; + case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: + { + if (*partial_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { + decode_ret = false; + break; + } + + /* find out which payload we're dealing with in this slot */ + switch(frame[0]) { + case 0x03: + { + uint16_t rx_voltage = (frame[1] << 8) | frame[2]; + isr_debug(30, "rx_voltage %d", (int)rx_voltage); + n_consumed = 3; + } + break; + case 0x0F: + /* the battery slot is unused and followed by a normal frame */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + n_consumed = 3; + } + } + break; + case SBUS2_DECODE_STATE_SBUS2_GPS: + { + if (*partial_count < 24) { + decode_ret = false; + break; + } + + /* find out which payload we're dealing with in this slot */ + switch(frame[0]) { + case 0x13: + { + uint16_t gps_something = (frame[1] << 8) | frame[2]; + isr_debug(30, "gps_something %d", (int)gps_something); + n_consumed = 24; + } + break; + case 0x0F: + /* the GPS slot is unused and followed by a normal frame */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + n_consumed = 3; + } + } + break; + default: + decode_ret = false; } - printf("\n"); - return false; + /* move buffer to start after this step, keep decoding if any bytes remain */ + + /* XXX should be still more efficient than single-byte ringbuffer accesses */ + uint8_t frame_buf[SBUS_FRAME_SIZE + 10]; + memcpy(&frame_buf[0], &frame[n_consumed], *partial_count - n_consumed); + memcpy(&frame[0], &frame_buf[0], *partial_count - n_consumed); + *partial_count = *partial_count - n_consumed; } - switch (frame[s + 24]) { - case 0x00: - /* this is S.BUS 1 */ - break; + *frame_drops = sbus_frame_drops; - case 0x03: - /* S.BUS 2 SLOT0: RX battery and external voltage */ - break; - - case 0x83: - /* S.BUS 2 SLOT1 */ - break; - - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: - case 0x04: - case 0x14: - case 0x24: - case 0x34: - break; - - default: - sbus_frame_drops++; - sbus_sync = SBUS_SYNC_PHASE0; - return false; - /* we expect one of the bits above, but there are some we don't know yet */ - break; - } - - /* we have received something we think is a frame */ - last_frame_time = frame_time; - - unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : max_values; - - /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < chancount; channel++) { - unsigned value = 0; - - for (unsigned pick = 0; pick < 3; pick++) { - const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; - - if (decode->mask != 0) { - unsigned piece = frame[s + 1 + decode->byte]; - piece >>= decode->rshift; - piece &= decode->mask; - piece <<= decode->lshift; - - value |= piece; - } - } - - - /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; - } - - /* decode switch channels if data fields are wide enough */ - if (max_values > 17 && chancount > 15) { - chancount = 18; - - /* channel 17 (index 16) */ - values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; - /* channel 18 (index 17) */ - values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; - } - - /* note the number of channels decoded */ - *num_values = chancount; - - /* decode and handle failsafe and frame-lost flags */ - if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ - /* report that we failed to read anything valid off the receiver */ - *sbus_failsafe = true; - *sbus_frame_drop = true; - - } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, - * e.g. by prematurely issueing return-to-launch!!! */ - - *sbus_failsafe = false; - *sbus_frame_drop = true; - - } else { - *sbus_failsafe = false; - *sbus_frame_drop = false; - } - - sbus_sync = SBUS_SYNC_PHASE0; - - return true; + /* return false as default */ + return decode_ret; } From 8c5ee059aee9237f524878382fae9108f46c6cf2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 13:39:29 +0100 Subject: [PATCH 572/624] S.BUS2: Initial parsing --- src/lib/rc/sbus.c | 152 +++++++++++++++++++++++++++++++++++++++++++--- src/lib/rc/sbus.h | 6 ++ 2 files changed, 151 insertions(+), 7 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 4120bb56d3..5b3ed90d9b 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -42,6 +42,7 @@ #include #include #include +#include #include "sbus.h" #include @@ -120,8 +121,9 @@ sbus_dropped_frames() return sbus_frame_drops; } -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, - bool *sbus_frame_drop, uint16_t max_channels); +static bool +sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); int sbus_init(const char *device, bool singlewire) @@ -201,7 +203,7 @@ sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) } bool -sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; @@ -254,8 +256,8 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb } bool -sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *values, - uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *frame_drops, uint16_t max_channels) +sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) { last_rx_time = now; @@ -326,7 +328,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v case 0x03: { uint16_t rx_voltage = (frame[1] << 8) | frame[2]; - isr_debug(30, "rx_voltage %d", (int)rx_voltage); + printf("rx_voltage %d", (int)rx_voltage); n_consumed = 3; } break; @@ -355,7 +357,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v case 0x13: { uint16_t gps_something = (frame[1] << 8) | frame[2]; - isr_debug(30, "gps_something %d", (int)gps_something); + printf("gps_something %d", (int)gps_something); n_consumed = 24; } break; @@ -390,3 +392,139 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v /* return false as default */ return decode_ret; } + +/* + * S.bus decoder matrix. + * + * Each channel value can come from up to 3 input bytes. Each row in the + * matrix describes up to three bytes, and each entry gives: + * + * - byte offset in the data portion of the frame + * - right shift applied to the data byte + * - mask for the data byte + * - left shift applied to the result into the channel value + */ +struct sbus_bit_pick { + uint8_t byte; + uint8_t rshift; + uint8_t mask; + uint8_t lshift; +}; +static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { + /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, + /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, + /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, + /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, + /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, + /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } +}; + +bool +sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) +{ + + /* check frame boundary markers to avoid out-of-sync cases */ + if ((frame[0] != 0x0f)) { + sbus_frame_drops++; + return false; + } + + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC; + break; + case 0x04: + /* receiver voltage */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE; + break; + case 0x14: + /* GPS / baro */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS; + break; + case 0x24: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + case 0x34: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + default: + return false; + } + + /* we have received something we think is a frame */ + last_frame_time = frame_time; + + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; + + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < chancount; channel++) { + unsigned value = 0; + + for (unsigned pick = 0; pick < 3; pick++) { + const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; + + if (decode->mask != 0) { + unsigned piece = frame[1 + decode->byte]; + piece >>= decode->rshift; + piece &= decode->mask; + piece <<= decode->lshift; + + value |= piece; + } + } + + + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + } + + /* decode switch channels if data fields are wide enough */ + if (max_values > 17 && chancount > 15) { + chancount = 18; + + /* channel 17 (index 16) */ + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; + } + + /* note the number of channels decoded */ + *num_values = chancount; + + /* decode and handle failsafe and frame-lost flags */ + if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ + /* report that we failed to read anything valid off the receiver */ + *sbus_failsafe = true; + *sbus_frame_drop = true; + } + else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, + * e.g. by prematurely issueing return-to-launch!!! */ + + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; + } + + return true; +} diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 685c9d515f..222cb5e4b9 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -46,6 +46,10 @@ __BEGIN_DECLS +#define SBUS_INTER_FRAME_TIMEOUT 3000 /**< 3000 us frame timeout */ +#define SBUS_FRAME_SIZE 25 +#define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2) + __EXPORT int sbus_init(const char *device, bool singlewire); /** @@ -68,6 +72,8 @@ __EXPORT int sbus_init(const char *device, bool singlewire); __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +__EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); From 1fdff53c82007a426c54c2b94429f048cd204989 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 13:40:30 +0100 Subject: [PATCH 573/624] Fix S.BUS2 test --- unittests/sbus2_test.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index ff8f14bd27..1a88508117 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -4,7 +4,7 @@ #include "../../src/systemcmds/tests/tests.h" #include -#include +#include #include #include @@ -38,7 +38,8 @@ TEST(SBUS2Test, SBUS2) // Trash the first 20 lines for (unsigned i = 0; i < 20; i++) { - (void)fscanf(fp, "%f,%x,,", &f, &x); + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); } // Init the parser @@ -46,7 +47,7 @@ TEST(SBUS2Test, SBUS2) unsigned partial_frame_count = 0; uint16_t rc_values[18]; uint16_t num_values; - uint16_t sbus_frame_drops = 0; + unsigned sbus_frame_drops = 0; unsigned sbus_frame_resets = 0; bool sbus_failsafe; bool sbus_frame_drop; @@ -58,14 +59,16 @@ TEST(SBUS2Test, SBUS2) while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + ASSERT_GT(ret, 0); + unsigned last_drop = sbus_frame_drops + sbus_frame_resets; unsigned interval_us = ((f - last_time) * 1000 * 1000); if (interval_us > SBUS_INTER_FRAME_TIMEOUT) { if (partial_frame_count != 0) { - warnx("[ %08.4fs ] INTERVAL: %u - FRAME RESET, DROPPED %d bytes", - interval_us, f, partial_frame_count); + warnx("[ %08.4fs ] INTERVAL: %f - FRAME RESET, DROPPED %d bytes", + interval_us / 1e6, f, partial_frame_count); printf("\t\tdropped: "); for (int i = 0; i < partial_frame_count; i++) { @@ -96,8 +99,7 @@ TEST(SBUS2Test, SBUS2) if (rate_limiter % byte_offset == 0) { bool result = sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, - &sbus_failsafe, &sbus_frame_drop, max_channels); - sbus_frame_drops = sbus_dropped_frames(); + &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); if (result) warnx("decoded packet"); From e11908697cc51255cebd95f4ec1dd94c2b70384e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 13:40:43 +0100 Subject: [PATCH 574/624] S.BUS2 test data: Remove invalid line --- unittests/testdata/sbus2_r7008SB.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/unittests/testdata/sbus2_r7008SB.txt b/unittests/testdata/sbus2_r7008SB.txt index d504ba7056..be2bccf0ae 100644 --- a/unittests/testdata/sbus2_r7008SB.txt +++ b/unittests/testdata/sbus2_r7008SB.txt @@ -1,5 +1,4 @@ Time [s],Value,Parity Error,Framing Error -0,0x0F,, 0.00012,0x11,, 0.00024,0x04,, 0.00036,0x20,, From d3be4352ea503363d405290648a9218f76377503 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 13:41:14 +0100 Subject: [PATCH 575/624] Add S.BUS parser to test --- unittests/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index c7e62a3535..fcfcee3702 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -148,7 +148,8 @@ target_link_libraries( conversion_test px4_platform ) add_gtest(conversion_test) # sbus2_test -add_executable(sbus2_test sbus2_test.cpp hrt.cpp) +add_executable(sbus2_test sbus2_test.cpp hrt.cpp + ${PX_SRC}/lib/rc/sbus.c) target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) From 4d4bb1b8b683a1e1305c6438663aef89e5426f5c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 16:09:21 +0100 Subject: [PATCH 576/624] S.BUS2 parser: Solid results on Futaba log --- src/lib/rc/sbus.c | 217 +++++++++++++++++++++++++++++++--------------- src/lib/rc/sbus.h | 2 +- 2 files changed, 147 insertions(+), 72 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 5b3ed90d9b..c1552a058e 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -198,8 +198,7 @@ sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) { - char b = 'B'; - write(sbus_fd, &b, 1); + sbus1_output(sbus_fd, values, num_values); } bool @@ -237,26 +236,28 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails * Fetch bytes, but no more than we would need to complete * a complete frame. */ - ret = read(sbus_fd, &sbus_frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + uint8_t buf[SBUS_FRAME_SIZE]; + bool decoded = false; + do { + ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - return false; - } + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + return false; + } - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; + /* + * Try to decode something with what we got + */ + decoded = decoded || sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); - /* - * Try to decode something with what we got - */ - return sbus_parse(now, sbus_frame, &partial_frame_count, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); + } while (ret > 0); + + return decoded; } bool -sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *values, +sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) { @@ -265,32 +266,49 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *valu /* this is set by the decoding state machine and will default to false * once everything that was decodable has been decoded. */ - bool decode_ret = true; + bool decode_ret = false; - /* keep decoding until we have consumed the buffer or have given up */ - for (unsigned d = 0; ((d < 10) && (*partial_count > 0) && (decode_ret)); d++) { - unsigned n_consumed = 0; + /* keep decoding until we have consumed the buffer */ + for (unsigned d = 0; d < len; d++) { + + /* overflow check */ + if (partial_frame_count == sizeof(sbus_frame) / sizeof(sbus_frame[0])) { + partial_frame_count = 0; + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + #ifdef SBUS_DEBUG + printf("SBUS2: RESET (BUF LIM)\n"); + #endif + } + + if (partial_frame_count == SBUS_FRAME_SIZE) { + partial_frame_count = 0; + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + #ifdef SBUS_DEBUG + printf("SBUS2: RESET (PACKET LIM)\n"); + #endif + } + + #ifdef SBUS_DEBUG + #if 0 + printf("sbus state: %s%s%s%s%s%s, count: %d, val: %02x\n", + (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", + partial_frame_count, + (unsigned)frame[d]); + #endif + #endif switch (sbus_decode_state) { case SBUS2_DECODE_STATE_DESYNC: /* we are de-synced and only interested in the frame marker */ - { - unsigned i = 0; - while (i < *partial_count) { - - /* for the case where frame[i] is the start sign, the index - * is exactly the number of bytes to consume - */ - n_consumed = i; - - if (frame[i] == 0x0F) { - sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; - break; - } - - /* increment index to next position */ - i++; - } + if (frame[d] == SBUS_START_SYMBOL) { + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + partial_frame_count = 0; + sbus_frame[partial_frame_count++] = frame[d]; } break; /* fall through */ @@ -299,9 +317,9 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *valu /* fall through */ case SBUS2_DECODE_STATE_SBUS2_SYNC: { + sbus_frame[partial_frame_count++] = frame[d]; /* decode whatever we got and expect */ - if (*partial_count < SBUS_FRAME_SIZE) { - decode_ret = false; + if (partial_frame_count < SBUS_FRAME_SIZE) { break; } @@ -309,82 +327,127 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *valu * Great, it looks like we might have a frame. Go ahead and * decode it. */ - decode_ret = sbus_decode(now, frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); - if (decode_ret) { - n_consumed = SBUS_FRAME_SIZE; + /* + * Offset recovery: If decoding failed, check if there is a second + * start marker in the packet. + */ + unsigned start_index = 0; + + if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) { + + for (unsigned i = 1; i < partial_frame_count; i++) { + if (sbus_frame[i] == SBUS_START_SYMBOL) { + start_index = i; + break; + } + } + + /* we found a second start marker */ + if (start_index != 0) { + /* shift everything in the buffer and reset the state machine */ + for (unsigned i = 0; i < partial_frame_count - start_index; i++) { + sbus_frame[i] = sbus_frame[i + start_index]; + } + + partial_frame_count -= start_index; + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + + #ifdef SBUS_DEBUG + printf("DECODE RECOVERY: %d\n", start_index); + #endif + } } + + /* if there has been no successful attempt at saving a failed + * decoding run, reset the frame count for successful and + * unsuccessful decode runs. + */ + if (start_index == 0) { + partial_frame_count = 0; + } + } break; case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: { - if (*partial_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { - decode_ret = false; + sbus_frame[partial_frame_count++] = frame[d]; + + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + } + + if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { break; } /* find out which payload we're dealing with in this slot */ - switch(frame[0]) { + switch(sbus_frame[0]) { case 0x03: { - uint16_t rx_voltage = (frame[1] << 8) | frame[2]; - printf("rx_voltage %d", (int)rx_voltage); - n_consumed = 3; + // Observed values: + // (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) + // (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f) + uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2]; + #ifdef SBUS_DEBUG + //printf("rx_voltage %d\n", (int)rx_voltage); + #endif } - break; - case 0x0F: - /* the battery slot is unused and followed by a normal frame */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + partial_frame_count = 0; break; default: /* this is not what we expect it to be, go back to sync */ sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; sbus_frame_drops++; - /* throw unknown bytes away */ - n_consumed = 3; } + } break; case SBUS2_DECODE_STATE_SBUS2_GPS: { - if (*partial_count < 24) { + sbus_frame[partial_frame_count++] = frame[d]; + + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + } + + if (partial_frame_count < 24) { decode_ret = false; break; } /* find out which payload we're dealing with in this slot */ - switch(frame[0]) { + switch(sbus_frame[0]) { case 0x13: { uint16_t gps_something = (frame[1] << 8) | frame[2]; - printf("gps_something %d", (int)gps_something); - n_consumed = 24; + #ifdef SBUS_DEBUG + printf("gps_something %d\n", (int)gps_something); + #endif } - break; - case 0x0F: - /* the GPS slot is unused and followed by a normal frame */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + partial_frame_count = 0; break; default: /* this is not what we expect it to be, go back to sync */ sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; sbus_frame_drops++; /* throw unknown bytes away */ - n_consumed = 3; } } break; default: + #ifdef SBUS_DEBUG + printf("UNKNOWN PROTO STATE"); + #endif decode_ret = false; } - /* move buffer to start after this step, keep decoding if any bytes remain */ - - /* XXX should be still more efficient than single-byte ringbuffer accesses */ - uint8_t frame_buf[SBUS_FRAME_SIZE + 10]; - memcpy(&frame_buf[0], &frame[n_consumed], *partial_count - n_consumed); - memcpy(&frame[0], &frame_buf[0], *partial_count - n_consumed); - *partial_count = *partial_count - n_consumed; + } *frame_drops = sbus_frame_drops; @@ -435,8 +498,16 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f)) { + if ((frame[0] != SBUS_START_SYMBOL)) { sbus_frame_drops++; + #ifdef SBUS_DEBUG + printf("DECODE FAIL: "); + for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { + printf("%0x ", frame[i]); + } + printf("\n"); + #endif + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; return false; } @@ -462,6 +533,10 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; break; default: + #ifdef SBUS_DEBUG + printf("DECODE FAIL: END MARKER\n"); + #endif + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; return false; } diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 222cb5e4b9..8bc6e808b3 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -72,7 +72,7 @@ __EXPORT int sbus_init(const char *device, bool singlewire); __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); -__EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned *partial_count, uint16_t *values, +__EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); From 856406fb6ce2c6d9fd20aa2eb7fc7f0ee1f46ebc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 16:12:05 +0100 Subject: [PATCH 577/624] Update S.BUS2 test --- unittests/sbus2_test.cpp | 55 ++++++++++------------------------------ 1 file changed, 13 insertions(+), 42 deletions(-) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 1a88508117..69882a4693 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -44,7 +44,6 @@ TEST(SBUS2Test, SBUS2) // Init the parser uint8_t frame[SBUS_BUFFER_SIZE]; - unsigned partial_frame_count = 0; uint16_t rc_values[18]; uint16_t num_values; unsigned sbus_frame_drops = 0; @@ -53,60 +52,32 @@ TEST(SBUS2Test, SBUS2) bool sbus_frame_drop; uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); - float last_time = 0; - int rate_limiter = 0; + unsigned last_drop = 0; while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { ASSERT_GT(ret, 0); - unsigned last_drop = sbus_frame_drops + sbus_frame_resets; - - unsigned interval_us = ((f - last_time) * 1000 * 1000); - - if (interval_us > SBUS_INTER_FRAME_TIMEOUT) { - if (partial_frame_count != 0) { - warnx("[ %08.4fs ] INTERVAL: %f - FRAME RESET, DROPPED %d bytes", - interval_us / 1e6, f, partial_frame_count); - - printf("\t\tdropped: "); - for (int i = 0; i < partial_frame_count; i++) { - printf("%02X ", frame[i]); - } - printf("\n"); - - sbus_frame_resets++; - } - - partial_frame_count = 0; - } - - last_time = f; - - frame[partial_frame_count] = x; - partial_frame_count++; - - //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - - if (partial_frame_count == sizeof(frame)) { - partial_frame_count = 0; - warnx("FRAME SIZE OVERFLOW!\n\n\n"); - } - + frame[0] = x; + unsigned len = 1; + // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - if (rate_limiter % byte_offset == 0) { - bool result = sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, + // if (rate_limiter % byte_offset == 0) { + bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); - if (result) - warnx("decoded packet"); - } + if (result) { + //warnx("decoded packet"); + } + // } - if (last_drop != (sbus_frame_drops + sbus_frame_resets)) + if (last_drop != (sbus_frame_drops + sbus_frame_resets)) { warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); + last_drop = sbus_frame_drops + sbus_frame_resets; + } rate_limiter++; } From cde803c5996c7e6c4f3d58ccddad158c77cd0adc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 16:12:25 +0100 Subject: [PATCH 578/624] Introduce errors in test data --- unittests/testdata/sbus2_r7008SB.txt | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/unittests/testdata/sbus2_r7008SB.txt b/unittests/testdata/sbus2_r7008SB.txt index be2bccf0ae..3de5544284 100644 --- a/unittests/testdata/sbus2_r7008SB.txt +++ b/unittests/testdata/sbus2_r7008SB.txt @@ -12,20 +12,6 @@ Time [s],Value,Parity Error,Framing Error 0.001320125,0x80,, 0.001440125,0x00,, 0.001560125,0x04,, -0.001680125,0x20,, -0.001800125,0x00,, -0.001920125,0x01,, -0.002040125,0x08,, -0.002160125,0x40,, -0.002280125,0x00,, -0.002400125,0x02,, -0.002520125,0x10,, -0.002640125,0x80,, -0.002760125,0x00,, -0.002880125,0x04,, -0.00504025,0x03,, -0.00516025,0xC4,, -0.00528025,0x00,, 0.015000625,0x0F,, 0.015120625,0x11,, 0.015240625,0x04,, @@ -113,7 +99,6 @@ Time [s],Value,Parity Error,Framing Error 0.061082625,0x03,, 0.061202625,0x10,, 0.061322625,0x80,, -0.061442625,0x00,, 0.061562625,0x04,, 0.061682625,0x20,, 0.061802625,0x00,, @@ -2117,16 +2102,6 @@ Time [s],Value,Parity Error,Framing Error 1.230450875,0x20,, 1.230570875,0xA8,, 1.230690875,0x01,, -1.230810875,0x08,, -1.230930875,0x16,, -1.231050875,0x50,, -1.231170875,0x03,, -1.231290875,0x10,, -1.231410875,0x80,, -1.231530875,0x00,, -1.231650875,0x04,, -1.231770875,0x20,, -1.231890875,0x00,, 1.232010875,0x01,, 1.232130875,0x08,, 1.232250875,0x40,, From c78f1ace1ca90f3cc9a0d83e2f24f05f03fb451f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 16:22:27 +0100 Subject: [PATCH 579/624] S.BUS: Cleanup and code style fixes --- src/lib/rc/sbus.c | 391 ++++++++++++++++++++++++---------------------- src/lib/rc/sbus.h | 2 +- 2 files changed, 202 insertions(+), 191 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index c1552a058e..aa19bdb3f9 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -77,11 +77,6 @@ #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f -#define SBUS_SYNC_PHASE0 0 -#define SBUS_SYNC_PHASE1 1 - -#define SBUS_DEBUG - #ifdef SBUS_DEBUG #include #endif @@ -113,7 +108,6 @@ static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; static unsigned partial_frame_count; static unsigned sbus_frame_drops; -static unsigned sbus_sync = SBUS_SYNC_PHASE0; unsigned sbus_dropped_frames() @@ -123,7 +117,7 @@ sbus_dropped_frames() static bool sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, - bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); int sbus_init(const char *device, bool singlewire) @@ -238,6 +232,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ uint8_t buf[SBUS_FRAME_SIZE]; bool decoded = false; + do { ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); @@ -249,7 +244,8 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails /* * Try to decode something with what we got */ - decoded = decoded || sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); + decoded = decoded + || sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); } while (ret > 0); @@ -258,7 +254,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) { last_rx_time = now; @@ -275,179 +271,186 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, if (partial_frame_count == sizeof(sbus_frame) / sizeof(sbus_frame[0])) { partial_frame_count = 0; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - #ifdef SBUS_DEBUG +#ifdef SBUS_DEBUG printf("SBUS2: RESET (BUF LIM)\n"); - #endif +#endif } if (partial_frame_count == SBUS_FRAME_SIZE) { partial_frame_count = 0; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - #ifdef SBUS_DEBUG +#ifdef SBUS_DEBUG printf("SBUS2: RESET (PACKET LIM)\n"); - #endif +#endif } - #ifdef SBUS_DEBUG - #if 0 +#ifdef SBUS_DEBUG +#if 0 printf("sbus state: %s%s%s%s%s%s, count: %d, val: %02x\n", - (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", - (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", - partial_frame_count, - (unsigned)frame[d]); - #endif - #endif + (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", + (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", + partial_frame_count, + (unsigned)frame[d]); +#endif +#endif switch (sbus_decode_state) { - case SBUS2_DECODE_STATE_DESYNC: - /* we are de-synced and only interested in the frame marker */ - if (frame[d] == SBUS_START_SYMBOL) { - sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + case SBUS2_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if (frame[d] == SBUS_START_SYMBOL) { + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + partial_frame_count = 0; + sbus_frame[partial_frame_count++] = frame[d]; + } + + break; + + /* fall through */ + case SBUS2_DECODE_STATE_SBUS_START: + case SBUS2_DECODE_STATE_SBUS1_SYNC: + + /* fall through */ + case SBUS2_DECODE_STATE_SBUS2_SYNC: { + sbus_frame[partial_frame_count++] = frame[d]; + + /* decode whatever we got and expect */ + if (partial_frame_count < SBUS_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + + /* + * Offset recovery: If decoding failed, check if there is a second + * start marker in the packet. + */ + unsigned start_index = 0; + + if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) { + + for (unsigned i = 1; i < partial_frame_count; i++) { + if (sbus_frame[i] == SBUS_START_SYMBOL) { + start_index = i; + break; + } + } + + /* we found a second start marker */ + if (start_index != 0) { + /* shift everything in the buffer and reset the state machine */ + for (unsigned i = 0; i < partial_frame_count - start_index; i++) { + sbus_frame[i] = sbus_frame[i + start_index]; + } + + partial_frame_count -= start_index; + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + +#ifdef SBUS_DEBUG + printf("DECODE RECOVERY: %d\n", start_index); +#endif + } + } + + /* if there has been no successful attempt at saving a failed + * decoding run, reset the frame count for successful and + * unsuccessful decode runs. + */ + if (start_index == 0) { partial_frame_count = 0; - sbus_frame[partial_frame_count++] = frame[d]; } - break; - /* fall through */ - case SBUS2_DECODE_STATE_SBUS_START: - case SBUS2_DECODE_STATE_SBUS1_SYNC: - /* fall through */ - case SBUS2_DECODE_STATE_SBUS2_SYNC: - { - sbus_frame[partial_frame_count++] = frame[d]; - /* decode whatever we got and expect */ - if (partial_frame_count < SBUS_FRAME_SIZE) { - break; - } - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + } + break; - /* - * Offset recovery: If decoding failed, check if there is a second - * start marker in the packet. - */ - unsigned start_index = 0; - - if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) { - - for (unsigned i = 1; i < partial_frame_count; i++) { - if (sbus_frame[i] == SBUS_START_SYMBOL) { - start_index = i; - break; - } - } - - /* we found a second start marker */ - if (start_index != 0) { - /* shift everything in the buffer and reset the state machine */ - for (unsigned i = 0; i < partial_frame_count - start_index; i++) { - sbus_frame[i] = sbus_frame[i + start_index]; - } - - partial_frame_count -= start_index; - sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; - - #ifdef SBUS_DEBUG - printf("DECODE RECOVERY: %d\n", start_index); - #endif - } - } - - /* if there has been no successful attempt at saving a failed - * decoding run, reset the frame count for successful and - * unsuccessful decode runs. - */ - if (start_index == 0) { - partial_frame_count = 0; - } + case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: { + sbus_frame[partial_frame_count++] = frame[d]; + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; } - break; - case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: - { - sbus_frame[partial_frame_count++] = frame[d]; - - if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { - /* this slot is unused and in fact S.BUS2 sync */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; - } - - if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { - break; - } - - /* find out which payload we're dealing with in this slot */ - switch(sbus_frame[0]) { - case 0x03: - { - // Observed values: - // (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) - // (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) - // (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) - // (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f) - uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2]; - #ifdef SBUS_DEBUG - //printf("rx_voltage %d\n", (int)rx_voltage); - #endif - } - partial_frame_count = 0; - break; - default: - /* this is not what we expect it to be, go back to sync */ - sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - sbus_frame_drops++; - } + if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { + break; } - break; - case SBUS2_DECODE_STATE_SBUS2_GPS: - { - sbus_frame[partial_frame_count++] = frame[d]; - if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { - /* this slot is unused and in fact S.BUS2 sync */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + /* find out which payload we're dealing with in this slot */ + switch (sbus_frame[0]) { + case 0x03: { + // Observed values: + // (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0) + // (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f) + // (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f) +#ifdef SBUS_DEBUG + //uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2]; + //printf("rx_voltage %d\n", (int)rx_voltage); +#endif } - if (partial_frame_count < 24) { - decode_ret = false; - break; - } + partial_frame_count = 0; + break; - /* find out which payload we're dealing with in this slot */ - switch(sbus_frame[0]) { - case 0x13: - { - uint16_t gps_something = (frame[1] << 8) | frame[2]; - #ifdef SBUS_DEBUG - printf("gps_something %d\n", (int)gps_something); - #endif - } - partial_frame_count = 0; - break; - default: - /* this is not what we expect it to be, go back to sync */ - sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - sbus_frame_drops++; - /* throw unknown bytes away */ - } + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; } - break; - default: - #ifdef SBUS_DEBUG - printf("UNKNOWN PROTO STATE"); - #endif - decode_ret = false; + + } + break; + + case SBUS2_DECODE_STATE_SBUS2_GPS: { + sbus_frame[partial_frame_count++] = frame[d]; + + if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { + /* this slot is unused and in fact S.BUS2 sync */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + } + + if (partial_frame_count < 24) { + decode_ret = false; + break; + } + + /* find out which payload we're dealing with in this slot */ + switch (sbus_frame[0]) { + case 0x13: { +#ifdef SBUS_DEBUG + uint16_t gps_something = (frame[1] << 8) | frame[2]; + printf("gps_something %d\n", (int)gps_something); +#endif + } + + partial_frame_count = 0; + break; + + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + } + } + break; + + default: +#ifdef SBUS_DEBUG + printf("UNKNOWN PROTO STATE"); +#endif + decode_ret = false; } - + } *frame_drops = sbus_frame_drops; @@ -494,50 +497,57 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { bool sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, - bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != SBUS_START_SYMBOL)) { sbus_frame_drops++; - #ifdef SBUS_DEBUG +#ifdef SBUS_DEBUG printf("DECODE FAIL: "); + for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { printf("%0x ", frame[i]); } + printf("\n"); - #endif +#endif sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; return false; } switch (frame[24]) { - case 0x00: - /* this is S.BUS 1 */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC; - break; - case 0x04: - /* receiver voltage */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE; - break; - case 0x14: - /* GPS / baro */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS; - break; - case 0x24: - /* Unknown SBUS2 data */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; - break; - case 0x34: - /* Unknown SBUS2 data */ - sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; - break; - default: - #ifdef SBUS_DEBUG - printf("DECODE FAIL: END MARKER\n"); - #endif - sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; - return false; + case 0x00: + /* this is S.BUS 1 */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC; + break; + + case 0x04: + /* receiver voltage */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE; + break; + + case 0x14: + /* GPS / baro */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS; + break; + + case 0x24: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + + case 0x34: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + + default: +#ifdef SBUS_DEBUG + printf("DECODE FAIL: END MARKER\n"); +#endif + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + return false; } /* we have received something we think is a frame */ @@ -565,7 +575,7 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ @@ -586,16 +596,17 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ *sbus_failsafe = false; *sbus_frame_drop = true; + } else { *sbus_failsafe = false; *sbus_frame_drop = false; diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 8bc6e808b3..7b13f27f79 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -73,7 +73,7 @@ __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bo bool *sbus_frame_drop, uint16_t max_channels); __EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); From 7c562d57fe3e89f7a6d05d2d655be4fbf2edebbd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 16:23:08 +0100 Subject: [PATCH 580/624] S.BUS test: Fix code style --- unittests/sbus2_test.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 69882a4693..828f2a6160 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -4,6 +4,8 @@ #include "../../src/systemcmds/tests/tests.h" #include +// Enable S.BUS parser output +#define SBUS_DEBUG #include #include #include @@ -61,17 +63,18 @@ TEST(SBUS2Test, SBUS2) frame[0] = x; unsigned len = 1; - + // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); // if (rate_limiter % byte_offset == 0) { - bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values, - &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + + if (result) { + //warnx("decoded packet"); + } - if (result) { - //warnx("decoded packet"); - } // } if (last_drop != (sbus_frame_drops + sbus_frame_resets)) { From 3be7a3323e79f651a70228263513a7d8b36d6a7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 16:30:00 +0100 Subject: [PATCH 581/624] Enable topic listener for Pixracer config --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 720b9e74b8..a6dd256a34 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -36,7 +36,7 @@ set(config_module_list drivers/meas_airspeed drivers/frsky_telemetry modules/sensors - #drivers/mkblctrl + drivers/mkblctrl drivers/px4flow drivers/oreoled drivers/gimbal @@ -53,7 +53,7 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot - #systemcmds/topic_listener + systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm From 6462deebadd68a26d386eeec2c919a242fb2aea5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 17:36:12 +0100 Subject: [PATCH 582/624] SBUS2 parser: Last set of fixes, validated in test and against Futaba TX --- src/lib/rc/sbus.c | 46 +++++++++++++++++++--------------------------- src/lib/rc/sbus.h | 1 - 2 files changed, 19 insertions(+), 28 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index aa19bdb3f9..5b8ff4dccb 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -49,8 +49,6 @@ #define SBUS_START_SYMBOL 0x0f -#define SBUS_FRAME_SIZE 25 -#define SBUS_MAX_BUF_SIZE (SBUS_FRAME_SIZE + 10) #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 @@ -199,7 +197,7 @@ bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { - ssize_t ret; + int ret = 1; hrt_abstime now; /* @@ -219,37 +217,30 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); - if ((now - last_rx_time) > SBUS_INTER_FRAME_TIMEOUT) { - if (partial_frame_count > 0) { - sbus_frame_drops++; - partial_frame_count = 0; - } - } - /* * Fetch bytes, but no more than we would need to complete * a complete frame. */ - uint8_t buf[SBUS_FRAME_SIZE]; - bool decoded = false; + uint8_t buf[SBUS_FRAME_SIZE * 2]; + bool sbus_decoded = false; - do { - ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); + ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - return false; - } + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + return false; + } - /* - * Try to decode something with what we got - */ - decoded = decoded - || sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); + /* + * Try to decode something with what we got + */ + if (sbus_parse(now, &buf[0], ret, values, num_values, sbus_failsafe, + sbus_frame_drop, &sbus_frame_drops, max_channels)) { - } while (ret > 0); + sbus_decoded = true; + } - return decoded; + return sbus_decoded; } bool @@ -418,7 +409,6 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, } if (partial_frame_count < 24) { - decode_ret = false; break; } @@ -453,7 +443,9 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, } - *frame_drops = sbus_frame_drops; + if (frame_drops) { + *frame_drops = sbus_frame_drops; + } /* return false as default */ return decode_ret; diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 7b13f27f79..0da5ea2935 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -46,7 +46,6 @@ __BEGIN_DECLS -#define SBUS_INTER_FRAME_TIMEOUT 3000 /**< 3000 us frame timeout */ #define SBUS_FRAME_SIZE 25 #define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2) From ec40c8268c7cf1136a53de9c205915de9eedc098 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 17:36:51 +0100 Subject: [PATCH 583/624] SBUS2: Code style fix --- src/lib/rc/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 5b8ff4dccb..f911648416 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -235,7 +235,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails * Try to decode something with what we got */ if (sbus_parse(now, &buf[0], ret, values, num_values, sbus_failsafe, - sbus_frame_drop, &sbus_frame_drops, max_channels)) { + sbus_frame_drop, &sbus_frame_drops, max_channels)) { sbus_decoded = true; } From ccd9990afdb126384a5b8cd69f06fca0a541e5f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 18:50:38 +0100 Subject: [PATCH 584/624] Fix FMUv2 build --- src/drivers/px4fmu/fmu.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 70ed184d0c..892d224f58 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -2242,7 +2242,9 @@ fmu_main(int argc, char *argv[]) } if (!strcmp(verb, "info")) { +#ifdef SBUS_SERIAL_PORT warnx("frame drops: %u", sbus_dropped_frames()); +#endif return 0; } From c253ce6695f289bddfc81648ac7c793575b0376b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Dec 2015 19:40:10 +0100 Subject: [PATCH 585/624] FMU SBUS interface: Set RSSI and failsafe flags correctly --- src/drivers/px4fmu/fmu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 892d224f58..8892cfedeb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -852,8 +852,8 @@ PX4FMU::cycle() _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; _rc_in.rc_ppm_frame_length = 0; - _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0; - _rc_in.rc_failsafe = false; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + _rc_in.rc_failsafe = sbus_failsafe; _rc_in.rc_lost = false; _rc_in.rc_lost_frame_count = sbus_dropped_frames(); _rc_in.rc_total_frame_count = 0; From ba00b2742ecc56093cc9dd271843315a02d62b30 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 13:10:30 +0100 Subject: [PATCH 586/624] QAV250 config: Set higher max rates by default for attitude --- ROMFS/px4fmu_common/init.d/4009_qav250 | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index dfd94992af..aed6aef3b7 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -30,4 +30,7 @@ then param set PWM_MIN 980 param set MPC_THR_MIN 0.06 param set MPC_MANTHR_MIN 0.06 + param set MC_ROLLRATE_MAX 360.0 + param set MC_PITCHRATE_MAX 360.0 + param set MC_YAWRATE_MAX 200.0 fi From 98a0a4042c67144100321ce83af8b1f16067e4a2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Dec 2015 05:46:48 +0100 Subject: [PATCH 587/624] ignore gtags/gnu global output files https://www.gnu.org/software/global/ --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 14183ed177..07848d7a90 100644 --- a/.gitignore +++ b/.gitignore @@ -57,3 +57,6 @@ ROMFS/*/*/rc.autostart rootfs/ *.autosave CMakeLists.txt.user +GPATH +GRTAGS +GTAGS From 8cf9b7820ef35cc57d40410636277c9c258d219e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Dec 2015 12:46:32 +0100 Subject: [PATCH 588/624] Updated README --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index eb33fff02e..abbf0975a6 100644 --- a/README.md +++ b/README.md @@ -38,10 +38,10 @@ Testing guide: http://px4.io/dev/unit_tests This repository contains code supporting these boards: - * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/) + * [Snapdragon Flight](http://dev.px4.io/hardware-snapdragon.html) * FMUv1.x - * FMUv2.x (Pixhawk) - * FMUv3.x (Pixhawk 2) + * FMUv2.x ([Pixhawk](http://dev.px4.io/hardware-pixhawk.html), Pixhawk 2 and [Pixfalcon](http://dev.px4.io/hardware-pixfalcon.html)) + * FMUv4.x (Pixhawk X and [Pixracer](http://dev.px4.io/hardware-pixracer.html)) * AeroCore (v1 and v2) * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery) From 18cd49292ec838565b291f1b533e5769601190b7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 3 Dec 2015 14:18:00 -0500 Subject: [PATCH 589/624] split FW_ATT_TC into FW_R_TC and FW_P_TC --- .../fw_att_control/fw_att_control_main.cpp | 16 +++++++++----- .../fw_att_control/fw_att_control_params.c | 22 ++++++++++++++++--- 2 files changed, 29 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 5d8a694877..fc4b570687 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -168,13 +168,14 @@ private: struct { - float tconst; + float p_tc; float p_p; float p_i; float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; + float r_tc; float r_p; float r_i; float r_ff; @@ -217,13 +218,14 @@ private: struct { - param_t tconst; + param_t p_tc; param_t p_p; param_t p_i; param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; + param_t r_tc; param_t r_p; param_t r_i; param_t r_ff; @@ -387,7 +389,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _vehicle_status = {}; - _parameter_handles.tconst = param_find("FW_ATT_TC"); + _parameter_handles.p_tc = param_find("FW_P_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); _parameter_handles.p_ff = param_find("FW_PR_FF"); @@ -395,6 +397,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); + _parameter_handles.r_tc = param_find("FW_R_TC"); _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_i = param_find("FW_RR_I"); _parameter_handles.r_ff = param_find("FW_RR_FF"); @@ -471,7 +474,7 @@ int FixedwingAttitudeControl::parameters_update() { - param_get(_parameter_handles.tconst, &(_parameters.tconst)); + param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); @@ -479,6 +482,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.r_tc, &(_parameters.r_tc)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); @@ -522,7 +526,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ - _pitch_ctrl.set_time_constant(_parameters.tconst); + _pitch_ctrl.set_time_constant(_parameters.p_tc); _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_ff(_parameters.p_ff); @@ -531,7 +535,7 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); /* roll control parameters */ - _roll_ctrl.set_time_constant(_parameters.tconst); + _roll_ctrl.set_time_constant(_parameters.r_tc); _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_ff(_parameters.r_ff); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index ce2db9ad5c..2782c5b542 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -46,9 +46,9 @@ */ /** - * Attitude Time Constant + * Attitude Roll Time Constant * - * This defines the latency between a step input and the achieved setpoint + * This defines the latency between a roll step input and the achieved setpoint * (inverse to a P gain). Half a second is a good start value and fits for * most average systems. Smaller systems may require smaller values, but as * this will wear out servos faster, the value should only be decreased as @@ -59,7 +59,23 @@ * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.4f); +PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); + +/** + * Attitude Pitch Time Constant + * + * This defines the latency between a pitch step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit seconds + * @min 0.2 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f); /** * Pitch rate proportional gain. From daf1b764eaec439c6fedb0a54f5fef148d60165c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Dec 2015 11:54:26 +0100 Subject: [PATCH 590/624] RC check: Remove redundant RC cal check --- src/modules/commander/commander.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 14aa97d375..1f5453aed5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1187,8 +1187,6 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd, true); - /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -1439,9 +1437,6 @@ int commander_thread_main(int argc, char *argv[]) status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } - - /* re-check RC calibration */ - rc_calibration_check(mavlink_fd, true); } /* Safety parameters */ From 0a56ba88737f24b9a16351536c54de5cb872c376 Mon Sep 17 00:00:00 2001 From: edkoch Date: Mon, 7 Dec 2015 13:30:38 -0800 Subject: [PATCH 591/624] Added new telemetry stream to display vision pose updates --- src/modules/mavlink/mavlink_main.cpp | 3 ++ src/modules/mavlink/mavlink_messages.cpp | 67 ++++++++++++++++++++++++ 2 files changed, 70 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9af32f4b13..07b768354f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1690,6 +1690,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("OPTICAL_FLOW_RAD", 5.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); configure_stream("ALTITUDE", 1.0f); + configure_stream("VISION_POSITION_NED", 10.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1717,6 +1718,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); configure_stream("ALTITUDE", 10.0f); + configure_stream("VISION_POSITION_NED", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1764,6 +1766,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); configure_stream("ALTITUDE", 10.0f); + configure_stream("VISION_POSITION_NED", 10.0f); default: break; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 798849f116..1a5c6664b2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -79,6 +80,8 @@ #include #include +#include + #include "mavlink_messages.h" #include "mavlink_main.h" @@ -1216,6 +1219,69 @@ protected: } }; +class MavlinkStreamVisionPositionNED : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamVisionPositionNED::get_name_static(); + } + + static const char *get_name_static() + { + return "VISION_POSITION_NED"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamVisionPositionNED(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } +private: + + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamVisionPositionNED(MavlinkStreamVisionPositionNED &); + MavlinkStreamVisionPositionNED& operator = (const MavlinkStreamVisionPositionNED &); + +protected: + explicit MavlinkStreamVisionPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vision_position_estimate))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vision_position_estimate_s vpos; + memset(&vpos, 0, sizeof(vpos)); + + if (_pos_sub->update(&_pos_time, &vpos)) { + mavlink_vision_position_estimate_t vmsg; + vmsg.usec = vpos.timestamp_boot / 1000; + vmsg.x = vpos.x; + vmsg.y = vpos.y; + vmsg.z = vpos.z; + math::Quaternion q(vpos.q); + math::Vector<3> rpy = q.to_euler(); + vmsg.roll = rpy(0); + vmsg.pitch = rpy(1); + vmsg.yaw = rpy(2); + + _mavlink->send_message(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, &vmsg); + } + } +}; class MavlinkStreamLocalPositionNED : public MavlinkStream { @@ -2644,6 +2710,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static), new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static), From 4bce677703b0bcd23887ad243c979587329f4c5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 23:51:26 +0100 Subject: [PATCH 592/624] Drop ignore voltage to prevent false positives on targets missing appropriate pulldown resistors --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b7bfba8ffc..b20b33011e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -122,7 +122,7 @@ using namespace DriverFramework; #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 2.5f +#define BATT_V_IGNORE_THRESHOLD 2.1f /** * HACK - true temperature is much less than indicated temperature in baro, From 47ff8b6bc9dbd1bd7aa3d7c1310a9234085b6d99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 15:34:24 +0100 Subject: [PATCH 593/624] IO Firmware: Let pilot take control if FMU is down --- src/modules/px4iofirmware/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index fa8d5ad48a..f7321a966d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -487,7 +487,7 @@ controls_tick() * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + if ((!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { From 48bbe8f6badaa626236ef4c026ce4d8c75a1c633 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 15:34:53 +0100 Subject: [PATCH 594/624] IO Firmware: Default override behavior to immediate override --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a3351bcbd5..951de1f044 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -153,7 +153,7 @@ volatile uint16_t r_page_setup[] = { #else [PX4IO_P_SETUP_FEATURES] = 0, #endif - [PX4IO_P_SETUP_ARMING] = 0, + [PX4IO_P_SETUP_ARMING] = (PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE), [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, From 27e1aaeea5b4a464f74c51e9418fee8afd50d95e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 08:50:21 +0100 Subject: [PATCH 595/624] control state: indicate if airspeed is not valid --- msg/control_state.msg | 1 + .../attitude_estimator_q/attitude_estimator_q_main.cpp | 9 ++++++++- .../ekf_att_pos_estimator_main.cpp | 9 ++++++++- src/modules/fw_att_control/fw_att_control_main.cpp | 4 +++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 +++++----- 5 files changed, 25 insertions(+), 8 deletions(-) diff --git a/msg/control_state.msg b/msg/control_state.msg index 4f130353d3..e504a765da 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -10,6 +10,7 @@ float32 x_pos # X position in local frame float32 y_pos # Y position in local frame float32 z_pos # z position in local frame float32 airspeed # Airspeed, estimated +bool airspeed_valid # False: Non-finite values or non-updating sensor float32[3] vel_variance # Variance in body velocity estimate float32[3] pos_variance # Variance in local position estimate float32[4] q # Attitude Quaternion diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f456e73120..3e44579c66 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -623,7 +623,14 @@ void AttitudeEstimatorQ::task_main() ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); /* Airspeed - take airspeed measurement directly here as no wind is estimated */ - ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 + && _airspeed.timestamp > 0) { + ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + ctrl_state.airspeed_valid = true; + + } else { + ctrl_state.airspeed_valid = false; + } /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4a03af7890..321f742fb5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -896,7 +896,14 @@ void AttitudePositionEstimatorEKF::publishControlState() //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL // and in outdoor tests - _ctrl_state.airspeed = _airspeed.true_airspeed_m_s; + if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 + && _airspeed.timestamp > 0) { + _ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; + _ctrl_state.airspeed_valid = true; + + } else { + _ctrl_state.airspeed_valid = false; + } /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; 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 fc4b570687..0ea5a20f8c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -889,11 +889,13 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) { + if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) { airspeed = _parameters.airspeed_trim; + if (nonfinite) { perf_count(_nonfinite_input_perf); } + } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ airspeed = math::max(0.5f, _ctrl_state.airspeed); 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 8b4e4224eb..752b1fd793 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 @@ -229,7 +229,7 @@ private: /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists - uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures + uint64_t _airspeed_last_received; ///< last time airspeed was received. Used to detect timeouts. float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid math::Matrix<3, 3> _R_nb; ///< current attitude @@ -558,7 +558,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), - _airspeed_last_valid(0), + _airspeed_last_received(0), _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), @@ -807,13 +807,13 @@ FixedwingPositionControl::control_state_poll() if (ctrl_state_updated) { orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - _airspeed_valid = true; - _airspeed_last_valid = hrt_absolute_time(); + _airspeed_valid = _ctrl_state.airspeed_valid; + _airspeed_last_received = hrt_absolute_time(); } else { /* no airspeed updates for one second */ - if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) { _airspeed_valid = false; } } From 4df4d2ff9348279be3f90e93be9cf0527b5441e1 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 08:51:55 +0100 Subject: [PATCH 596/624] code style --- .../ekf_att_pos_estimator_main.cpp | 220 ++++---- .../fw_att_control/fw_att_control_main.cpp | 109 ++-- .../fw_pos_control_l1_main.cpp | 506 ++++++++++-------- 3 files changed, 486 insertions(+), 349 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 321f742fb5..f9b0b1cbca 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -74,9 +74,12 @@ static uint64_t IMUusec = 0; static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure -static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) -static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) -static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +static constexpr unsigned MAG_SWITCH_HYSTERESIS = + 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) +static constexpr unsigned GYRO_SWITCH_HYSTERESIS = + 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) +static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = + 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) static constexpr float EPH_LARGE_VALUE = 1000.0f; static constexpr float EPV_LARGE_VALUE = 1000.0f; @@ -118,7 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_running(false), _estimator_task(-1), -/* subscriptions */ + /* subscriptions */ _sensor_combined_sub(-1), _distance_sub(-1), _airspeed_sub(-1), @@ -163,7 +166,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined{}, - _pos_ref{}, + _pos_ref{}, _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -177,7 +180,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ + /* states */ _gps_alt_filt(0.0f), _baro_alt_filt(0.0f), _covariancePredictionDt(0.0f), @@ -270,6 +273,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } + delete _terrain_estimator; delete _ekf; @@ -324,13 +328,13 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF - #if 0 +#if 0 // Initially disable loading until // convergence is flight-test proven _ekf->magBias.x = _mag_offset_x.get(); _ekf->magBias.y = _mag_offset_y.get(); _ekf->magBias.z = _mag_offset_z.get(); - #endif +#endif } return OK; @@ -453,18 +457,18 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s", - ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { PX4_INFO("timeout: %s%s%s%s", - ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), - ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), - ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -476,7 +480,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Copy diagonal elemnts of covariance matrix float covariances[28]; _ekf->get_covariance(covariances); - + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; rep.covariances[i] = covariances[i]; @@ -645,6 +649,7 @@ void AttitudePositionEstimatorEKF::task_main() if (_gpsIsGood) { _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; } + // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); @@ -733,13 +738,15 @@ void AttitudePositionEstimatorEKF::task_main() } void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, - bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (PX4_ISFINITE(_ekf->states[9])) { _filter_ref_offset = _ekf->states[9]; + } else if (PX4_ISFINITE(-_ekf->hgtMea)) { _filter_ref_offset = -_ekf->hgtMea; + } else { _filter_ref_offset = -_baro.altitude; } @@ -795,11 +802,11 @@ void AttitudePositionEstimatorEKF::initializeGPS() #if 0 PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, - (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_filter_ref_offset); + (double)_filter_ref_offset); PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, - (double)math::degrees(declination)); + (double)math::degrees(declination)); #endif _gps_initialized = true; @@ -904,6 +911,7 @@ void AttitudePositionEstimatorEKF::publishControlState() } else { _ctrl_state.airspeed_valid = false; } + /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; @@ -911,12 +919,11 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Guard from bad data */ if (!PX4_ISFINITE(_ctrl_state.x_vel) || - !PX4_ISFINITE(_ctrl_state.y_vel) || - !PX4_ISFINITE(_ctrl_state.z_vel) || - !PX4_ISFINITE(_ctrl_state.x_pos) || - !PX4_ISFINITE(_ctrl_state.y_pos) || - !PX4_ISFINITE(_ctrl_state.z_pos)) - { + !PX4_ISFINITE(_ctrl_state.y_vel) || + !PX4_ISFINITE(_ctrl_state.z_vel) || + !PX4_ISFINITE(_ctrl_state.x_pos) || + !PX4_ISFINITE(_ctrl_state.y_pos) || + !PX4_ISFINITE(_ctrl_state.z_pos)) { // bad data, abort publication return; } @@ -956,12 +963,11 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.yaw = _att.yaw; if (!PX4_ISFINITE(_local_pos.x) || - !PX4_ISFINITE(_local_pos.y) || - !PX4_ISFINITE(_local_pos.z) || - !PX4_ISFINITE(_local_pos.vx) || - !PX4_ISFINITE(_local_pos.vy) || - !PX4_ISFINITE(_local_pos.vz)) - { + !PX4_ISFINITE(_local_pos.y) || + !PX4_ISFINITE(_local_pos.z) || + !PX4_ISFINITE(_local_pos.vx) || + !PX4_ISFINITE(_local_pos.vy) || + !PX4_ISFINITE(_local_pos.vz)) { // bad data, abort publication return; } @@ -987,6 +993,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { _global_pos.lat = 0.0; _global_pos.lon = 0.0; @@ -1007,6 +1014,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { _global_pos.vel_d = 0.0f; } @@ -1015,6 +1023,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_terrain_estimator->is_valid()) { _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); _global_pos.terrain_alt_valid = true; + } else { _global_pos.terrain_alt_valid = false; } @@ -1028,8 +1037,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (!_local_pos.xy_global || !_local_pos.v_xy_valid || - _gps.timestamp_position == 0 || - (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; @@ -1040,12 +1049,11 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } if (!PX4_ISFINITE(_global_pos.lat) || - !PX4_ISFINITE(_global_pos.lon) || - !PX4_ISFINITE(_global_pos.alt) || - !PX4_ISFINITE(_global_pos.vel_n) || - !PX4_ISFINITE(_global_pos.vel_e) || - !PX4_ISFINITE(_global_pos.vel_d)) - { + !PX4_ISFINITE(_global_pos.lon) || + !PX4_ISFINITE(_global_pos.alt) || + !PX4_ISFINITE(_global_pos.vel_n) || + !PX4_ISFINITE(_global_pos.vel_e) || + !PX4_ISFINITE(_global_pos.vel_d)) { // bad data, abort publication return; } @@ -1102,6 +1110,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) { _prediction_steps++; return; + } else { _prediction_steps = 0; _prediction_last = hrt_absolute_time(); @@ -1221,11 +1230,11 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 4600, - (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 4600, + (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + nullptr); if (_estimator_task < 0) { warn("task start failed"); @@ -1242,7 +1251,7 @@ void AttitudePositionEstimatorEKF::print_status() math::Vector<3> euler = R.to_euler(); PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: // 0-3: quaternions (q0, q1, q2, q3) @@ -1258,51 +1267,51 @@ void AttitudePositionEstimatorEKF::print_status() PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, - (double)_baro_gps_offset); + (double)_baro_gps_offset); PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, - (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, - (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, - (double)_ekf->correctedDelAng.z); + (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, + (double)_ekf->correctedDelAng.z); PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], - (double)_ekf->states[2], (double)_ekf->states[3]); + (double)_ekf->states[2], (double)_ekf->states[3]); PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], - (double)_ekf->states[6]); + (double)_ekf->states[6]); PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8], - (double)_ekf->states[9]); + (double)_ekf->states[9]); PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11], - (double)_ekf->states[12]); + (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], - (double)_ekf->states[18]); + (double)_ekf->states[18]); PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], - (double)_ekf->states[21]); + (double)_ekf->states[21]); PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], - (double)_ekf->states[18]); + (double)_ekf->states[18]); PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], - (double)_ekf->states[21]); + (double)_ekf->states[21]); } PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); PX4_INFO("gyro status:"); _voter_gyro.print(); @@ -1333,6 +1342,7 @@ void AttitudePositionEstimatorEKF::pollData() if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) { deltaT = _ekf->dtIMUfilt; + } else { deltaT = 0.01f; } @@ -1345,16 +1355,17 @@ void AttitudePositionEstimatorEKF::pollData() for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], - _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], - _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], - _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); } // Get best measurement values hrt_abstime curr_time = hrt_absolute_time(); (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { // Use pre-integrated values if possible @@ -1362,12 +1373,15 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + } else { float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; + if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { deltaT = dt_gyro; _ekf->dtIMU = deltaT; } + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; @@ -1380,6 +1394,7 @@ void AttitudePositionEstimatorEKF::pollData() } (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { // Use pre-integrated values if possible @@ -1387,6 +1402,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + } else { _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; @@ -1400,9 +1416,10 @@ void AttitudePositionEstimatorEKF::pollData() } (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], - _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); /* fail over to the 2nd mag if we know the first is down */ if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { @@ -1416,26 +1433,28 @@ void AttitudePositionEstimatorEKF::pollData() } if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { _failsafe = true; mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { if (_vibration_warning_timestamp == 0) { _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { _vibration_warning = true; mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", - (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), - (int)(100 * _voter_accel.get_vibration_factor(curr_time)), - (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); } + } else { _vibration_warning_timestamp = 0; } @@ -1444,7 +1463,7 @@ void AttitudePositionEstimatorEKF::pollData() // XXX this is for assessing the filter performance // leave this in as long as larger improvements are still being made. - #if 0 +#if 0 float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; @@ -1455,32 +1474,32 @@ void AttitudePositionEstimatorEKF::pollData() if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) { PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", - (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, - dtoverflow5, dtoverflow10); + (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, + dtoverflow5, dtoverflow10); PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, - (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); + (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), - (double)(_sensor_combined.accelerometer_integral_m_s[0]), - (double)(_sensor_combined.accelerometer_integral_m_s[1]), - (double)(_sensor_combined.accelerometer_integral_m_s[2])); + (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), + (double)(_sensor_combined.accelerometer_integral_m_s[0]), + (double)(_sensor_combined.accelerometer_integral_m_s[1]), + (double)(_sensor_combined.accelerometer_integral_m_s[2])); PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", - (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); + (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", - (double)_sensor_combined.gyro_rad_s[0], - (double)_sensor_combined.gyro_rad_s[1], - (double)_sensor_combined.gyro_rad_s[2]); + (double)_sensor_combined.gyro_rad_s[0], + (double)_sensor_combined.gyro_rad_s[1], + (double)_sensor_combined.gyro_rad_s[2]); lastprint = hrt_absolute_time(); } @@ -1492,7 +1511,8 @@ void AttitudePositionEstimatorEKF::pollData() if (deltaT > 0.01f) { dtoverflow10++; } - #endif + +#endif _data_good = true; @@ -1652,6 +1672,7 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; + if (!_baro_init) { _baro_init = true; _baro_alt_filt = _baro.altitude; @@ -1674,8 +1695,9 @@ void AttitudePositionEstimatorEKF::pollData() if (_newRangeData) { orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) - && (_distance.current_distance < _distance.max_distance)) { + && (_distance.current_distance < _distance.max_distance)) { _ekf->rngMea = _distance.current_distance; _distance_last_valid = _distance.timestamp; 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 0ea5a20f8c..b5e49d67c8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include @@ -347,7 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_running(false), _control_task(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _accel_sub(-1), _vcontrol_mode_sub(-1), @@ -356,7 +356,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos_sub(-1), _vehicle_status_sub(-1), -/* publications */ + /* publications */ _rate_sp_pub(nullptr), _attitude_sp_pub(nullptr), _actuators_0_pub(nullptr), @@ -366,11 +366,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_id(0), _attitude_setpoint_id(0), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ + /* states */ _setpoint_valid(false), _debug(false), _flaps_cmd_last(0), @@ -635,12 +635,14 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_rates_sp_id) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); @@ -727,8 +729,9 @@ FixedwingAttitudeControl::task_main() last_run = hrt_absolute_time(); /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (deltaT > 1.0f) { deltaT = 0.01f; + } /* load local copies */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); @@ -821,6 +824,7 @@ FixedwingAttitudeControl::task_main() if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; //warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { _actuators_airframe.control[7] = 0.0f; //warnx("_actuators_airframe.control[1] = -1.0f;"); @@ -838,13 +842,15 @@ FixedwingAttitudeControl::task_main() /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { - flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; + flaps_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } // move the actual control value continuous with time static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { t_flaps_changed = hrt_absolute_time(); delta_flaps = flaps_control - _flaps_cmd_last; @@ -865,12 +871,14 @@ FixedwingAttitudeControl::task_main() /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } // move the actual control value continuous with time static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { t_flaperons_changed = hrt_absolute_time(); delta_flaperon = flaperon - _flaperons_cmd_last; @@ -880,7 +888,8 @@ FixedwingAttitudeControl::task_main() static float flaperon_applied = 0.0f; if (fabsf(flaperon_applied - flaperon) > 0.01f) { - flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * + (delta_flaperon) / 1000000; } /* decide if in stabilized or full manual control */ @@ -908,13 +917,14 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : + airspeed); /* Use min airspeed to calculate ground speed scaling region. * Don't scale below gspd_scaling_trim */ float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); + _global_pos.vel_e * _global_pos.vel_e); float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); @@ -930,7 +940,7 @@ FixedwingAttitudeControl::task_main() * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_auto_enabled || - !_vcontrol_mode.flag_control_manual_enabled) { + !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; @@ -941,13 +951,16 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } + } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* the pilot does not want to change direction, @@ -955,9 +968,10 @@ FixedwingAttitudeControl::task_main() */ if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { roll_sp = (_manual.y * _parameters.man_roll_max) - + _parameters.rollsp_offset_rad; + + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; @@ -967,16 +981,18 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; @@ -987,13 +1003,16 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } + } else { /* * Scale down roll and pitch as the setpoints are radians @@ -1029,6 +1048,7 @@ FixedwingAttitudeControl::task_main() if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); + } else if (_attitude_setpoint_id) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); @@ -1090,6 +1110,7 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1101,26 +1122,29 @@ FixedwingAttitudeControl::task_main() float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), - (double)airspeed, (double)airspeed_scaling, - (double)roll_sp, (double)pitch_sp, - (double)_roll_ctrl.get_desired_rate(), - (double)_pitch_ctrl.get_desired_rate(), - (double)_att_sp.roll_body); + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); } } float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } @@ -1128,14 +1152,17 @@ FixedwingAttitudeControl::task_main() else { yaw_u = _yaw_ctrl.control_bodyrate(control_input); } + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -1144,16 +1171,19 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && - !(_vehicle_status.engine_failure || - _vehicle_status.engine_failure_cmd)) ? - throttle_sp : 0.0f; + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } + } else { perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } @@ -1172,6 +1202,7 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); @@ -1197,15 +1228,15 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ - if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) - { + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else if (_actuators_id) { - _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub != nullptr) { @@ -1236,11 +1267,11 @@ FixedwingAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1300, - (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1300, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1287,13 +1318,15 @@ int fw_att_control_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); } + return 0; } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr){ + if (att_control::g_control == nullptr) { warnx("not running"); return 1; } 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 752b1fd793..d7aebbe0a8 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 @@ -108,8 +108,10 @@ static int _control_task = -1; /**< task handle for sensor task */ #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) #define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid -static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float THROTTLE_THRESH = + 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = + 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** @@ -400,7 +402,7 @@ private: * @param waypoint_next the waypoint in the heading direction */ void get_waypoint_heading_distance(float heading, float distance, - struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); /** * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available @@ -439,7 +441,8 @@ private: const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, + const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -466,13 +469,13 @@ private: * XXX need to clean up/remove this function once mtecs fully replaces TECS */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, - float pitch_min_rad, float pitch_max_rad, - float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad, - float altitude, - const math::Vector<3> &ground_speed, - unsigned mode = tecs_status_s::TECS_MODE_NORMAL, - bool pitch_max_special = false); + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + unsigned mode = tecs_status_s::TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -494,7 +497,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _task_should_exit(false), _task_running(false), -/* subscriptions */ + /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), _ctrl_state_sub(-1), @@ -504,15 +507,15 @@ FixedwingPositionControl::FixedwingPositionControl() : _manual_control_sub(-1), _sensor_combined_sub(-1), -/* publications */ + /* publications */ _attitude_sp_pub(nullptr), _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), -/* publication ID */ + /* publication ID */ _attitude_setpoint_id(0), -/* states */ + /* states */ _ctrl_state(), _att_sp(), _nav_capabilities(), @@ -523,7 +526,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet(), _sensor_combined(), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), @@ -590,7 +593,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); - _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_use_terrain_estimate = param_find("FW_LND_USETER"); _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -691,7 +694,8 @@ FixedwingPositionControl::parameters_update() _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; } - param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.land_heading_hold_horizontal_distance, + &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); @@ -732,7 +736,8 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, + _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -772,10 +777,12 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } @@ -827,7 +834,7 @@ FixedwingPositionControl::control_state_poll() _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); - + /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); } @@ -904,7 +911,8 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, + const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { @@ -917,11 +925,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; + if (pos_sp_triplet.previous.valid) { - distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, + pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; + } else { - distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, + pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } @@ -950,13 +962,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() if (_nav_capabilities_pub != nullptr) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); } } void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance, - struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) { waypoint_prev.valid = true; waypoint_prev.alt = _hold_alt; @@ -965,6 +978,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa // on init set first waypoint to momentary position waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + } else { /* for previous waypoint use the one still in front of us but shift it such that it is @@ -980,7 +994,8 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa waypoint_next.alt = _hold_alt; } -float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, + const struct vehicle_global_position_s &global_pos) { if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; @@ -989,17 +1004,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { - if(!land_useterrain) { + if (!land_useterrain) { mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } + return global_pos.terrain_alt; + } else { return land_setpoint_alt; } } -float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, + const struct vehicle_global_position_s &global_pos) { if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { return global_pos.terrain_alt; @@ -1048,33 +1066,39 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) float pitch = -(_manual.x - deadBand) / factor; _hold_alt += (_parameters.max_sink_rate * dt) * pitch; _was_in_deadband = false; + } else if (_manual.x < - deadBand) { /* pitching up */ float pitch = -(_manual.x + deadBand) / factor; _hold_alt += (_parameters.max_climb_rate * dt) * pitch; _was_in_deadband = false; climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); + } else if (!_was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; _althold_epv = _global_pos.epv; _was_in_deadband = true; } + if (_vehicle_status.is_vtol) { if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { _hold_alt = _global_pos.alt; } } + return climbout_mode; } -bool FixedwingPositionControl::in_takeoff_situation() { +bool FixedwingPositionControl::in_takeoff_situation() +{ const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold + && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { return true; } @@ -1087,6 +1111,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc if (in_takeoff_situation()) { *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; *pitch_limit_min = math::radians(10.0f); + } else { *pitch_limit_min = _parameters.pitch_limit_min; } @@ -1097,9 +1122,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi const struct position_setpoint_triplet_s &pos_sp_triplet) { float dt = 0.01; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } + _control_position_last_called = hrt_absolute_time(); bool setpoint = true; @@ -1114,14 +1141,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_vehicle_status.condition_landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); /* update TECS filters */ if (!_mTecs.getEnabled()) { _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; @@ -1139,13 +1166,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _time_went_in_air = hrt_absolute_time(); _takeoff_ground_alt = _global_pos.alt; } + /* reset flag when airplane landed */ if (_vehicle_status.condition_landed) { _was_in_air = false; } if (_control_mode.flag_control_auto_enabled && - pos_sp_triplet.current.valid) { + pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ @@ -1154,10 +1182,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ @@ -1211,39 +1241,42 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed_2d); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); float alt_sp; + if (_nav_capabilities.abort_landing == true) { // if we entered loiter due to an aborted landing, demand // altitude setpoint well above landing waypoint alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { // use altitude given by wapoint alt_sp = _pos_sp_triplet.current.alt; } if (in_takeoff_situation() || - ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff) + && _nav_capabilities.abort_landing == true)) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); } tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { @@ -1257,13 +1290,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), + curr_wp(1)); /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { wp_distance_save = 0.0f; } @@ -1275,7 +1310,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<2> curr_wp_shifted; double lat; double lon; - create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat, + pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; @@ -1289,9 +1325,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; + } else { target_bearing = _yaw; } + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } @@ -1327,53 +1365,60 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ float terrain_alt; + if (_parameters.land_use_terrain_estimate) { if (_global_pos.terrain_alt_valid) { // all good, have valid terrain altitude terrain_alt = _global_pos.terrain_alt; _t_alt_prev_valid = terrain_alt; _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { // we have started landing phase but don't have valid terrain // wait for some time, maybe we will soon get a valid estimate // until then just use the altitude of the landing waypoint if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { terrain_alt = _pos_sp_triplet.current.alt; + } else { // still no valid terrain, abort landing terrain_alt = _pos_sp_triplet.current.alt; _nav_capabilities.abort_landing = true; } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) - || land_noreturn_vertical) { + || land_noreturn_vertical) { // use previous terrain estimate for some time and hope to recover // if we are already flaring (land_noreturn_vertical) then just // go with the old estimate terrain_alt = _t_alt_prev_valid; + } else { // terrain alt was not valid for long time, abort landing terrain_alt = _t_alt_prev_valid; _nav_capabilities.abort_landing = true; } + } else { // no terrain estimation, just use landing waypoint altitude terrain_alt = _pos_sp_triplet.current.alt; } - + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? - _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a * horizontal limit (with some tolerance) * The horizontal limit is only applied when we are in front of the wp */ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance_save < landingslope.flare_length() + 5.0f)) || - land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + (wp_distance_save < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ @@ -1386,32 +1431,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.yaw_body = target_bearing; _att_sp.fw_control_yaw = true; - if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if (!land_motor_lim) { land_motor_lim = true; mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } - } + } - float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) - { + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), eas2tas, - math::radians(_parameters.land_flare_pitch_min_deg), - math::radians(_parameters.land_flare_pitch_max_deg), - 0.0f, throttle_max, throttle_land, - false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), - _global_pos.alt, ground_speed, - land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + calculate_target_airspeed(airspeed_land), eas2tas, + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), + 0.0f, throttle_max, throttle_land, + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians( + _parameters.pitch_limit_min), + _global_pos.alt, ground_speed, + land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { // just started with the flaring phase @@ -1419,52 +1466,58 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi height_flare = _global_pos.alt - terrain_alt; mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { if (_global_pos.vel_d > 0.1f) { _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * - math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + } else { _att_sp.pitch_body = _att_sp.pitch_body; } } flare_curve_alt_rel_last = flare_curve_alt_rel; + } else { - /* intersect glide slope: - * minimize speed to approach speed - * if current position is higher than the slope follow the glide slope (sink to the - * glide slope) - * also if the system captures the slope it should stay - * on the slope (bool land_onslope) - * if current position is below the slope continue at previous wp altitude - * until the intersection with slope - * */ + /* intersect glide slope: + * minimize speed to approach speed + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope + * */ float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; + if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } + } else { /* continue horizontally */ altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : - _global_pos.alt - terrain_alt; + _global_pos.alt - terrain_alt; } tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, - calculate_target_airspeed(airspeed_approach), eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -1503,23 +1556,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed( - _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, - _runway_takeoff.climbout(), - math::radians(_runway_takeoff.getMinPitch( - _pos_sp_triplet.current.pitch_min, - 10.0f, - _parameters.pitch_limit_min)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); @@ -1537,10 +1590,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* Perform launch detection */ if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { + + if (hrt_absolute_time() - last_sent > 4e6) { mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1550,6 +1604,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update our copy of the launch detection state */ launch_detection_state = launchDetector.getLaunchDetected(); + } else { /* no takeoff detection --> fly */ launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; @@ -1566,8 +1621,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use * full throttle, otherwise we use the preTakeOff Throttle */ float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ @@ -1579,38 +1634,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, - _parameters.throttle_cruise, - true, - math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + math::radians(15.0f)); } else { tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } + } else { /* Tell the attitude controller to stop integrating while we are waiting * for the launch */ @@ -1621,7 +1677,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + math::radians(10.0f)); } } @@ -1643,7 +1699,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else if (_control_mode.flag_control_velocity_enabled && - _control_mode.flag_control_altitude_enabled) { + _control_mode.flag_control_altitude_enabled) { /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, heading is set to a distant waypoint */ @@ -1654,27 +1710,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_POSITION; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground * and set limit to pitch angle to prevent stearing into ground */ float pitch_limit_min; @@ -1683,23 +1742,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - throttle_max, - _parameters.throttle_cruise, - climbout_requested, - ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); /* heading control */ @@ -1731,7 +1791,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* we have a valid heading hold position, are we too close? */ if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } @@ -1752,9 +1812,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (in_takeoff_situation()) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + math::radians(15.0f)); } } + } else { _hdg_hold_enabled = false; _yaw_lock_engaged = false; @@ -1767,27 +1828,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground * and set limit to pitch angle to prevent stearing into ground */ float pitch_limit_min; @@ -1795,23 +1859,25 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - throttle_max, - _parameters.throttle_cruise, - climbout_requested, - ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); + } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1843,34 +1909,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && - !_runway_takeoff.runwayTakeoffEnabled()) { - /* making sure again that the correct thrust is used, - * without depending on library calls for safety reasons */ + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_runway_takeoff.runwayTakeoffEnabled()) { + /* making sure again that the correct thrust is used, + * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - _runway_takeoff.runwayTakeoffEnabled()) { + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _runway_takeoff.runwayTakeoffEnabled()) { _att_sp.thrust = _runway_takeoff.getThrottle( - math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max)); + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && - (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) - { + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) { // when we are landed in these modes we want the motor to spin _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + _tecs.get_throttle_demand(), throttle_max); } @@ -1879,17 +1945,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled())) || - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && - land_noreturn_vertical)) - )) { + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } if (_control_mode.flag_control_position_enabled) { last_manual = false; + } else { last_manual = true; } @@ -2017,7 +2084,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) + || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0)) { /* set new turn distance */ @@ -2028,6 +2096,7 @@ FixedwingPositionControl::task_main() } } + perf_end(_loop_perf); } @@ -2074,10 +2143,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + flightPathAngle = -asinf(ground_speed(2) / ground_speed_length); } + fwPosctrl::LimitOverride limitOverride; + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Force the slow downwards spiral */ limitOverride.enablePitchMinOverride(-1.0f); @@ -2085,6 +2157,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { limitOverride.disablePitchMinOverride(); } @@ -2092,12 +2165,15 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (pitch_max_special) { /* Use the maximum pitch from the argument */ limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, - limitOverride); + limitOverride); + } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Force the slow downwards spiral */ @@ -2105,8 +2181,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; } -/* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND + || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, @@ -2123,18 +2200,21 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.timestamp = s.timestamp; switch (s.mode) { - case TECS::ECL_TECS_MODE_NORMAL: - t.mode = tecs_status_s::TECS_MODE_NORMAL; - break; - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; - break; + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = tecs_status_s::TECS_MODE_NORMAL; + break; + + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; + break; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; } t.altitudeSp = s.altitude_sp; @@ -2153,12 +2233,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.totalEnergyRateError = s.total_energy_rate_error; t.energyDistributionError = s.energy_distribution_error; t.energyDistributionRateError = s.energy_distribution_rate_error; - + t.throttle_integ = s.throttle_integ; t.pitch_integ = s.pitch_integ; if (_tecs_status_pub != nullptr) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); } @@ -2172,11 +2253,11 @@ FixedwingPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("fw_pos_control_l1", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1300, - (px4_main_t)&FixedwingPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1300, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -2211,13 +2292,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); return 0; } if (!strcmp(argv[1], "stop")) { - if (l1_control::g_control == nullptr){ + if (l1_control::g_control == nullptr) { warnx("not running"); return 1; } From 7fcf2d9733e80671adbc39b84562bb4b9b62cd7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Dec 2015 09:53:32 +0100 Subject: [PATCH 597/624] Add FW attitude controller to code check regime --- Tools/check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index c3dd2a3f32..cd42b65c80 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -14,6 +14,7 @@ for fn in $(find src/examples \ src/lib/rc \ src/lib/version \ src/modules/attitude_estimator_q \ + src/modules/fw_att_control \ src/modules/gpio_led \ src/modules/land_detector \ src/modules/muorb \ From c0070a19a85a2534548e470dcedc26613e1eccd3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 10 Dec 2015 10:27:46 -0500 Subject: [PATCH 598/624] VFR_HUD should be indicated airspeed --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1a5c6664b2..0a5ff711d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -920,7 +920,7 @@ protected: if (updated) { mavlink_vfr_hud_t msg; - msg.airspeed = airspeed.true_airspeed_m_s; + msg.airspeed = airspeed.indicated_airspeed_m_s; msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; From 0e48b58d7d778db6f6efb88c7f7c268737c5ae70 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 11 Dec 2015 06:33:23 -1000 Subject: [PATCH 599/624] Support Mavlink on USART2 (ESP8266) Needed bigger buffers --- nuttx-configs/px4fmu-v4/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index a5b79ebf48..5e9c276494 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -551,8 +551,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=128 -CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1100 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 From 988ff591030ba16484c256b0e05bb94d5f116136 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:16:25 +0000 Subject: [PATCH 600/624] Fix simulator PWM range --- src/modules/simulator/simulator_mavlink.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 12402b4632..507f220a43 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -76,21 +76,21 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8] = {}; - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; // for now we only support quadrotors unsigned n = 4; if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) { for (unsigned i = 0; i < 8; i++) { - if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { + if (_actuators.output[i] > PWM_DEFAULT_MIN / 2) { if (i < n) { // scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + out[i] = (_actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } else { // scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } } else { From c1b6fcbb772559f90fc3e01e0244edde9f3b4e2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:36:42 +0000 Subject: [PATCH 601/624] IO config: Revert stack management changes as we saw stack smashing --- nuttx-configs/px4io-v2/nsh/defconfig | 1162 +++++++++++--------------- 1 file changed, 492 insertions(+), 670 deletions(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 56892e3f93..a9b8477948 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,705 +1,198 @@ +############################################################################ +# configs/px4io/nsh/defconfig # -# Automatically generated file; DO NOT EDIT. -# Nuttx/ Configuration +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt # -CONFIG_NUTTX_NEWCONFIG=y - +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: # -# Build Setup +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. # -# CONFIG_EXPERIMENTAL is not set -CONFIG_HOST_LINUX=y -# CONFIG_HOST_OSX is not set -# CONFIG_HOST_WINDOWS is not set -# CONFIG_HOST_OTHER is not set - +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. # -# Build Configuration +############################################################################ # -CONFIG_APPS_DIR="../apps" -# CONFIG_BUILD_2PASS is not set - +# architecture selection # -# Binary Output Formats +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization # -# CONFIG_RRLOAD_BINARY is not set -# CONFIG_INTELHEX_BINARY is not set -# CONFIG_MOTOROLA_SREC is not set -CONFIG_RAW_BINARY=y - -# -# Customize Header Files -# -# CONFIG_ARCH_STDBOOL_H is not set -CONFIG_ARCH_MATH_H=y -# CONFIG_ARCH_FLOAT_H is not set -# CONFIG_ARCH_STDARG_H is not set - -# -# Debug Options -# -# CONFIG_DEBUG is not set -CONFIG_DEBUG_SYMBOLS=y -# -# System Type -# -# CONFIG_ARCH_8051 is not set -CONFIG_ARCH_ARM=y -# CONFIG_ARCH_AVR is not set -# CONFIG_ARCH_HC is not set -# CONFIG_ARCH_MIPS is not set -# CONFIG_ARCH_RGMP is not set -# CONFIG_ARCH_SH is not set -# CONFIG_ARCH_SIM is not set -# CONFIG_ARCH_X86 is not set -# CONFIG_ARCH_Z16 is not set -# CONFIG_ARCH_Z80 is not set CONFIG_ARCH="arm" - -# -# ARM Options -# -# CONFIG_ARCH_CHIP_C5471 is not set -# CONFIG_ARCH_CHIP_CALYPSO is not set -# CONFIG_ARCH_CHIP_DM320 is not set -# CONFIG_ARCH_CHIP_IMX is not set -# CONFIG_ARCH_CHIP_KINETIS is not set -# CONFIG_ARCH_CHIP_KL is not set -# CONFIG_ARCH_CHIP_LM is not set -# CONFIG_ARCH_CHIP_LPC17XX is not set -# CONFIG_ARCH_CHIP_LPC214X is not set -# CONFIG_ARCH_CHIP_LPC2378 is not set -# CONFIG_ARCH_CHIP_LPC31XX is not set -# CONFIG_ARCH_CHIP_LPC43XX is not set -# CONFIG_ARCH_CHIP_NUC1XX is not set -# CONFIG_ARCH_CHIP_SAM34 is not set -CONFIG_ARCH_CHIP_STM32=y -# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_FAMILY="armv7-m" CONFIG_ARCH_CHIP="stm32" -# CONFIG_ARMV7M_USEBASEPRI is not set -CONFIG_ARCH_HAVE_CMNVECTOR=y -CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARCH_HAVE_FPU is not set -CONFIG_ARCH_HAVE_MPU=y -# CONFIG_ARMV7M_MPU is not set - -# -# ARMV7M Configuration Options -# -# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set -# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set -# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set -CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -# CONFIG_ARMV7M_STACKCHECK is not set -CONFIG_SERIAL_TERMIOS=y - -# -# STM32 Configuration Options -# -# CONFIG_ARCH_CHIP_STM32L151C6 is not set -# CONFIG_ARCH_CHIP_STM32L151C8 is not set -# CONFIG_ARCH_CHIP_STM32L151CB is not set -# CONFIG_ARCH_CHIP_STM32L151R6 is not set -# CONFIG_ARCH_CHIP_STM32L151R8 is not set -# CONFIG_ARCH_CHIP_STM32L151RB is not set -# CONFIG_ARCH_CHIP_STM32L151V6 is not set -# CONFIG_ARCH_CHIP_STM32L151V8 is not set -# CONFIG_ARCH_CHIP_STM32L151VB is not set -# CONFIG_ARCH_CHIP_STM32L152C6 is not set -# CONFIG_ARCH_CHIP_STM32L152C8 is not set -# CONFIG_ARCH_CHIP_STM32L152CB is not set -# CONFIG_ARCH_CHIP_STM32L152R6 is not set -# CONFIG_ARCH_CHIP_STM32L152R8 is not set -# CONFIG_ARCH_CHIP_STM32L152RB is not set -# CONFIG_ARCH_CHIP_STM32L152V6 is not set -# CONFIG_ARCH_CHIP_STM32L152V8 is not set -# CONFIG_ARCH_CHIP_STM32L152VB is not set CONFIG_ARCH_CHIP_STM32F100C8=y -# CONFIG_ARCH_CHIP_STM32F100CB is not set -# CONFIG_ARCH_CHIP_STM32F100R8 is not set -# CONFIG_ARCH_CHIP_STM32F100RB is not set -# CONFIG_ARCH_CHIP_STM32F100RC is not set -# CONFIG_ARCH_CHIP_STM32F100RD is not set -# CONFIG_ARCH_CHIP_STM32F100RE is not set -# CONFIG_ARCH_CHIP_STM32F100V8 is not set -# CONFIG_ARCH_CHIP_STM32F100VB is not set -# CONFIG_ARCH_CHIP_STM32F100VC is not set -# CONFIG_ARCH_CHIP_STM32F100VD is not set -# CONFIG_ARCH_CHIP_STM32F100VE is not set -# CONFIG_ARCH_CHIP_STM32F102CB is not set -# CONFIG_ARCH_CHIP_STM32F103C4 is not set -# CONFIG_ARCH_CHIP_STM32F103C8 is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set -# CONFIG_ARCH_CHIP_STM32F107VC is not set -# CONFIG_ARCH_CHIP_STM32F207IG is not set -# CONFIG_ARCH_CHIP_STM32F302CB is not set -# CONFIG_ARCH_CHIP_STM32F302CC is not set -# CONFIG_ARCH_CHIP_STM32F302RB is not set -# CONFIG_ARCH_CHIP_STM32F302RC is not set -# CONFIG_ARCH_CHIP_STM32F302VB is not set -# CONFIG_ARCH_CHIP_STM32F302VC is not set -# CONFIG_ARCH_CHIP_STM32F303CB is not set -# CONFIG_ARCH_CHIP_STM32F303CC is not set -# CONFIG_ARCH_CHIP_STM32F303RB is not set -# CONFIG_ARCH_CHIP_STM32F303RC is not set -# CONFIG_ARCH_CHIP_STM32F303VB is not set -# CONFIG_ARCH_CHIP_STM32F303VC is not set -# CONFIG_ARCH_CHIP_STM32F405RG is not set -# CONFIG_ARCH_CHIP_STM32F405VG is not set -# CONFIG_ARCH_CHIP_STM32F405ZG is not set -# CONFIG_ARCH_CHIP_STM32F407VE is not set -# CONFIG_ARCH_CHIP_STM32F407VG is not set -# CONFIG_ARCH_CHIP_STM32F407ZE is not set -# CONFIG_ARCH_CHIP_STM32F407ZG is not set -# CONFIG_ARCH_CHIP_STM32F407IE is not set -# CONFIG_ARCH_CHIP_STM32F407IG is not set -# CONFIG_ARCH_CHIP_STM32F427V is not set -# CONFIG_ARCH_CHIP_STM32F427Z is not set -# CONFIG_ARCH_CHIP_STM32F427I is not set -# CONFIG_STM32_STM32L15XX is not set -# CONFIG_STM32_ENERGYLITE is not set -CONFIG_STM32_STM32F10XX=y -CONFIG_STM32_VALUELINE=y -# CONFIG_STM32_CONNECTIVITYLINE is not set -# CONFIG_STM32_PERFORMANCELINE is not set -# CONFIG_STM32_HIGHDENSITY is not set -# CONFIG_STM32_MEDIUMDENSITY is not set -# CONFIG_STM32_LOWDENSITY is not set -# CONFIG_STM32_STM32F20XX is not set -# CONFIG_STM32_STM32F30XX is not set -# CONFIG_STM32_STM32F40XX is not set - -# -# STM32 Peripheral Support -# -CONFIG_STM32_ADC1=y -# CONFIG_STM32_BKP is not set -# CONFIG_STM32_CEC is not set -# CONFIG_STM32_CRC is not set -CONFIG_STM32_DMA1=y -# CONFIG_STM32_DAC1 is not set -# CONFIG_STM32_DAC2 is not set -# CONFIG_STM32_I2C1 is not set -# CONFIG_STM32_I2C2 is not set -# CONFIG_STM32_PWR is not set -# CONFIG_STM32_SPI1 is not set -# CONFIG_STM32_TIM1 is not set -# CONFIG_STM32_TIM2 is not set -# CONFIG_STM32_TIM3 is not set -# CONFIG_STM32_TIM4 is not set -# CONFIG_STM32_TIM5 is not set -# CONFIG_STM32_TIM6 is not set -# CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM12 is not set -# CONFIG_STM32_TIM13 is not set -# CONFIG_STM32_TIM14 is not set -# CONFIG_STM32_TIM15 is not set -# CONFIG_STM32_TIM16 is not set -# CONFIG_STM32_TIM17 is not set -CONFIG_STM32_USART1=y -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -# CONFIG_STM32_UART4 is not set -# CONFIG_STM32_UART5 is not set -# CONFIG_STM32_IWDG is not set -# CONFIG_STM32_WWDG is not set -CONFIG_STM32_ADC=y - -# -# Alternate Pin Mapping -# -# CONFIG_STM32_USART1_REMAP is not set -# CONFIG_STM32_USART2_REMAP is not set -CONFIG_STM32_USART3_NO_REMAP=y -# CONFIG_STM32_USART3_FULL_REMAP is not set -# CONFIG_STM32_USART3_PARTIAL_REMAP is not set -# CONFIG_STM32_JTAG_DISABLE is not set -# CONFIG_STM32_JTAG_FULL_ENABLE is not set -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set -CONFIG_STM32_JTAG_SW_ENABLE=y -# CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG is not set -# CONFIG_STM32_FORCEPOWER is not set -# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_DMACAPABLE is not set -CONFIG_STM32_USART=y - -# -# U[S]ART Configuration -# -# CONFIG_USART1_RS485 is not set -# CONFIG_USART2_RS485 is not set -# CONFIG_USART2_RXDMA is not set -# CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y -# CONFIG_SERIAL_DISABLE_REORDERING is not set -# CONFIG_STM32_USART_SINGLEWIRE is not set - -# -# USB Host Configuration -# - -# -# USB Device Configuration -# - -# -# External Memory Configuration -# - -# -# Architecture Options -# -# CONFIG_ARCH_NOINTC is not set -# CONFIG_ARCH_VECNOTIRQ is not set -CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y -# CONFIG_CUSTOM_STACK is not set -# CONFIG_ADDRENV is not set -CONFIG_ARCH_HAVE_VFORK=y -CONFIG_ARCH_STACKDUMP=y -# CONFIG_ENDIAN_BIG is not set -# CONFIG_ARCH_HAVE_RAMFUNCS is not set -CONFIG_ARCH_HAVE_RAMVECTORS=y -# CONFIG_ARCH_RAMVECTORS is not set - -# -# Board Settings -# -CONFIG_BOARD_LOOPSPERMSEC=2000 -# CONFIG_ARCH_CALIBRATION is not set -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=0x2000 -CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=500 - -# -# Boot options -# -# CONFIG_BOOT_RUNFROMEXTSRAM is not set -CONFIG_BOOT_RUNFROMFLASH=y -# CONFIG_BOOT_RUNFROMISRAM is not set -# CONFIG_BOOT_RUNFROMSDRAM is not set -# CONFIG_BOOT_COPYTORAM is not set - -# -# Board Selection -# CONFIG_ARCH_BOARD="px4io-v2" CONFIG_ARCH_BOARD_PX4IO_V2=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARMV7M_STACKCHECK=n +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=n +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # -# Common Board Options +# Individual subsystems can be enabled: # +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=n +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + # -# Board-Specific Options +# STM32F100 specific serial device driver settings # - +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits # -# RTOS Features -# -# CONFIG_BOARD_INITIALIZE is not set -CONFIG_MSEC_PER_TICK=1 -CONFIG_RR_INTERVAL=0 -# CONFIG_SCHED_INSTRUMENTATION is not set -CONFIG_TASK_NAME_SIZE=8 -# CONFIG_SCHED_HAVE_PARENT is not set -# CONFIG_JULIAN_TIME is not set -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_DEV_CONSOLE=y -# CONFIG_MUTEX_TYPES is not set -# CONFIG_PRIORITY_INHERITANCE is not set -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -# CONFIG_SCHED_WAITPID is not set -# CONFIG_SCHED_STARTHOOK is not set -# CONFIG_SCHED_ATEXIT is not set -# CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="user_start" -CONFIG_DISABLE_OS_API=y -# CONFIG_DISABLE_CLOCK is not set -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_ENVIRON=y - -# -# Sizes of configurable things (0 disables) -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=0 - -# -# Stack and heap information -# -CONFIG_IDLETHREAD_STACKSIZE=290 -CONFIG_USERMAIN_STACKSIZE=800 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 - -# -# Device Drivers -# -CONFIG_DISABLE_POLL=y -CONFIG_DEV_NULL=y -# CONFIG_DEV_ZERO is not set -# CONFIG_LOOP is not set -# CONFIG_RAMDISK is not set -# CONFIG_CAN is not set -# CONFIG_PWM is not set -# CONFIG_I2C is not set -CONFIG_ARCH_HAVE_I2CRESET=y -# CONFIG_SPI is not set -# CONFIG_RTC is not set -# CONFIG_WATCHDOG is not set -# CONFIG_ANALOG is not set -# CONFIG_AUDIO_DEVICES is not set -# CONFIG_BCH is not set -# CONFIG_INPUT is not set -# CONFIG_LCD is not set -# CONFIG_MMCSD is not set -# CONFIG_MTD is not set -# CONFIG_PIPES is not set -# CONFIG_PM is not set -# CONFIG_POWER is not set -# CONFIG_SENSORS is not set -CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set -# CONFIG_16550_UART is not set -CONFIG_ARCH_HAVE_USART1=y -CONFIG_ARCH_HAVE_USART2=y -CONFIG_ARCH_HAVE_USART3=y -CONFIG_MCU_SERIAL=y +CONFIG_SERIAL_TERMIOS=y CONFIG_STANDARD_SERIAL=y + CONFIG_USART1_SERIAL_CONSOLE=y -# CONFIG_USART2_SERIAL_CONSOLE is not set -# CONFIG_USART3_SERIAL_CONSOLE is not set -# CONFIG_NO_SERIAL_CONSOLE is not set +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n -# -# USART1 Configuration -# -CONFIG_USART1_RXBUFSIZE=64 CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART1_BAUD=115200 -CONFIG_USART1_BITS=8 -CONFIG_USART1_PARITY=0 -CONFIG_USART1_2STOP=0 -# CONFIG_USART1_IFLOWCONTROL is not set -# CONFIG_USART1_OFLOWCONTROL is not set - -# -# USART2 Configuration -# -CONFIG_USART2_RXBUFSIZE=64 CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART2_BAUD=115200 -CONFIG_USART2_BITS=8 -CONFIG_USART2_PARITY=0 -CONFIG_USART2_2STOP=0 -# CONFIG_USART2_IFLOWCONTROL is not set -# CONFIG_USART2_OFLOWCONTROL is not set - -# -# USART3 Configuration -# -CONFIG_USART3_RXBUFSIZE=64 CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 -# CONFIG_USART3_IFLOWCONTROL is not set -# CONFIG_USART3_OFLOWCONTROL is not set -# CONFIG_SERIAL_IFLOWCONTROL is not set -# CONFIG_SERIAL_OFLOWCONTROL is not set -# CONFIG_USBDEV is not set -# CONFIG_USBHOST is not set -# CONFIG_WIRELESS is not set -# -# System Logging Device Options -# - -# -# System Logging -# -# CONFIG_RAMLOG is not set - -# -# Networking Support -# -# CONFIG_NET is not set - -# -# File Systems -# - -# -# File system configuration -# -CONFIG_DISABLE_MOUNTPOINT=y -# CONFIG_FS_RAMMAP is not set - -# -# System Logging -# -# CONFIG_SYSLOG_ENABLE is not set -# CONFIG_SYSLOG is not set - -# -# Graphics Support -# -# CONFIG_NX is not set - -# -# Memory Management -# -# CONFIG_MM_MULTIHEAP is not set -CONFIG_MM_SMALL=y -CONFIG_MM_REGIONS=1 -# CONFIG_GRAN is not set - -# -# Audio Support -# -# CONFIG_AUDIO is not set - -# -# Binary Formats -# -# CONFIG_BINFMT_DISABLE is not set -# CONFIG_NXFLAT is not set -# CONFIG_ELF is not set -CONFIG_BUILTIN=y -# CONFIG_PIC is not set -# CONFIG_SYMTAB_ORDEREDBYNAME is not set - -# -# Library Routines -# - -# -# Standard C Library Options -# -CONFIG_STDIO_BUFFER_SIZE=32 -# CONFIG_STDIO_LINEBUFFER is not set -CONFIG_NUNGET_CHARS=2 -# CONFIG_NOPRINTF_FIELDWIDTH is not set -# CONFIG_LIBC_FLOATINGPOINT is not set -CONFIG_LIB_RAND_ORDER=1 -# CONFIG_EOL_IS_CR is not set -# CONFIG_EOL_IS_LF is not set -# CONFIG_EOL_IS_BOTH_CRLF is not set -CONFIG_EOL_IS_EITHER_CRLF=y -# CONFIG_LIBC_EXECFUNCS is not set -CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 -CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 -# CONFIG_LIBC_STRERROR is not set -# CONFIG_LIBC_PERROR_STDOUT is not set -CONFIG_ARCH_LOWPUTC=y -CONFIG_LIB_SENDFILE_BUFSIZE=512 -# CONFIG_ARCH_ROMGETC is not set -# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set - -# -# Non-standard Library Support -# -# CONFIG_LIB_KBDCODEC is not set -# CONFIG_LIB_SLCDCODEC is not set - -# -# Basic CXX Support -# -# CONFIG_C99_BOOL8 is not set -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -# CONFIG_CXX_NEWLONG is not set - -# -# uClibc++ Standard C++ Library -# -# CONFIG_UCLIBCXX is not set - -# -# Application Configuration -# - -# -# Built-In Applications -# - -# -# Examples -# -# CONFIG_EXAMPLES_BUTTONS is not set -# CONFIG_EXAMPLES_CAN is not set -# CONFIG_EXAMPLES_COMPOSITE is not set -# CONFIG_EXAMPLES_CXXTEST is not set -# CONFIG_EXAMPLES_DHCPD is not set -# CONFIG_EXAMPLES_ELF is not set -# CONFIG_EXAMPLES_FTPC is not set -# CONFIG_EXAMPLES_FTPD is not set -# CONFIG_EXAMPLES_HELLO is not set -# CONFIG_EXAMPLES_HELLOXX is not set -# CONFIG_EXAMPLES_JSON is not set -# CONFIG_EXAMPLES_HIDKBD is not set -# CONFIG_EXAMPLES_KEYPADTEST is not set -# CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set -# CONFIG_EXAMPLES_MM is not set -# CONFIG_EXAMPLES_MODBUS is not set -# CONFIG_EXAMPLES_MOUNT is not set -# CONFIG_EXAMPLES_NRF24L01TERM is not set -# CONFIG_EXAMPLES_NSH is not set -# CONFIG_EXAMPLES_NULL is not set -# CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set -# CONFIG_EXAMPLES_NXFFS is not set -# CONFIG_EXAMPLES_NXFLAT is not set -# CONFIG_EXAMPLES_NXHELLO is not set -# CONFIG_EXAMPLES_NXIMAGE is not set -# CONFIG_EXAMPLES_NXLINES is not set -# CONFIG_EXAMPLES_NXTEXT is not set -# CONFIG_EXAMPLES_OSTEST is not set -# CONFIG_EXAMPLES_PASHELLO is not set -# CONFIG_EXAMPLES_PIPE is not set -# CONFIG_EXAMPLES_POLL is not set -# CONFIG_EXAMPLES_POSIXSPAWN is not set -# CONFIG_EXAMPLES_QENCODER is not set -# CONFIG_EXAMPLES_RGMP is not set -# CONFIG_EXAMPLES_ROMFS is not set -# CONFIG_EXAMPLES_SENDMAIL is not set -# CONFIG_EXAMPLES_SERLOOP is not set -# CONFIG_EXAMPLES_SLCD is not set -# CONFIG_EXAMPLES_SMART is not set -# CONFIG_EXAMPLES_TCPECHO is not set -# CONFIG_EXAMPLES_TELNETD is not set -# CONFIG_EXAMPLES_THTTPD is not set -# CONFIG_EXAMPLES_TIFF is not set -# CONFIG_EXAMPLES_TOUCHSCREEN is not set -# CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set -# CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set -# CONFIG_EXAMPLES_USBTERM is not set -# CONFIG_EXAMPLES_WATCHDOG is not set - -# -# Graphics Support -# -# CONFIG_TIFF is not set - -# -# Interpreters -# -# CONFIG_INTERPRETERS_FICL is not set -# CONFIG_INTERPRETERS_PCODE is not set - -# -# Network Utilities -# - -# -# Networking Utilities -# -# CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set -# CONFIG_NETUTILS_DHCPD is not set -# CONFIG_NETUTILS_FTPC is not set -# CONFIG_NETUTILS_FTPD is not set -# CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set -# CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set -# CONFIG_NETUTILS_TFTPC is not set -# CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set -# CONFIG_NETUTILS_WEBCLIENT is not set - -# -# FreeModBus -# -# CONFIG_MODBUS is not set - -# -# NSH Library -# -# CONFIG_NSH_LIBRARY is not set - -# -# NxWidgets/NxWM -# - -# -# System NSH Add-Ons -# - -# -# Custom Free Memory Command -# -# CONFIG_SYSTEM_FREE is not set - -# -# I2C tool -# - -# -# FLASH Program Installation -# -# CONFIG_SYSTEM_INSTALL is not set - -# -# FLASH Erase-all Command -# - -# -# readline() -# -# CONFIG_SYSTEM_READLINE is not set - -# -# Power Off -# -# CONFIG_SYSTEM_POWEROFF is not set - -# -# RAMTRON -# -# CONFIG_SYSTEM_RAMTRON is not set - -# -# SD Card -# -# CONFIG_SYSTEM_SDCARD is not set - -# -# Sysinfo -# -# CONFIG_SYSTEM_SYSINFO is not set - -# -# USB Monitor -# +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y # # PX4IO specific driver settings @@ -723,3 +216,332 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_HRT_TIMER=y CONFIG_HRT_PPM=y +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= From 3bb0c4f6b954f1abc7ec236fa70c2a994c725999 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 16 Dec 2015 01:25:32 -0500 Subject: [PATCH 602/624] add geofence param metadata --- src/modules/navigator/geofence_params.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 070fde6174..14902fdd6d 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -95,6 +95,8 @@ PARAM_DEFINE_INT32(GF_COUNT, -1); * * Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. * + * @unit meters + * @min -1 * @group Geofence */ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); @@ -104,6 +106,8 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); * * Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. * + * @unit meters + * @min -1 * @group Geofence */ PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1); From 364da5343bbd8f15c38ca1ccb0960bcbdc81d52e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Dec 2015 16:37:56 +0000 Subject: [PATCH 603/624] Add motor off PWM value --- src/drivers/drv_pwm_output.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index fd20814b7d..06d1f10d47 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -78,6 +78,11 @@ __BEGIN_DECLS */ #define PWM_LOWEST_MIN 90 +/** + * Default value for a shutdown motor + */ +#define PWM_MOTOR_OFF 800 + /** * Default minimum PWM in us */ From 9714af3a8b66aecabd6b25c21ff057d4a363e1c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Dec 2015 16:38:15 +0000 Subject: [PATCH 604/624] Fix usage of PWM defines in mavlink app --- src/modules/mavlink/mavlink_messages.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0a5ff711d4..4f12a25120 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1801,7 +1801,7 @@ protected: float out[8]; - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; unsigned system_type = _mavlink->get_system_type(); @@ -1839,14 +1839,14 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } else { /* scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } } else { @@ -1859,14 +1859,14 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { if (i != 3) { /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } else { /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } } else { From 9bb819b8013b40db296cea7e9551d8c6a6303a00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Dec 2015 16:38:27 +0000 Subject: [PATCH 605/624] Fix usage of PWM defines in VTOL app --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- src/modules/vtol_att_control/vtol_type.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index b9c2d64fe2..f21ed3fe52 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -104,7 +104,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_batt_status, 0, sizeof(_batt_status)); memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); - _params.idle_pwm_mc = PWM_LOWEST_MIN; + _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; _params.vtol_fw_permanent_stab = 0; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 0120cadc84..2f361eb8f4 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -117,13 +117,12 @@ void VtolType::set_idle_fw() if (fd < 0) {PX4_WARN("can't open %s", dev);} - unsigned pwm_value = PWM_LOWEST_MIN; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; + pwm_values.values[i] = PWM_MOTOR_OFF; pwm_values.channel_count++; } From a3b16057823065a9c25aae898f5e084c3177e66f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Dec 2015 16:38:41 +0000 Subject: [PATCH 606/624] Fix usage of PWM defines in test command --- src/systemcmds/tests/test_mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 98b65f10d1..4da4b77c1e 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -193,7 +193,7 @@ int test_mixer(int argc, char *argv[]) /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; - r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_disarmed[i] = PWM_MOTOR_OFF; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } From c6584635674d7f0ee5183949df0d9652f17f5cfd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Dec 2015 17:07:45 +0000 Subject: [PATCH 607/624] EKF: Set better value for accel process noise --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 45e2f24939..208449790b 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); * @max 1.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.125f); /** * Gyro bias estimate process noise From e9da8167e0825aaf0729fa5a892fb37b5442fd2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:10:31 +0000 Subject: [PATCH 608/624] LPE: Fix config name --- posix-configs/SITL/init/{rcS_lpe_gazebo => rcS_lpe_gazebo_iris} | 0 posix-configs/SITL/init/{rcS_lpe_jmavsim => rcS_lpe_jmavsim_iris} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename posix-configs/SITL/init/{rcS_lpe_gazebo => rcS_lpe_gazebo_iris} (100%) rename posix-configs/SITL/init/{rcS_lpe_jmavsim => rcS_lpe_jmavsim_iris} (100%) diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo_iris similarity index 100% rename from posix-configs/SITL/init/rcS_lpe_gazebo rename to posix-configs/SITL/init/rcS_lpe_gazebo_iris diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris similarity index 100% rename from posix-configs/SITL/init/rcS_lpe_jmavsim rename to posix-configs/SITL/init/rcS_lpe_jmavsim_iris From bdce1a902f5168b6f1f2abe359a103448cc0f4c9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 17 Dec 2015 18:25:23 +0100 Subject: [PATCH 609/624] update missin param in inav --- .../position_estimator_inav/position_estimator_inav_params.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp index f303abaf6d..9178a9d659 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp @@ -384,6 +384,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_gps_v, &(p->w_z_gps_v)); param_get(h->w_z_vision_p, &(p->w_z_vision_p)); param_get(h->w_z_lidar, &(p->w_z_lidar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); From 6f09c8c3b79b6c09c73059a5614463a16635600f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 22:07:27 +0000 Subject: [PATCH 610/624] Start Wifi MAVLink on Pixracer --- 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 1c04627982..a2a726bd7c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -554,6 +554,8 @@ then if ver hwcmp PX4FMU_V4 then set MAVLINK_F "-r 1200 -d /dev/ttyS2" + # Start MAVLink on Wifi (ESP8266 port) + mavlink start -r 800000 -b 921600 -d /dev/ttyS0 fi fi fi From 699c5f2f6d5c3b47a6a386b9f089924f032d7c6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Dec 2015 10:29:26 +0000 Subject: [PATCH 611/624] Enable 2M flash in Pixracer --- nuttx-configs/px4fmu-v4/scripts/ld.script | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v4/scripts/ld.script b/nuttx-configs/px4fmu-v4/scripts/ld.script index 3649227a43..f0f9e0f244 100644 --- a/nuttx-configs/px4fmu-v4/scripts/ld.script +++ b/nuttx-configs/px4fmu-v4/scripts/ld.script @@ -50,8 +50,7 @@ MEMORY { - /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ - flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } From c1e40bdd6093630ccd7ae714f4f52b08fe760a09 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 4 Dec 2015 15:42:17 +0100 Subject: [PATCH 612/624] limit maximal desired horizontal acceleration --- .../mc_pos_control/mc_pos_control_main.cpp | 32 +++++++++++++++++-- .../mc_pos_control/mc_pos_control_params.c | 10 ++++++ 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7adc51392e..66274e844b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -188,6 +188,7 @@ private: param_t hold_z_dz; param_t hold_max_xy; param_t hold_max_z; + param_t acc_hor_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -204,6 +205,7 @@ private: float hold_z_dz; float hold_max_xy; float hold_max_z; + float acc_hor_max; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -232,6 +234,7 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; + math::Vector<3> _vel_sp_prev; math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ @@ -384,6 +387,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); _vel_ff.zero(); + _vel_sp_prev.zero(); _R.identity(); @@ -412,6 +416,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); + _params_handles.acc_hor_max = param_find("MPC_ACC_HOR_MAX"); /* fetch initial parameter values */ @@ -510,6 +515,8 @@ MulticopterPositionControl::parameters_update(bool force) _params.hold_max_xy = (v < 0.0f ? 0.0f : v); param_get(_params_handles.hold_max_z, &v); _params.hold_max_z = (v < 0.0f ? 0.0f : v); + param_get(_params_handles.acc_hor_max, &v); + _params.acc_hor_max = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -537,10 +544,12 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } @@ -1179,6 +1188,23 @@ MulticopterPositionControl::task_main() control_auto(dt); } + // limit total horizontal acceleration + math::Vector<2> acc_hor; + acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; + acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt; + + if (acc_hor.length() > _params.acc_hor_max) { + acc_hor.normalize(); + acc_hor *= _params.acc_hor_max; + math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1)); + math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; + _vel_sp(0) = vel_sp_hor(0); + _vel_sp(1) = vel_sp_hor(1); + } + + _vel_sp_prev = _vel_sp; + + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ @@ -1196,6 +1222,7 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } @@ -1627,11 +1654,12 @@ MulticopterPositionControl::task_main() * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 7c63a82afa..65673a8284 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -309,3 +309,13 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); + +/** + * Maximum horizonal acceleration in velocity controlled modes + * + * @unit m/s/s + * @min 2.0 + * @max 10.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 4.0f); From 4f698347615d30dc762d9b883abf9dbc4e5d46e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 10:14:04 +0100 Subject: [PATCH 613/624] MC pos control: Add missing previous velocity SP checks --- .../mc_pos_control/mc_pos_control_main.cpp | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 66274e844b..88e00cb7a1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.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 @@ -261,12 +261,21 @@ private: */ void update_ref(); /** - * Reset position setpoint to current position + * Reset position setpoint to current position. + * + * This reset will only occur if the _reset_pos_sp flag has been set. + * The general logic is to first "activate" the flag in the flight + * regime where a switch to a position control mode should hold the + * very last position. Once switching to a position control mode + * the last position is stored once. */ void reset_pos_sp(); /** - * Reset altitude setpoint to current altitude + * Reset altitude setpoint to current altitude. + * + * This reset will only occur if the _reset_alt_sp flag has been set. + * The general logic follows the reset_pos_sp() architecture. */ void reset_alt_sp(); @@ -650,7 +659,8 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + + //mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -660,7 +670,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + //mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -873,6 +883,8 @@ void MulticopterPositionControl::control_auto(float dt) /* reset position setpoint on AUTO mode activation */ reset_pos_sp(); reset_alt_sp(); + /* set current velocity as last target velocity to smooth out transition */ + _vel_sp_prev = _vel; } //Poll position setpoint @@ -1124,6 +1136,7 @@ MulticopterPositionControl::task_main() /* reset setpoints and integrals on arming */ _reset_pos_sp = true; _reset_alt_sp = true; + _vel_sp_prev.zero(); reset_int_z = true; reset_int_xy = true; reset_yaw_sp = true; @@ -1646,6 +1659,8 @@ MulticopterPositionControl::task_main() /* update previous velocity for velocity controller D part */ _vel_prev = _vel; + /* store last velocity in case a mode switch to auto occurs */ + _vel_sp_prev = _vel; /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, From 7b06c4afb9eeb19e06f1c5d9b9cfbc0cb35dcc77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 13:36:36 +0100 Subject: [PATCH 614/624] Acceleration limits: Limit at correct location --- .../mc_pos_control/mc_pos_control_main.cpp | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 88e00cb7a1..1e1af3c4c4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1201,23 +1201,6 @@ MulticopterPositionControl::task_main() control_auto(dt); } - // limit total horizontal acceleration - math::Vector<2> acc_hor; - acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; - acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt; - - if (acc_hor.length() > _params.acc_hor_max) { - acc_hor.normalize(); - acc_hor *= _params.acc_hor_max; - math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1)); - math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; - _vel_sp(0) = vel_sp_hor(0); - _vel_sp(1) = vel_sp_hor(1); - } - - _vel_sp_prev = _vel_sp; - - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ @@ -1291,6 +1274,22 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + // limit total horizontal acceleration + math::Vector<2> acc_hor; + acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; + acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt; + + if (acc_hor.length() > _params.acc_hor_max) { + acc_hor.normalize(); + acc_hor *= _params.acc_hor_max; + math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1)); + math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; + _vel_sp(0) = vel_sp_hor(0); + _vel_sp(1) = vel_sp_hor(1); + } + + _vel_sp_prev = _vel_sp; + _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); From f533f6ce1968f3ecbb7a7b46a203bde5bfda3674 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 14:57:31 +0100 Subject: [PATCH 615/624] Store prev velocity at right location --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 1e1af3c4c4..c9512a33e8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1599,6 +1599,9 @@ MulticopterPositionControl::task_main() _mode_auto = false; reset_int_z = true; reset_int_xy = true; + + /* store last velocity in case a mode switch to position control occurs */ + _vel_sp_prev = _vel; } /* generate attitude setpoint from manual controls */ @@ -1658,8 +1661,6 @@ MulticopterPositionControl::task_main() /* update previous velocity for velocity controller D part */ _vel_prev = _vel; - /* store last velocity in case a mode switch to auto occurs */ - _vel_sp_prev = _vel; /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, From 6ed702094bf0f05d9dfed443128e0ba45efbc43e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Dec 2015 16:46:21 +0100 Subject: [PATCH 616/624] MC pos control: Keep correctly track of velocity and accelerations --- .../mc_pos_control/mc_pos_control_main.cpp | 47 ++++++++++++------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c9512a33e8..a2f38f2828 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -235,6 +235,7 @@ private: math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; math::Vector<3> _vel_sp_prev; + math::Vector<3> _vel_err_d; /**< derivative of current velocity */ math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ @@ -397,6 +398,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_prev.zero(); _vel_ff.zero(); _vel_sp_prev.zero(); + _vel_err_d.zero(); _R.identity(); @@ -1155,19 +1157,39 @@ MulticopterPositionControl::task_main() update_ref(); + /* Update velocity derivative, + * independent of the current flight mode + */ + if (_local_pos.timestamp > 0) { + + if (PX4_ISFINITE(_local_pos.x) && + PX4_ISFINITE(_local_pos.y) && + PX4_ISFINITE(_local_pos.z)) { + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + } + + if (PX4_ISFINITE(_local_pos.vx) && + PX4_ISFINITE(_local_pos.vy) && + PX4_ISFINITE(_local_pos.vz)) { + + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; + } + + _vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); + _vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); + _vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); + } + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - _pos(2) = _local_pos.z; - - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - _vel(2) = _local_pos.vz; - _vel_ff.zero(); /* by default, run position/altitude controller. the control_* functions @@ -1341,15 +1363,8 @@ MulticopterPositionControl::task_main() /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - /* derivative of velocity error, / - * does not includes setpoint acceleration */ - math::Vector<3> vel_err_d; - vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); - vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); - vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); - /* thrust vector in NED frame */ - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; From eef4ccea1a684c8f206e46cd2b57e0cd64da60fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 01:02:14 +0100 Subject: [PATCH 617/624] Adjust jMAVSim Iris gains --- posix-configs/SITL/init/rcS_jmavsim_iris | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 88a3f3978e..ea18e6463c 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -24,9 +24,9 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.4 -param set MPC_XY_VEL_P 0.2 -param set MPC_XY_VEL_D 0.005 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.12 +param set MPC_XY_VEL_D 0.012 param set SENS_BOARD_ROT 0 param set COM_RC_IN_MODE 1 param set NAV_ACC_RAD 2.0 From 144e0793b1fb4d1ce6b23781bc9419428e297fad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 01:10:55 +0100 Subject: [PATCH 618/624] SITL: Further adjust gains to more realistic values --- posix-configs/SITL/init/rcS_jmavsim_iris | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index ea18e6463c..0323bc9b13 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -2,10 +2,10 @@ uorb start simulator start -s param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.15 -param set MC_PITCH_P 7 -param set MC_ROLL_P 7 -param set MC_ROLLRATE_P 0.15 +param set MC_PITCHRATE_P 0.12 +param set MC_PITCH_P 6 +param set MC_ROLL_P 6 +param set MC_ROLLRATE_P 0.12 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 From 948d9ee71e8bbba395b7d610e7e2dfd43691100f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 19:21:38 +0100 Subject: [PATCH 619/624] Report acceleration --- src/modules/mavlink/mavlink_messages.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4f12a25120..ea8d2fae84 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2013,6 +2013,9 @@ protected: msg.vx = pos_sp.vx; msg.vy = pos_sp.vy; msg.vz = pos_sp.vz; + msg.afx = pos_sp.acc_x; + msg.afy = pos_sp.acc_y; + msg.afz = pos_sp.acc_z; _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } From 6053066e6055d5a7e014aa1f2df1e4ee838e8510 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Dec 2015 19:22:36 +0100 Subject: [PATCH 620/624] Limit jerk of all commanded trust in velocity mode so that we never get steps in the attitude response. --- .../mc_pos_control/mc_pos_control_main.cpp | 44 +++++++++++++++++-- 1 file changed, 40 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a2f38f2828..9f650a9d66 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -91,6 +91,7 @@ #define SIGMA 0.000001f #define MIN_DIST 0.01f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f +#define ONE_G 9.8066f /** * Multicopter position control app start / stop handling function @@ -235,6 +236,7 @@ private: math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; math::Vector<3> _vel_sp_prev; + math::Vector<3> _thrust_sp_prev; math::Vector<3> _vel_err_d; /**< derivative of current velocity */ math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ @@ -1407,6 +1409,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_velocity_enabled) { + /* limit max tilt */ if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ @@ -1494,6 +1497,36 @@ MulticopterPositionControl::task_main() } } + /* limit max change rate */ + math::Vector<2> jerk_hor; + math::Vector<3> thrust_diff = thrust_sp - _thrust_sp_prev; + + /* this is jerk times dt + * we call it jerk so its clear what the intention is - + * to limit the jerk, not the total acceleration. + */ + jerk_hor(0) = thrust_diff(0); + jerk_hor(1) = thrust_diff(1); + + /* because we compare with jerk times dt, we multiply here as well */ + float max_hor_jerk = (_params.acc_hor_max / ONE_G) * dt; + + if (jerk_hor.length() > max_hor_jerk) { + /* bring it to length one */ + jerk_hor.normalize(); + /* bring it to the max length for jerk * dt for the next step */ + jerk_hor *= max_hor_jerk; + /* now use the previous trust setpoint and add the allowable delta */ + math::Vector<2> thrust_sp_hor_prev(_thrust_sp_prev(0), _thrust_sp_prev(1)); + math::Vector<2> thrust_sp_hor = thrust_sp_hor_prev + jerk_hor; + + thrust_sp(0) = thrust_sp_hor(0); + thrust_sp(1) = thrust_sp_hor(1); + } + + /* store last thrust vector */ + _thrust_sp_prev = thrust_sp; + /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ @@ -1577,9 +1610,9 @@ MulticopterPositionControl::task_main() _att_sp.thrust = thrust_abs; /* save thrust setpoint for logging */ - _local_pos_sp.acc_x = thrust_sp(0); - _local_pos_sp.acc_y = thrust_sp(1); - _local_pos_sp.acc_z = thrust_sp(2); + _local_pos_sp.acc_x = thrust_sp(0) * ONE_G; + _local_pos_sp.acc_y = thrust_sp(1) * ONE_G; + _local_pos_sp.acc_z = thrust_sp(2) * ONE_G; _att_sp.timestamp = hrt_absolute_time(); @@ -1620,7 +1653,8 @@ MulticopterPositionControl::task_main() } /* generate attitude setpoint from manual controls */ - if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled + && !_control_mode.flag_control_velocity_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -1670,6 +1704,8 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); + _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + } else { reset_yaw_sp = true; } From bc3f5daeec8697d1aaf7c9d8461948eeab881168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Dec 2015 21:06:01 +0000 Subject: [PATCH 621/624] Fix attitude yaw generation, store last acceleration setpoint for all non-velocity / position flight modes. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9f650a9d66..2596e8d89e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1652,9 +1652,10 @@ MulticopterPositionControl::task_main() _vel_sp_prev = _vel; } + math::Matrix<3, 3> R_sp; + /* generate attitude setpoint from manual controls */ - if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled - && !_control_mode.flag_control_velocity_enabled) { + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -1694,7 +1695,6 @@ MulticopterPositionControl::task_main() } /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); @@ -1704,8 +1704,6 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); - } else { reset_yaw_sp = true; } @@ -1723,6 +1721,8 @@ MulticopterPositionControl::task_main() !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { + _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); From d2e4566c49f0023ccb19f480fa8d789dbf590320 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:29:51 +0000 Subject: [PATCH 622/624] Reset Iris jMAVSim tuning gains from master --- posix-configs/SITL/init/rcS_jmavsim_iris | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 0323bc9b13..88a3f3978e 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -2,10 +2,10 @@ uorb start simulator start -s param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.12 -param set MC_PITCH_P 6 -param set MC_ROLL_P 6 -param set MC_ROLLRATE_P 0.12 +param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 +param set MC_ROLLRATE_P 0.15 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 @@ -24,9 +24,9 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.15 -param set MPC_XY_VEL_P 0.12 -param set MPC_XY_VEL_D 0.012 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 param set COM_RC_IN_MODE 1 param set NAV_ACC_RAD 2.0 From 35df66ce5d50bc35266944c3497f2928a3508c88 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:33:25 +0000 Subject: [PATCH 623/624] MC pos control: Fix attitude and thrust SP generation --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2596e8d89e..97d0e097f6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1652,8 +1652,6 @@ MulticopterPositionControl::task_main() _vel_sp_prev = _vel; } - math::Matrix<3, 3> R_sp; - /* generate attitude setpoint from manual controls */ if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { @@ -1694,10 +1692,20 @@ MulticopterPositionControl::task_main() } } + math::Matrix<3, 3> R_sp; + /* construct attitude setpoint rotation matrix */ R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); + /* reset the acceleration set point for all non-attitude flight modes */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + + _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + } + /* copy quaternion setpoint to attitude setpoint topic */ math::Quaternion q_sp; q_sp.from_dcm(R_sp); @@ -1721,8 +1729,6 @@ MulticopterPositionControl::task_main() !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); - if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); From 0f67d3fcd8232ff342a8dc1fd2843fd0a1836ebf Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 15 Dec 2015 10:59:56 +0100 Subject: [PATCH 624/624] removed jerk fix and reset prev horizontal vel sp with current velocity --- .../mc_pos_control/mc_pos_control_main.cpp | 33 ++----------------- 1 file changed, 3 insertions(+), 30 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 97d0e097f6..a49714ff52 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1284,6 +1284,8 @@ MulticopterPositionControl::task_main() } if (!_control_mode.flag_control_velocity_enabled) { + _vel_sp_prev(0) = _vel(0); + _vel_sp_prev(1) = _vel(1); _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -1366,6 +1368,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* thrust vector in NED frame */ + // TODO?: + _vel_sp.emult(_params.vel_ff) math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { @@ -1497,36 +1500,6 @@ MulticopterPositionControl::task_main() } } - /* limit max change rate */ - math::Vector<2> jerk_hor; - math::Vector<3> thrust_diff = thrust_sp - _thrust_sp_prev; - - /* this is jerk times dt - * we call it jerk so its clear what the intention is - - * to limit the jerk, not the total acceleration. - */ - jerk_hor(0) = thrust_diff(0); - jerk_hor(1) = thrust_diff(1); - - /* because we compare with jerk times dt, we multiply here as well */ - float max_hor_jerk = (_params.acc_hor_max / ONE_G) * dt; - - if (jerk_hor.length() > max_hor_jerk) { - /* bring it to length one */ - jerk_hor.normalize(); - /* bring it to the max length for jerk * dt for the next step */ - jerk_hor *= max_hor_jerk; - /* now use the previous trust setpoint and add the allowable delta */ - math::Vector<2> thrust_sp_hor_prev(_thrust_sp_prev(0), _thrust_sp_prev(1)); - math::Vector<2> thrust_sp_hor = thrust_sp_hor_prev + jerk_hor; - - thrust_sp(0) = thrust_sp_hor(0); - thrust_sp(1) = thrust_sp_hor(1); - } - - /* store last thrust vector */ - _thrust_sp_prev = thrust_sp; - /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */