From b23af6108772f8049ca94dfd8c648e1014917062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:47:35 +0100 Subject: [PATCH 001/154] System disables all driver publications it can get hold of once entering HIL --- .../commander/state_machine_helper.cpp | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6b..44e3aa787f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include #include @@ -51,6 +53,7 @@ #include #include #include +#include #include #include "state_machine_helper.h" @@ -491,6 +494,34 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; + + // Disable publication of all attached sensors + + /* list directory */ + DIR *d; + struct dirent *direntry; + d = opendir("/dev"); + if (d) { + + while ((direntry = readdir(d)) != NULL) { + + bool blocked = false; + int sensfd = ::open(direntry->d_name, 0); + ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (blocked) ? "OK" : "FAIL"); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); + return 1; + } } break; From c6e196edca7acae0b548a85c94d0c8f37df3c7aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:52:19 +0100 Subject: [PATCH 002/154] Support disabling GPS output via IOCTL, general cleanup of author and copyright code style --- src/drivers/gps/gps.cpp | 21 +++++++++++++-------- src/drivers/gps/gps_helper.cpp | 7 ++++--- src/drivers/gps/gps_helper.h | 6 +++--- src/drivers/gps/module.mk | 2 +- src/drivers/gps/mtk.cpp | 11 +++++++---- src/drivers/gps/mtk.h | 11 +++++++---- src/drivers/gps/ubx.cpp | 10 ++++++---- src/drivers/gps/ubx.h | 17 ++++++++++++----- 8 files changed, 53 insertions(+), 32 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560fa..a736cbdf6b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -289,11 +289,13 @@ GPS::task_main() //no time and satellite information simulated - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } usleep(2e5); @@ -330,11 +332,14 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } last_rate_count++; diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2e2cbc8ddf..2360ff39bd 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2012-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 @@ -41,6 +39,9 @@ /** * @file gps_helper.cpp + * + * @author Thomas Gubler + * @author Julian Oes */ float diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 73d4b889cb..cfb9e0d43d 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2012-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 @@ -35,6 +33,8 @@ /** * @file gps_helper.h + * @author Thomas Gubler + * @author Julian Oes */ #ifndef GPS_HELPER_H diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 097db2abfc..82c67d40a3 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-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 diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 56b702ea6c..456a9645bf 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * 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 @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mkt.cpp */ +/** + * @file mkt.cpp + * + * @author Thomas Gubler + * @author Julian Oes + */ #include #include diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index b5cfbf0a6d..9032e45b03 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * 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 @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mtk.h */ +/** + * @file mkt.cpp + * + * @author Thomas Gubler + * @author Julian Oes + */ #ifndef MTK_H_ #define MTK_H_ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 86291901cb..8a2afecb7e 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * 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 @@ -40,8 +37,13 @@ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description * including Prototol Specification. * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ + #include #include #include diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 76ef873a36..79a904f4a4 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * 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 @@ -34,7 +31,17 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/** + * @file ubx.h + * + * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * + */ #ifndef UBX_H_ #define UBX_H_ From 9bf512cac82815e690774ddfc2fdeda29c22f4a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:52:41 +0100 Subject: [PATCH 003/154] Framework to support disabling publications via IOCTL --- src/drivers/device/cdev.cpp | 11 ++++++++++- src/drivers/device/device.cpp | 2 +- src/drivers/device/device.h | 4 +++- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 7954ce5ab5..65a9705f55 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -38,6 +38,7 @@ */ #include "device.h" +#include "drivers/drv_device.h" #include #include @@ -93,6 +94,7 @@ CDev::CDev(const char *name, Device(name, irq), // public // protected + _pub_blocked(false), // private _devname(devname), _registered(false), @@ -256,6 +258,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; return OK; + break; + case DEVIOCSPUBBLOCK: + _pub_blocked = (arg != 0); + break; + case DEVIOCGPUBBLOCK: + return _pub_blocked; + break; } return -ENOTTY; diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index c3ee77b1ca..6834551493 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 0235f62844..d99f229220 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -415,6 +415,8 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + bool _pub_blocked; /**< true if publishing should be blocked */ + private: static const unsigned _max_pollwaiters = 8; From d72c82f66bf6dac8e6d10bf1024641908d3b854c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:53:15 +0100 Subject: [PATCH 004/154] Airspeed does not publish if disabled --- src/drivers/airspeed/airspeed.cpp | 12 +++++++----- src/drivers/airspeed/airspeed.h | 4 +++- src/drivers/ets_airspeed/ets_airspeed.cpp | 15 +++++++++++++-- src/drivers/meas_airspeed/meas_airspeed.cpp | 15 +++++++++++++-- 4 files changed, 36 insertions(+), 10 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936c..215d3792e8 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) @@ -102,6 +103,9 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); + if (_class_instance != -1) + unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -126,10 +130,8 @@ Airspeed::init() if (_reports == nullptr) goto out; - /* get a publish handle on the airspeed topic */ - differential_pressure_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(AIRSPEED_DEVICE_PATH); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c341aa2c6d..c27b1bcd80 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -127,6 +127,8 @@ protected: orb_advert_t _airspeed_pub; + int _class_instance; + unsigned _conversion_interval; perf_counter_t _sample_perf; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index de371bf324..cdc70ac37c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -185,7 +185,18 @@ ETSAirspeed::collect() report.max_differential_pressure_pa = _max_differential_pressure_pa; /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_airspeed_pub > 0) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } else { + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report); + + if (_airspeed_pub < 0) + debug("failed to create differential_pressure publication"); + } + } new_report(report); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a95c4576b0..fee13f139c 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -217,7 +217,18 @@ MEASAirspeed::collect() report.max_differential_pressure_pa = _max_differential_pressure_pa; /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_airspeed_pub > 0) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } else { + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report); + + if (_airspeed_pub < 0) + debug("failed to create differential_pressure publication"); + } + } new_report(report); From c7e2841baa98bb985b402c89bef85e56f765ec11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:53:31 +0100 Subject: [PATCH 005/154] BMA180 does not publish if disabled --- src/drivers/bma180/bma180.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 1590cc182b..df4e8f9989 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -723,7 +723,8 @@ BMA180::measure() poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); + if !(_pub_blocked) + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); From 28a3dc726f8e4f3696736798a1d92bf7d9de6100 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:53:56 +0100 Subject: [PATCH 006/154] Support for publication blocking: HMC5883 --- src/drivers/hmc5883/hmc5883.cpp | 37 +++++++++++++++++---------------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66e..49b72cf794 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -381,16 +381,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the mag topic if we are - * the primary mag */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); - } ret = OK; /* sensor is ok, but not calibrated */ @@ -885,9 +875,18 @@ HMC5883::collect() } #endif - if (_mag_topic != -1) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } else { + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report); + + if (_mag_topic < 0) + debug("failed to create sensor_mag publication"); + } + } /* post a report to the ring */ @@ -1134,10 +1133,12 @@ int HMC5883::check_calibration() SUBSYSTEM_TYPE_MAG}; static orb_advert_t pub = -1; - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + if (!(_pub_blocked)) { + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } } From 3c7766db6c58dea67b66263d2a7c01bb57177db5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:54:10 +0100 Subject: [PATCH 007/154] Support for publication blocking: L3GD20(H) --- src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 670e51b979..e885b1bf9e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -379,12 +379,6 @@ L3GD20::init() goto out; _class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); - } reset(); @@ -894,8 +888,19 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_gyro_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + } else { + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &report); + + if (_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + } + + } _read++; From a34a14ce862c270192283514d8a1914fbe43bd48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:54:25 +0100 Subject: [PATCH 008/154] Support for publication blocking: LSM303D, cleaned up device start --- src/drivers/lsm303d/lsm303d.cpp | 64 +++++++++++++++++---------------- 1 file changed, 34 insertions(+), 30 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 969b5e25f9..3bd7a66f6b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -277,15 +277,15 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - int _class_instance; + int _accel_class_instance; unsigned _accel_read; unsigned _mag_read; perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _reg7_resets; perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; @@ -295,8 +295,8 @@ private: // expceted values of reg1 and reg7 to catch in-flight // brownouts of the sensor - uint8_t _reg7_expected; uint8_t _reg1_expected; + uint8_t _reg7_expected; // accel logging int _accel_log_fd; @@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _class_instance(-1), + _accel_class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -551,8 +551,8 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; - if (_class_instance != -1) - unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); delete _mag; @@ -562,13 +562,13 @@ LSM303D::~LSM303D() perf_free(_reg1_resets); perf_free(_reg7_resets); perf_free(_extreme_values); + perf_free(_accel_reschedules); } int LSM303D::init() { int ret = ERROR; - int mag_ret; /* do SPI init (and probe) first */ if (SPI::init() != OK) { @@ -597,14 +597,7 @@ LSM303D::init() goto out; } - _class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - // we are the primary accel device, so advertise to - // the ORB - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); out: return ret; @@ -727,7 +720,7 @@ LSM303D::check_extremes(const accel_report *arb) _last_log_us = now; ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", (unsigned long long)arb->timestamp, - arb->x, arb->y, arb->z, + (double)arb->x, (double)arb->y, (double)arb->z, (int)arb->x_raw, (int)arb->y_raw, (int)arb->z_raw, @@ -1517,9 +1510,18 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_accel_topic != -1) { - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_accel_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } else { + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &accel_report); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + } + } _accel_read++; @@ -1591,9 +1593,18 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_mag->_mag_topic != -1) { - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_mag->_mag_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } else { + _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mag_report); + + if (_mag->_mag_topic < 0) + debug("failed to create sensor_mag publication"); + } + } _mag_read++; @@ -1707,13 +1718,6 @@ LSM303D_mag::init() goto out; _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); - if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { - // we are the primary mag device, so advertise to - // the ORB - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - } out: return ret; From 7af62bbe9e4baaa846a37176d6942e1893e42715 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:54:38 +0100 Subject: [PATCH 009/154] Support for publication blocking: MPU6000, cleaned up device start --- src/drivers/mpu6000/mpu6000.cpp | 46 +++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bbc595af46..02fe6df4a1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -443,7 +443,6 @@ int MPU6000::init() { int ret; - int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -488,16 +487,7 @@ MPU6000::init() return ret; } - /* fetch an initial set of measurements for advertisement */ - measure(); - _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); - } out: return ret; @@ -1307,11 +1297,32 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic != -1) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_accel_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } else { + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + } + } - if (_gyro->_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + + if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_gyro->_gyro_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } else { + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb); + + if (_gyro->_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + } + } /* stop measuring */ @@ -1356,11 +1367,6 @@ MPU6000_gyro::init() } _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { - gyro_report gr; - memset(&gr, 0, sizeof(gr)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } out: return ret; From e6a67b1deb0e7d5b315d3c570ff010de4d54b65f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:54:55 +0100 Subject: [PATCH 010/154] Support for publication blocking: MS5611, cleaned up device start --- src/drivers/ms5611/ms5611.cpp | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6326cf7fcb..0893315667 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -132,6 +132,8 @@ protected: orb_advert_t _baro_topic; + int _class_instance; + perf_counter_t _sample_perf; perf_counter_t _measure_perf; perf_counter_t _comms_errors; @@ -204,6 +206,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), @@ -218,6 +221,9 @@ MS5611::~MS5611() /* make sure we are truly inactive */ stop_cycle(); + if (_class_instance != -1) + unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -251,16 +257,8 @@ MS5611::init() goto out; } - /* get a publish handle on the baro topic */ - struct baro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); - - if (_baro_topic < 0) { - debug("failed to create sensor_baro object"); - ret = -ENOSPC; - goto out; - } + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_DEVICE_PATH); ret = OK; out: @@ -670,7 +668,18 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_baro_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } else { + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &report); + + if (_baro_topic < 0) + debug("failed to create sensor_baro publication"); + } + } if (_reports->force(&report)) { perf_count(_buffer_overflows); From dbbe4ab1d587376bc2d530b3e80a001aa30b69c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:55:16 +0100 Subject: [PATCH 011/154] Header for publication disable --- src/drivers/drv_device.h | 62 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 src/drivers/drv_device.h diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h new file mode 100644 index 0000000000..b310beb746 --- /dev/null +++ b/src/drivers/drv_device.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 drv_device.h + * + * Generic device / sensor interface. + */ + +#ifndef _DRV_DEVICE_H +#define _DRV_DEVICE_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +/* + * ioctl() definitions + */ + +#define _DEVICEIOCBASE (0x100) +#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n)) + +/** ask device to stop publishing */ +#define DEVIOCSPUBBLOCK _DEVICEIOC(0) + +/** check publication block status */ +#define DEVIOCGPUBBLOCK _DEVICEIOC(1) + +#endif /* _DRV_DEVICE_H */ From 47e0c926a6932b7a60ce85a5c748ce5bfcc102e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 14:02:16 +0100 Subject: [PATCH 012/154] Fixed two typos identified by kroimon --- src/drivers/gps/mtk.cpp | 2 +- src/drivers/gps/mtk.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 456a9645bf..c90ecbe281 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mkt.cpp + * @file mtk.cpp * * @author Thomas Gubler * @author Julian Oes diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index 9032e45b03..a2d5e27bbf 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mkt.cpp + * @file mtk.cpp * * @author Thomas Gubler * @author Julian Oes From c4dc310ebda8f79ec13c68745408444661b32fe1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 14:03:57 +0100 Subject: [PATCH 013/154] Fixed bogus return value of publication blocking disable --- src/modules/commander/state_machine_helper.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 44e3aa787f..7ae61d9ef2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -505,12 +505,11 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s while ((direntry = readdir(d)) != NULL) { - bool blocked = false; int sensfd = ::open(direntry->d_name, 0); - ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (blocked) ? "OK" : "FAIL"); + printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); } closedir(d); From d19971065140bdfbbe5972f2a394597504abef9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 15:40:46 +0100 Subject: [PATCH 014/154] Fixed up init sequence of all sensors - we can publish in interrupt context, but not advertise! All advertisements now contain valid data --- src/drivers/airspeed/airspeed.cpp | 16 ++++- src/drivers/bma180/bma180.cpp | 22 ++++-- src/drivers/ets_airspeed/ets_airspeed.cpp | 15 +--- src/drivers/l3gd20/l3gd20.cpp | 30 ++++---- src/drivers/lsm303d/lsm303d.cpp | 61 +++++++++------- src/drivers/meas_airspeed/meas_airspeed.cpp | 15 +--- src/drivers/mpu6000/mpu6000.cpp | 59 ++++++++------- src/drivers/ms5611/ms5611.cpp | 80 +++++++++++++++------ 8 files changed, 186 insertions(+), 112 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 215d3792e8..71c0b70f01 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -133,8 +133,20 @@ Airspeed::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_DEVICE_PATH); - if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. Did you start uOrb?"); + /* publication init */ + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct differential_pressure_s arp; + measure(); + _reports->get(&arp); + + /* measurement will have generated a report, publish */ + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); + + if (_airspeed_pub < 0) + warnx("failed to create airspeed sensor object. uORB started?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index df4e8f9989..e43a348052 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -153,6 +153,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _class_instance; unsigned _current_lowpass; unsigned _current_range; @@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _class_instance(-1), _current_lowpass(0), _current_range(0), _sample_perf(perf_alloc(PC_ELAPSED, "bma180_read")) @@ -282,11 +284,6 @@ BMA180::init() if (_reports == nullptr) goto out; - /* advertise sensor topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -322,6 +319,19 @@ BMA180::init() ret = ERROR; } + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + + /* advertise sensor topic, measure manually to initialize valid report */ + measure(); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + struct accel_report arp; + _reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + } + out: return ret; } @@ -723,7 +733,7 @@ BMA180::measure() poll_notify(POLLIN); /* publish for subscribers */ - if !(_pub_blocked) + if (_accel_topic > 0 && !(_pub_blocked)) orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index cdc70ac37c..8bbef5cfa0 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -184,18 +184,9 @@ ETSAirspeed::collect() report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* announce the airspeed if needed, just publish else */ - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_airspeed_pub > 0) { - /* publish it */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - } else { - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report); - - if (_airspeed_pub < 0) - debug("failed to create differential_pressure publication"); - } + if (_airspeed_pub > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } new_report(report); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e885b1bf9e..90c3db9ae9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -382,6 +382,21 @@ L3GD20::init() reset(); + measure(); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + + if (_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + + } + ret = OK; out: return ret; @@ -888,18 +903,9 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_gyro_topic > 0) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); - } else { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &report); - - if (_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); - } - + if (_gyro_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3bd7a66f6b..4dee7649bc 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -597,8 +597,39 @@ LSM303D::init() goto out; } + /* fill report structures */ + measure(); + + if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + /* measurement will have generated a report, publish */ + _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); + + if (_mag->_mag_topic < 0) + debug("failed to create sensor_mag publication"); + + } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + + } + out: return ret; } @@ -1510,18 +1541,9 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_accel_topic > 0) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } else { - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &accel_report); - - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); - } - + if (_accel_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } _accel_read++; @@ -1593,18 +1615,9 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_mag->_mag_topic > 0) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); - } else { - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mag_report); - - if (_mag->_mag_topic < 0) - debug("failed to create sensor_mag publication"); - } - + if (_mag->_mag_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } _mag_read++; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index fee13f139c..51a059e394 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -216,18 +216,9 @@ MEASAirspeed::collect() report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* announce the airspeed if needed, just publish else */ - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_airspeed_pub > 0) { - /* publish it */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - } else { - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report); - - if (_airspeed_pub < 0) - debug("failed to create differential_pressure publication"); - } + if (_airspeed_pub > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); } new_report(report); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 02fe6df4a1..bf80c9cff2 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -489,6 +489,35 @@ MPU6000::init() _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + measure(); + + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + + } + + if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + + if (_gyro->_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + + } + out: return ret; } @@ -1297,32 +1326,14 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_accel_topic > 0) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - } else { - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb); - - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); - } - + if (_accel_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_gyro->_gyro_topic > 0) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); - } else { - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb); - - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); - } - + if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0893315667..0ef056273f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,6 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_BARO_DEVICE_PATH "/dev/ms5611" class MS5611 : public device::CDev { @@ -194,7 +195,7 @@ protected: extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : - CDev("MS5611", BARO_DEVICE_PATH), + CDev("MS5611", MS5611_BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -222,7 +223,7 @@ MS5611::~MS5611() stop_cycle(); if (_class_instance != -1) - unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance); /* free any existing reports */ if (_reports != nullptr) @@ -260,7 +261,54 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); - ret = OK; + struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + _reports->flush(); + + /* this do..while is goto without goto */ + do { + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + _reports->get(&brp); + + ret = OK; + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp); + + if (_baro_topic < 0) + debug("failed to create sensor_baro publication"); + } + + } while (0); + out: return ret; } @@ -668,17 +716,9 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { - - if (_baro_topic > 0) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - } else { - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &report); - - if (_baro_topic < 0) - debug("failed to create sensor_baro publication"); - } + if (_baro_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } if (_reports->force(&report)) { @@ -821,7 +861,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(BARO_DEVICE_PATH, O_RDONLY); + fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) { warnx("can't open baro device"); goto fail; @@ -855,10 +895,10 @@ test() ssize_t sz; int ret; - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -914,7 +954,7 @@ test() void reset() { - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -953,10 +993,10 @@ calibrate(unsigned altitude) float pressure; float p1; - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); /* start the sensor polling at max */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) From 58d78e57b7b7de57a347ad94e70d2d19bb7230d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 15:41:12 +0100 Subject: [PATCH 015/154] Build the sensors as part of the test binary --- makefiles/config_px4fmu-v1_test.mk | 3 +++ makefiles/config_px4fmu-v2_test.mk | 9 +++++++++ 2 files changed, 12 insertions(+) diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk index 41e8b95ff5..300afa3d5b 100644 --- a/makefiles/config_px4fmu-v1_test.mk +++ b/makefiles/config_px4fmu-v1_test.mk @@ -29,6 +29,9 @@ MODULES += systemcmds/nshterm MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index f54a4d8257..ea52213df0 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -18,6 +18,12 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/px4io +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests @@ -30,6 +36,9 @@ MODULES += systemcmds/mtd MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter # # Transitional support - add commands from the NuttX export archive. From bb8956c84e83c22d143a99d4ca37491574200438 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 16:04:12 +0100 Subject: [PATCH 016/154] Fixed return value --- src/drivers/device/cdev.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 65a9705f55..b157b3f181 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -261,6 +261,7 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) break; case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); + return OK; break; case DEVIOCGPUBBLOCK: return _pub_blocked; From ac326beaaae7b38d65ad6d7d13f00dfeaa6ae520 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 16:04:26 +0100 Subject: [PATCH 017/154] Improved config tool to also do device IOCTLs --- src/systemcmds/config/config.c | 56 +++++++++++++++++++++++++++++----- 1 file changed, 49 insertions(+), 7 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 80689f20c8..476015f3e3 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -56,6 +56,7 @@ #include #include #include +#include #include "systemlib/systemlib.h" #include "systemlib/err.h" @@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]); static void do_gyro(int argc, char *argv[]); static void do_accel(int argc, char *argv[]); static void do_mag(int argc, char *argv[]); +static void do_device(int argc, char *argv[]); int config_main(int argc, char *argv[]) @@ -72,20 +74,60 @@ config_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { do_gyro(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "accel")) { + } else if (!strcmp(argv[1], "accel")) { do_accel(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "mag")) { + } else if (!strcmp(argv[1], "mag")) { do_mag(argc - 2, argv + 2); + } else { + do_device(argc - 1, argv + 1); } } errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); } +static void +do_device(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "no device path provided and command provided."); + } + + int fd; + int ret; + + fd = open(argv[0], 0); + + if (fd < 0) { + warn("%s", argv[0]); + errx(1, "FATAL: no device found"); + + } else { + + if (argc == 2 && !strcmp(argv[1], "block")) { + + /* disable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); + + if (ret) + errx(ret,"uORB publications could not be blocked"); + + } else if (argc == 2 && !strcmp(argv[1], "unblock")) { + + /* enable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); + + if (ret) + errx(ret,"uORB publications could not be unblocked"); + + } else { + errx("no valid command: %s", argv[1]); + } + } + + exit(0); +} + static void do_gyro(int argc, char *argv[]) { @@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[]) if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 1 && !strcmp(argv[0], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { From 9defc6cb235e13ef912a70466acf1d14314f1e3d Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 14:26:17 +0100 Subject: [PATCH 018/154] mkblctrl fmuv2 support added --- makefiles/config_px4fmu-v2_default.mk | 2 + src/drivers/mkblctrl/mkblctrl.cpp | 152 +++++++++++++++++++++++++- 2 files changed, 148 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 880e2738ac..dc9208339b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -38,6 +38,8 @@ MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors +MODULES += drivers/mkblctrl + # Needs to be burned to the ground and re-written; for now, # just don't build it. diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 30d6069b32..dbba91786b 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * Author: Marco Bauer * * Redistribution and use in source and binary forms, with or without @@ -93,6 +93,9 @@ #define MOTOR_SPINUP_COUNTER 30 #define ESC_UORB_PUBLISH_DELAY 500000 + + + class MK : public device::I2C { public: @@ -181,6 +184,7 @@ private: static const unsigned _ngpio; void gpio_reset(void); + void sensor_reset(int ms); void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); @@ -196,6 +200,7 @@ private: }; const MK::GPIOConfig MK::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -204,6 +209,22 @@ const MK::GPIOConfig MK::_gpio_tab[] = { {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + {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_5V_PERIPH_EN, 0}, + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, + {GPIO_VDD_SERVO_VALID, 0, 0}, + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, +#endif }; const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]); @@ -623,9 +644,11 @@ MK::task_main() if(!_overrideSecurityChecks) { /* don't go under BLCTRL_MIN_VALUE */ + if (outputs.output[i] < BLCTRL_MIN_VALUE) { outputs.output[i] = BLCTRL_MIN_VALUE; } + } /* output to BLCtrl's */ @@ -1075,6 +1098,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; @@ -1198,23 +1225,115 @@ MK::write(file *filp, const char *buffer, size_t len) return count * 2; } +void +MK::sensor_reset(int ms) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_OFF); + + stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 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_GYRO_DRDY_OFF); + stm32_configgpio(GPIO_MAG_DRDY_OFF); + stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); + + stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 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_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 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 +} + + void MK::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip - * to input mode. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* if we have a GPIO direction control, set it to zero (input) */ stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_configgpio(GPIO_GPIO_DIR); +#endif } void MK::gpio_set_function(uint32_t gpios, int function) { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -1227,6 +1346,8 @@ MK::gpio_set_function(uint32_t gpios, int function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } +#endif + /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { if (gpios & (1 << i)) { @@ -1248,9 +1369,13 @@ MK::gpio_set_function(uint32_t gpios, int function) } } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); + +#endif } void @@ -1418,6 +1543,20 @@ mk_start(unsigned bus, unsigned motors, char *device_path) return ret; } +void +sensor_reset(int ms) +{ + int fd; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) + err(1, "servo arm failed"); + +} int mk_check_for_i2c_esc_bus(char *device_path, int motors) @@ -1426,6 +1565,7 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) if (g_mk == nullptr) { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) g_mk = new MK(3, device_path); if (g_mk == nullptr) { @@ -1441,7 +1581,7 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) } } - +#endif g_mk = new MK(1, device_path); From 0089db7ef3961c36d6513877b7681ab548d20ccf Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 16:28:56 +0100 Subject: [PATCH 019/154] code cleanup --- src/drivers/mkblctrl/mkblctrl.cpp | 425 +++--------------------------- 1 file changed, 30 insertions(+), 395 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index dbba91786b..c3c4bf8c1d 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -65,7 +65,7 @@ #include #include -#include +//#include #include #include @@ -123,8 +123,7 @@ public: virtual int init(unsigned motors); virtual ssize_t write(file *filp, const char *buffer, size_t len); - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); + int set_update_rate(unsigned rate); int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); @@ -136,7 +135,6 @@ private: static const unsigned _max_actuators = MAX_MOTORS; static const bool showDebug = false; - Mode _mode; int _update_rate; int _current_update_rate; int _task; @@ -183,51 +181,15 @@ private: static const GPIOConfig _gpio_tab[]; static const unsigned _ngpio; - void gpio_reset(void); - void sensor_reset(int ms); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); - int mk_servo_set(unsigned int chan, short val); int mk_servo_set_value(unsigned int chan, short val); int mk_servo_test(unsigned int chan); short scaling(float val, float inMin, float inMax, float outMin, float outMax); - - }; -const MK::GPIOConfig MK::_gpio_tab[] = { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - {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_5V_PERIPH_EN, 0}, - {0, GPIO_VDD_3V3_SENSORS_EN, 0}, - {GPIO_VDD_BRICK_VALID, 0, 0}, - {GPIO_VDD_SERVO_VALID, 0, 0}, - {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, - {GPIO_VDD_5V_PERIPH_OC, 0, 0}, -#endif -}; -const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]); const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration @@ -268,8 +230,7 @@ MK *g_mk; MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), - _mode(MODE_NONE), - _update_rate(50), + _update_rate(400), _task(-1), _t_actuators(-1), _t_actuator_armed(-1), @@ -348,9 +309,6 @@ MK::init(unsigned motors) } - /* reset GPIOs */ - gpio_reset(); - /* start the IO interface task */ _task = task_spawn_cmd("mkblctrl", SCHED_DEFAULT, @@ -375,43 +333,7 @@ MK::task_main_trampoline(int argc, char *argv[]) } int -MK::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; /* default output rate */ - break; - - case MODE_4PWM: - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - -int -MK::set_pwm_rate(unsigned rate) +MK::set_update_rate(unsigned rate) { if ((rate > 500) || (rate < 10)) return -EINVAL; @@ -1042,28 +964,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - - /* try it as a GPIO ioctl first */ - ret = gpio_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) - return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - /* - switch (_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_6PWM: - ret = pwm_ioctl(filp, cmd, arg); - break; - - default: - debug("not in a PWM mode"); - break; - } - */ ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ @@ -1099,7 +999,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_GET_UPDATE_RATE: - *(uint32_t *)arg = 400; + *(uint32_t *)arg = _update_rate; break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: @@ -1225,237 +1125,10 @@ MK::write(file *filp, const char *buffer, size_t len) return count * 2; } -void -MK::sensor_reset(int ms) -{ -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - - if (ms < 1) { - ms = 1; - } - - /* disable SPI bus */ - stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); - stm32_configgpio(GPIO_SPI_CS_BARO_OFF); - stm32_configgpio(GPIO_SPI_CS_MPU_OFF); - - stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 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_GYRO_DRDY_OFF); - stm32_configgpio(GPIO_MAG_DRDY_OFF); - stm32_configgpio(GPIO_ACCEL_DRDY_OFF); - stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 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_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); - stm32_configgpio(GPIO_SPI_CS_MPU); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 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 -} - - -void -MK::gpio_reset(void) -{ - /* - * Setup default GPIO config - all pins as GPIOs, input if - * possible otherwise output if possible. - */ - for (unsigned i = 0; i < _ngpio; i++) { - if (_gpio_tab[i].input != 0) { - stm32_configgpio(_gpio_tab[i].input); - - } else if (_gpio_tab[i].output != 0) { - stm32_configgpio(_gpio_tab[i].output); - } - } - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - /* if we have a GPIO direction control, set it to zero (input) */ - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - stm32_configgpio(GPIO_GPIO_DIR); -#endif -} - -void -MK::gpio_set_function(uint32_t gpios, int function) -{ -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - - /* - * GPIOs 0 and 1 must have the same direction as they are buffered - * by a shared 2-port driver. Any attempt to set either sets both. - */ - if (gpios & 3) { - gpios |= 3; - - /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) - stm32_gpiowrite(GPIO_GPIO_DIR, 1); - } - -#endif - - /* configure selected GPIOs as required */ - for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1 << i)) { - switch (function) { - case GPIO_SET_INPUT: - stm32_configgpio(_gpio_tab[i].input); - break; - - case GPIO_SET_OUTPUT: - stm32_configgpio(_gpio_tab[i].output); - break; - - case GPIO_SET_ALT_1: - if (_gpio_tab[i].alt != 0) - stm32_configgpio(_gpio_tab[i].alt); - - break; - } - } - } - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - - /* flip buffer to input mode if required */ - if ((GPIO_SET_INPUT == function) && (gpios & 3)) - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - -#endif -} - -void -MK::gpio_write(uint32_t gpios, int function) -{ - int value = (function == GPIO_SET) ? 1 : 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1 << i)) - stm32_gpiowrite(_gpio_tab[i].output, value); -} - -uint32_t -MK::gpio_read(void) -{ - uint32_t bits = 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) - bits |= (1 << i); - - return bits; -} - -int -MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - - case GPIO_RESET: - gpio_reset(); - break; - - case GPIO_SET_OUTPUT: - case GPIO_SET_INPUT: - case GPIO_SET_ALT_1: - gpio_set_function(arg, cmd); - break; - - case GPIO_SET_ALT_2: - case GPIO_SET_ALT_3: - case GPIO_SET_ALT_4: - ret = -EINVAL; - break; - - case GPIO_SET: - case GPIO_CLEAR: - gpio_write(arg, cmd); - break; - - case GPIO_GET: - *(uint32_t *)arg = gpio_read(); - break; - - default: - ret = -ENOTTY; - } - - unlock(); - - return ret; -} namespace { -enum PortMode { - PORT_MODE_UNSET = 0, - PORT_FULL_GPIO, - PORT_FULL_SERIAL, - PORT_FULL_PWM, - PORT_GPIO_AND_SERIAL, - PORT_PWM_AND_SERIAL, - PORT_PWM_AND_GPIO, -}; - enum MappingMode { MAPPING_MK = 0, MAPPING_PX4, @@ -1466,20 +1139,11 @@ enum FrameType { FRAME_X, }; -PortMode g_port_mode; int -mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) +mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) { - uint32_t gpio_bits; int shouldStop = 0; - MK::Mode servo_mode; - - /* reset to all-inputs */ - g_mk->ioctl(0, GPIO_RESET, 0); - - gpio_bits = 0; - servo_mode = MK::MODE_NONE; /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); @@ -1493,7 +1157,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* ovveride security checks if enabled */ g_mk->set_overrideSecurityChecks(overrideSecurityChecks); - /* count used motors */ do { if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { @@ -1508,12 +1171,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); - /* (re)set the PWM output mode */ - g_mk->set_mode(servo_mode); - - - if ((servo_mode != MK::MODE_NONE) && (update_rate != 0)) - g_mk->set_pwm_rate(update_rate); + g_mk->set_update_rate(update_rate); return OK; } @@ -1543,60 +1201,38 @@ mk_start(unsigned bus, unsigned motors, char *device_path) return ret; } -void -sensor_reset(int ms) -{ - int fd; - - fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - - if (fd < 0) - errx(1, "open fail"); - - if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) - err(1, "servo arm failed"); - -} int mk_check_for_i2c_esc_bus(char *device_path, int motors) { int ret; + g_mk = new MK(1, device_path); + if (g_mk == nullptr) { - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - g_mk = new MK(3, device_path); - - if (g_mk == nullptr) { - return -1; - - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - - if (ret > 0) { - return 3; - } - + return -1; + } else if (OK != g_mk) { + delete g_mk; + g_mk = nullptr; + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + if (ret > 0) { + return 3; } -#endif + } - g_mk = new MK(1, device_path); - - if (g_mk == nullptr) { - return -1; - - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - - if (ret > 0) { - return 1; - } + g_mk = new MK(1, device_path); + if (g_mk == nullptr) { + return -1; + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + if (ret > 0) { + return 1; } } @@ -1612,7 +1248,6 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); int mkblctrl_main(int argc, char *argv[]) { - PortMode port_mode = PORT_FULL_PWM; int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; int bus = -1; @@ -1729,7 +1364,7 @@ mkblctrl_main(int argc, char *argv[]) /* parameter set ? */ if (newMode) { /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); } exit(0); From 816229652f1eecf8322603eb918f787bdd77d7e2 Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 20:36:11 +0100 Subject: [PATCH 020/154] i2c1 bug and bus scan fixed --- src/drivers/mkblctrl/mkblctrl.cpp | 62 +++++++++++++++---------------- 1 file changed, 29 insertions(+), 33 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c3c4bf8c1d..f692a0dd0d 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -65,7 +65,6 @@ #include #include -//#include #include #include @@ -99,13 +98,6 @@ class MK : public device::I2C { public: - enum Mode { - MODE_NONE, - MODE_2PWM, - MODE_4PWM, - MODE_6PWM, - }; - enum MappingMode { MAPPING_MK = 0, MAPPING_PX4, @@ -1207,11 +1199,11 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) { int ret; - g_mk = new MK(1, device_path); + // try bus 3 first + warnx("scanning i2c3..."); + g_mk = new MK(3, device_path); - if (g_mk == nullptr) { - return -1; - } else if (OK != g_mk) { + if (g_mk != nullptr && OK != g_mk->init(motors)) { delete g_mk; g_mk = nullptr; } else { @@ -1223,10 +1215,13 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) } } + // fallback to bus 1 + warnx("scanning i2c1..."); g_mk = new MK(1, device_path); - if (g_mk == nullptr) { - return -1; + if (g_mk != nullptr && OK != g_mk->init(motors)) { + delete g_mk; + g_mk = nullptr; } else { ret = g_mk->mk_check_for_blctrl(8, false, true); delete g_mk; @@ -1240,7 +1235,6 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) } - } // namespace extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); @@ -1348,31 +1342,33 @@ mkblctrl_main(int argc, char *argv[]) if (!motortest) { - if (g_mk == nullptr) { - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - } + if (g_mk == nullptr) { + if (bus == -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - if (bus != -1) { + + } + + if (bus != -1) { if (mk_start(bus, motorcount, devicepath) != OK) { errx(1, "failed to start the MK-BLCtrl driver"); } - } else { - errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); - } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } - exit(0); - } else { - errx(1, "MK-BLCtrl driver already running"); - } + exit(0); + } else { + errx(1, "MK-BLCtrl driver already running"); + } - } else { + } else { if (g_mk == nullptr) { errx(1, "MK-BLCtrl driver not running. You have to start it first."); From 1ef7320e0c9fe00fdc13b1078d6350240a337179 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 4 Feb 2014 16:50:22 +0100 Subject: [PATCH 021/154] startup rewrite --- src/drivers/mkblctrl/mkblctrl.cpp | 108 ++++++++---------------------- 1 file changed, 27 insertions(+), 81 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index f692a0dd0d..d1c817cf31 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1169,69 +1169,39 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr } int -mk_start(unsigned bus, unsigned motors, char *device_path) -{ - int ret = OK; - - if (g_mk == nullptr) { - - g_mk = new MK(bus, device_path); - - if (g_mk == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_mk->init(motors); - - if (ret != OK) { - delete g_mk; - g_mk = nullptr; - } - } - } - - return ret; -} - - -int -mk_check_for_i2c_esc_bus(char *device_path, int motors) +mk_start(unsigned motors, char *device_path) { int ret; - // try bus 3 first - warnx("scanning i2c3..."); - g_mk = new MK(3, device_path); + // try i2c3 first + g_mk = new MK(3, device_path); - if (g_mk != nullptr && OK != g_mk->init(motors)) { - delete g_mk; - g_mk = nullptr; - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - if (ret > 0) { - return 3; - } - } + if (g_mk && OK == g_mk->init(motors)) { + fprintf(stderr, "[mkblctrl] scanning i2c3...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + if (ret > 0) { + return OK; + } + } + + delete g_mk; + g_mk = nullptr; // fallback to bus 1 - warnx("scanning i2c1..."); g_mk = new MK(1, device_path); - if (g_mk != nullptr && OK != g_mk->init(motors)) { - delete g_mk; - g_mk = nullptr; - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - if (ret > 0) { - return 1; - } - } + if (g_mk && OK == g_mk->init(motors)) { + fprintf(stderr, "[mkblctrl] scanning i2c1...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + if (ret > 0) { + return OK; + } + } - return -1; + delete g_mk; + g_mk = nullptr; + + return -ENOMEM; } @@ -1244,7 +1214,6 @@ mkblctrl_main(int argc, char *argv[]) { int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; @@ -1258,18 +1227,6 @@ mkblctrl_main(int argc, char *argv[]) */ for (int i = 1; i < argc; i++) { - /* look for the optional i2c bus parameter */ - if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { - if (argc > i + 1) { - bus = atoi(argv[i + 1]); - newMode = true; - - } else { - errx(1, "missing argument for i2c bus (-b)"); - return 1; - } - } - /* look for the optional frame parameter */ if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { if (argc > i + 1) { @@ -1329,7 +1286,6 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, "mkblctrl: help:\n"); fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); - fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); fprintf(stderr, "\n"); @@ -1343,19 +1299,9 @@ mkblctrl_main(int argc, char *argv[]) if (!motortest) { if (g_mk == nullptr) { - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - - - } - - if (bus != -1) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - } - } else { - errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); - } + if (mk_start(motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } /* parameter set ? */ if (newMode) { From 94b162d0e076a872af9d1b1538d7f688d51bfef0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 09:34:21 +0100 Subject: [PATCH 022/154] Fixed up nullptr handling --- src/drivers/mkblctrl/mkblctrl.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index d1c817cf31..46f7905fff 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1176,8 +1176,11 @@ mk_start(unsigned motors, char *device_path) // try i2c3 first g_mk = new MK(3, device_path); - if (g_mk && OK == g_mk->init(motors)) { - fprintf(stderr, "[mkblctrl] scanning i2c3...\n"); + if (!g_mk) + return -ENOMEM; + + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c3...\n"); ret = g_mk->mk_check_for_blctrl(8, false, true); if (ret > 0) { return OK; @@ -1190,8 +1193,11 @@ mk_start(unsigned motors, char *device_path) // fallback to bus 1 g_mk = new MK(1, device_path); - if (g_mk && OK == g_mk->init(motors)) { - fprintf(stderr, "[mkblctrl] scanning i2c1...\n"); + if (!g_mk) + return -ENOMEM; + + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c1...\n"); ret = g_mk->mk_check_for_blctrl(8, false, true); if (ret > 0) { return OK; @@ -1201,7 +1207,7 @@ mk_start(unsigned motors, char *device_path) delete g_mk; g_mk = nullptr; - return -ENOMEM; + return -ENXIO; } From 399d59483ede8a6c7c66c3d56f3025e1650d665e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 09:36:22 +0100 Subject: [PATCH 023/154] Fixed code style --- src/drivers/mkblctrl/mkblctrl.cpp | 107 ++++++++++++++++-------------- 1 file changed, 57 insertions(+), 50 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 46f7905fff..ec5f77d747 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -291,23 +291,23 @@ MK::init(unsigned motors) usleep(500000); - if (sizeof(_device) > 0) { - ret = register_driver(_device, &fops, 0666, (void *)this); + if (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { + if (ret == OK) { log("creating alternate output device"); _primary_pwm_device = true; } - } + } /* start the IO interface task */ _task = task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - (main_t)&MK::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + (main_t)&MK::task_main_trampoline, + nullptr); if (_task < 0) { @@ -556,7 +556,7 @@ MK::task_main() } } - if(!_overrideSecurityChecks) { + if (!_overrideSecurityChecks) { /* don't go under BLCTRL_MIN_VALUE */ if (outputs.output[i] < BLCTRL_MIN_VALUE) { @@ -612,21 +612,24 @@ MK::task_main() esc.esc[i].esc_current = (uint16_t) Motor[i].Current; esc.esc[i].esc_rpm = (uint16_t) 0; esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; + if (Motor[i].Version == 1) { // BLCtrl 2.0 (11Bit) - esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits; + esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits; + } else { // BLCtrl < 2.0 (8Bit) esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } + esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; - // if motortest is requested - do it... - if (_motortest == true) { - mk_servo_test(i); - } + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } } @@ -665,7 +668,7 @@ MK::mk_servo_arm(bool status) unsigned int MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { - if(initI2C) { + if (initI2C) { I2C::init(); } @@ -718,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } - - if(!_overrideSecurityChecks) { + + if (!_overrideSecurityChecks) { if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { _task_should_exit = true; } @@ -748,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val) tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff; - Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07; + Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff; + Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07; if (_armed == false) { Motor[chan].SetPoint = 0; @@ -1003,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (arg < 2150) { Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047)); + } else { ret = -EINVAL; } @@ -1173,19 +1177,20 @@ mk_start(unsigned motors, char *device_path) { int ret; - // try i2c3 first - g_mk = new MK(3, device_path); + // try i2c3 first + g_mk = new MK(3, device_path); - if (!g_mk) - return -ENOMEM; + if (!g_mk) + return -ENOMEM; - if (OK == g_mk->init(motors)) { - warnx("[mkblctrl] scanning i2c3...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, true); - if (ret > 0) { - return OK; - } - } + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c3...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + + if (ret > 0) { + return OK; + } + } delete g_mk; g_mk = nullptr; @@ -1194,15 +1199,16 @@ mk_start(unsigned motors, char *device_path) g_mk = new MK(1, device_path); if (!g_mk) - return -ENOMEM; + return -ENOMEM; - if (OK == g_mk->init(motors)) { - warnx("[mkblctrl] scanning i2c1...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, true); - if (ret > 0) { - return OK; - } - } + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c1...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + + if (ret > 0) { + return OK; + } + } delete g_mk; g_mk = nullptr; @@ -1298,16 +1304,16 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, "Motortest:\n"); fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); fprintf(stderr, "mkblctrl -t\n"); - fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } if (!motortest) { if (g_mk == nullptr) { - if (mk_start(motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - } + if (mk_start(motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } /* parameter set ? */ if (newMode) { @@ -1316,18 +1322,19 @@ mkblctrl_main(int argc, char *argv[]) } exit(0); + } else { errx(1, "MK-BLCtrl driver already running"); } } else { - if (g_mk == nullptr) { - errx(1, "MK-BLCtrl driver not running. You have to start it first."); + if (g_mk == nullptr) { + errx(1, "MK-BLCtrl driver not running. You have to start it first."); - } else { - g_mk->set_motor_test(motortest); - exit(0); + } else { + g_mk->set_motor_test(motortest); + exit(0); - } - } + } + } } From 080b2da214e014933e9062aca9a0be5bd1a2340a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 14:23:48 +0100 Subject: [PATCH 024/154] Updated uploader from Bootloader repo --- Tools/px_uploader.py | 48 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce5e5e54a..23dc484505 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -50,6 +50,9 @@ # Currently only used for informational purposes. # +# for python2.7 compatibility +from __future__ import print_function + import sys import argparse import binascii @@ -154,6 +157,8 @@ class uploader(object): PROG_MULTI = b'\x27' READ_MULTI = b'\x28' # rev2 only GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -175,6 +180,8 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' def close(self): if self.port is not None: @@ -237,6 +244,22 @@ class uploader(object): self.__getSync() return value + # send the GET_OTP command and wait for an info parameter + def __getOTP(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_OTP + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + + # send the GET_OTP command and wait for an info parameter + def __getSN(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_SN + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self): self.__send(uploader.CHIP_ERASE @@ -353,6 +376,31 @@ class uploader(object): if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('utf-8') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + print("type: " + self.otp_id.decode('utf-8')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('utf-8')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('utf-8')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('utf-8')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('utf-8')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('utf-8'), end='') # show user + print('') print("erase...") self.__erase() From 29b0678d841666000fd67cc955dba5cf3cc980c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 11:56:01 +0100 Subject: [PATCH 025/154] navigator: forbid READY - > RTL transition --- src/modules/navigator/navigator_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..abd7119f78 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -689,7 +689,7 @@ Navigator::task_main() if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -748,7 +748,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -756,9 +756,7 @@ Navigator::task_main() break; case NAV_STATE_LAND: - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); - } + dispatch(EVENT_LAND_REQUESTED); break; From 1d40582cc0301f0af330fe69ee63aa8e3d301214 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 12:42:20 +0100 Subject: [PATCH 026/154] navigator: forbid READY - > RTL transition fix --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index abd7119f78..6f0fec9179 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -965,7 +965,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, From 70e1bfa4d69e967b309d177060804e7567f148b2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 22:28:42 +0100 Subject: [PATCH 027/154] Startup scripts: use rc.mc_defaults for default MC parameters --- .../px4fmu_common/init.d/10015_tbs_discovery | 41 +--------------- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 38 +-------------- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 4 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 4 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 4 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 44 +---------------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 47 +------------------ ROMFS/px4fmu_common/init.d/4011_dji_f450 | 47 +------------------ ROMFS/px4fmu_common/init.d/4012_hk_x550 | 6 --- ROMFS/px4fmu_common/init.d/5001_quad_+ | 4 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 6 ++- ROMFS/px4fmu_common/init.d/8001_octo_x | 4 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 4 +- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 45 ++++++++++++++++++ 15 files changed, 79 insertions(+), 223 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index a3d7c3d977..b09765e8e7 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -5,45 +5,8 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 5.0 - param set MC_ROLLRATE_P 0.17 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.006 - param set MC_PITCH_P 5.0 - param set MC_PITCHRATE_P 0.17 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.006 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0.0 +sh /etc/init.d/rc.mc_defaults - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi - -set VEHICLE_TYPE mc set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 -set PWM_RATE 400 +set PWM_OUTPUTS 1234 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 1c7ecb7129..42d5169935 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -5,51 +5,17 @@ # Anton Babushkin # +sh /etc/init.d/rc.mc_defaults + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ROLL_P 9.0 - param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 9.0 - param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0.0 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 - param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 fi -set VEHICLE_TYPE mc set MIXER FMU_quad_w set PWM_OUTPUTS 1234 -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1000 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 45880f44be..7a7a9542c0 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults + +set MIXER FMU_quad_x set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 2b26810e71..c47500c7ac 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/1001_rc_quad_x.hil +sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ + +set HIL yes \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 8a813595e4..c5d3e78078 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -5,6 +5,8 @@ # Lorenz Meier , Anton Babushkin # -sh /etc/init.d/8001_octo_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_cox + +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 0f1288dec3..fa751f1e31 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -5,48 +5,8 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi +sh /etc/init.d/rc.mc_defaults -set VEHICLE_TYPE mc set MIXER FMU_quad_x -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1100 -set PWM_MAX 2000 +set PWM_OUTPUTS 1234 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7829460339..232af57b4a 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -5,49 +5,6 @@ # Anton Babushkin # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.05 - param set MC_YAWRATE_D 0.0 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi +sh /etc/init.d/4001_quad_x -set VEHICLE_TYPE mc -set MIXER FMU_quad_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 +set PWM_MIN 1175 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 8eb53d1720..259636acc2 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -5,49 +5,6 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi +sh /etc/init.d/4001_quad_x -set VEHICLE_TYPE mc -set MIXER FMU_quad_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 +set PWM_MIN 1175 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 3d8e9fcf70..a5c4a86902 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -6,9 +6,3 @@ # sh /etc/init.d/4001_quad_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1000 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index ff11bccfe6..2f5ab44d77 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index fd57565860..73ef12569b 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -5,10 +5,10 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x # We only can run one channel group with one rate, # so all 8 at 400 Hz -set PWM_OUTPUTS 12345678 +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 45279ec390..ef4b6297de 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -5,6 +5,10 @@ # Anton Babushkin # -sh /etc/init.d/6001_hexa_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ + +# We only can run one channel group with one rate, +# so all 8 at 400 Hz +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 6fa9626684..bb87f89fe7 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -5,8 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_x -set PWM_OUTPUTS 12345678 +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index fa3869f9f9..81132fd3e7 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/8001_octo_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_+ + +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults new file mode 100644 index 0000000000..14b6fe12cb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -0,0 +1,45 @@ +#!nsh + +set VEHICLE_TYPE mc + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 +fi + +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1075 +set PWM_MAX 2000 \ No newline at end of file From fff00318cdeac8b52affb2210dd0975c65826333 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 22:39:06 +0100 Subject: [PATCH 028/154] Startup scripts: get the indentation right --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 68 +++++++++++------------ 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 14b6fe12cb..13b2c97174 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -1,42 +1,42 @@ #!nsh -set VEHICLE_TYPE mc +set VEHICLE_TYPE fw if [ $DO_AUTOCONFIG == yes ] then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 + # + # Default parameters for this platform + # + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 fi set PWM_RATE 400 From df41c04be74c6d4d576c188f2327226c264d371c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 23:13:12 +0100 Subject: [PATCH 029/154] Startup scripts: use rc.fw_defaults for default FW parameters --- .../init.d/1000_rc_fw_easystar.hil | 30 +---------- .../init.d/1004_rc_fw_Rascal110.hil | 32 +---------- .../init.d/1005_rc_fw_Malolo1.hil | 53 +------------------ ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 33 +----------- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 32 +---------- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 30 +---------- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 35 +----------- ROMFS/px4fmu_common/init.d/3031_phantom | 36 ------------- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 35 +----------- ROMFS/px4fmu_common/init.d/3033_wingwing | 35 +----------- ROMFS/px4fmu_common/init.d/3034_fx79 | 35 +----------- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 35 ++++++++++++ 12 files changed, 47 insertions(+), 374 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index d7e524b414..36194ad682 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -5,36 +5,10 @@ # Lorenz Meier # -echo "HIL Rascal 110 starting.." +sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +echo "HIL Rascal 110 starting.." set HIL yes -set VEHICLE_TYPE fw set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index c639bfc254..4e3e183262 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -5,38 +5,10 @@ # Thomas Gubler # -echo "HIL Rascal 110 starting.." +sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +echo "HIL Rascal 110 starting.." set HIL yes -set VEHICLE_TYPE fw set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 124bf63abf..abbe626b13 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -5,60 +5,9 @@ # Thomas Gubler # -echo "HIL Malolo 1 starting.." - -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_AIRSPD_MIN 12 - param set FW_AIRSPD_TRIM 25 - param set FW_ATT_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 - param set FW_PR_I 0.05 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.1 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 - param set FW_RR_I 0.02 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.1 - param set FW_R_LIM 45 - param set FW_R_RMAX 0 - param set FW_THR_CRUISE 0.6 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_CLMB_MAX 5 - param set FW_T_HRATE_P 0.02 - param set FW_T_PTCH_DAMP 0 - param set FW_T_RLL2THR 15 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 3 - param set FW_T_VERT_ACC 7 - param set FW_YCO_VMIN 1000 - param set FW_YR_FF 0.0 - param set FW_YR_I 0 - param set FW_YR_IMAX 0.2 - param set FW_YR_P 0.0 - param set FW_Y_RMAX 0 - param set NAV_LAND_ALT 90 - param set NAV_RTL_ALT 100 - param set NAV_RTL_LAND_T -1 - - param set SYS_AUTOCONFIG 0 - param save -fi +sh /etc/init.d/rc.fw_defaults set HIL yes -set VEHICLE_TYPE fw # Set the AERT mixer for HIL (even if the malolo is a flying wing) set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 0e5bf60d60..465a22c53a 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -2,38 +2,7 @@ # # MPX EasyStar Plane # -# Maintainers: ??? -# -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index de5e5a8d3a..dcc5db824a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,35 +1,5 @@ #!nsh -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index 07f215f6cf..dcc5db824a 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -1,33 +1,5 @@ #!nsh -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 3b7323ac4e..83c470234c 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -1,38 +1,5 @@ #!nsh -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1.0 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a4ff61d937..2e2434bb85 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -5,40 +5,4 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 -fi - -set VEHICLE_TYPE fw set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index c1e78b6f1e..1657e6d368 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -5,39 +5,6 @@ # Thomas Gubler , Julian Oes # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 2f68797999..2af3618d98 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -5,39 +5,6 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index bc02d87f3d..f4bd18269d 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -5,39 +5,6 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults new file mode 100644 index 0000000000..0de13af070 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -0,0 +1,35 @@ +#!nsh + +set VEHICLE_TYPE fw + +if [ $DO_AUTOCONFIG == yes ] +then +# +# Default parameters for this platform +# + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 80 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.05 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.05 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi \ No newline at end of file From ac653416f8f5163a4f162ab431d69129c9e1858e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 7 Feb 2014 22:13:20 +0100 Subject: [PATCH 030/154] X5: copy content of FMU_Q.mix to FMU_X5.mix because FMU_Q was used previously by the X5 startup script --- ROMFS/px4fmu_common/mixers/FMU_X5.mix | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix index 6108683549..80e3bac09c 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -23,13 +23,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -3000 -5000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 6000 6000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -3000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 -6000 -6000 0 -10000 10000 Output 2 -------- @@ -48,7 +48,7 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels ----------------------------------------------------- M: 1 @@ -66,4 +66,3 @@ 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 008a973ef7de6433d3215df5cc8f553ce494ba22 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 23:51:57 +0100 Subject: [PATCH 031/154] Startup scripts: get the indentation right, take 2 --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 50 +++++++++++------------ 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 0de13af070..cbf0ba4000 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -7,29 +7,29 @@ then # # Default parameters for this platform # - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 80 - param set FW_ATT_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.05 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.05 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 80 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.05 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.05 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 fi \ No newline at end of file From 096735897c70076ee26c1d343538eae8a585f285 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 23:52:56 +0100 Subject: [PATCH 032/154] Startup scripts: added important LAND and RTL parameters for FW --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index cbf0ba4000..a28f67b656 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -32,4 +32,8 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 + + param set NAV_LAND_ALT 90 + param set NAV_RTL_ALT 100 + param set NAV_RTL_LAND_T -1 fi \ No newline at end of file From de8a9268f3c476c2695df84f0e1a429ea10ddeb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 8 Feb 2014 00:31:53 +0100 Subject: [PATCH 033/154] Startup scripts: bring back HIL parameters for Malolo --- .../init.d/1005_rc_fw_Malolo1.hil | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index abbe626b13..c753ded233 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -7,6 +7,40 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 25 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0.05 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.1 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.02 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.1 + param set FW_R_LIM 45 + param set FW_R_RMAX 0 + param set FW_T_CLMB_MAX 5 + param set FW_T_HRATE_P 0.02 + param set FW_T_PTCH_DAMP 0 + param set FW_T_RLL2THR 15 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 3 + param set FW_T_VERT_ACC 7 + param set FW_YR_FF 0.0 + param set FW_YR_I 0 + param set FW_YR_IMAX 0.2 + param set FW_YR_P 0.0 +fi + set HIL yes # Set the AERT mixer for HIL (even if the malolo is a flying wing) From 0a87f1d01ce089e6f6ddba42be6847d5783cbfaf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 8 Feb 2014 00:32:57 +0100 Subject: [PATCH 034/154] Startup scripts: move X5 attitude parameters back to X5 script and only leave airframe independent params in FW defaults script --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 29 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.fw_defaults | 28 +------------------ ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 1657e6d368..3f5f79857d 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,4 +7,33 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.05 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.05 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index a28f67b656..3e340699fc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -5,34 +5,8 @@ set VEHICLE_TYPE fw if [ $DO_AUTOCONFIG == yes ] then # -# Default parameters for this platform +# Default parameters for FW # - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 80 - param set FW_ATT_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.05 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.05 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 - param set NAV_LAND_ALT 90 param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 13b2c97174..50d02d9e47 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -5,7 +5,7 @@ set VEHICLE_TYPE fw if [ $DO_AUTOCONFIG == yes ] then # - # Default parameters for this platform + # Default parameters for MC # param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.12 From 70964dd87c72b2b9bd475645654e894ba827ac64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 18:34:54 +0100 Subject: [PATCH 035/154] Update upload script from bootloader repo --- Tools/px_uploader.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 23dc484505..e4a8b3c054 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -381,7 +381,7 @@ class uploader(object): for byte in range(0,32*6,4): x = self.__getOTP(byte) self.otp = self.otp + x - print(binascii.hexlify(x).decode('utf-8') + ' ', end='') + print(binascii.hexlify(x).decode('Latin-1') + ' ', end='') # see src/modules/systemlib/otp.h in px4 code: self.otp_id = self.otp[0:4] self.otp_idtype = self.otp[4:5] @@ -389,17 +389,17 @@ class uploader(object): self.otp_pid = self.otp[12:8:-1] self.otp_coa = self.otp[32:160] # show user: - print("type: " + self.otp_id.decode('utf-8')) - print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('utf-8')) - print("vid: " + binascii.hexlify(self.otp_vid).decode('utf-8')) - print("pid: "+ binascii.hexlify(self.otp_pid).decode('utf-8')) - print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('utf-8')) + print("type: " + self.otp_id.decode('Latin-1')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) print("sn: ", end='') for byte in range(0,12,4): x = self.__getSN(byte) x = x[::-1] # reverse the bytes self.sn = self.sn + x - print(binascii.hexlify(x).decode('utf-8'), end='') # show user + print(binascii.hexlify(x).decode('Latin-1'), end='') # show user print('') print("erase...") self.__erase() From a9e5e2e31a73e4ca546bf89e807b71187a41b657 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Feb 2014 08:54:48 +0100 Subject: [PATCH 036/154] position_estimator_inav: default parameters and min/max EPH/EPV updated --- .../position_estimator_inav_main.c | 8 ++++---- .../position_estimator_inav_params.c | 2 +- 2 files changed, 5 insertions(+), 5 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 e045ce4cca..bf4f7ae974 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -527,13 +527,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { gps_valid = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } @@ -589,8 +589,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); - w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); + w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); + w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); } } else { 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 e1bbd75a6e..b71f9472f5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); From b0c60296f58dfec02f115d0033f61e95389f5931 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 10 Feb 2014 12:08:39 +1100 Subject: [PATCH 037/154] FMUv2: fixed UART3 flow control pins --- nuttx-configs/px4fmu-v2/include/board.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index e56b14ba41..850043ddf5 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -209,8 +209,8 @@ #define GPIO_USART3_RX GPIO_USART3_RX_3 #define GPIO_USART3_TX GPIO_USART3_TX_3 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 +#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 From 0d4b5d9395e3fd668b217301d0f888fa0238998d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:15:34 +0100 Subject: [PATCH 038/154] X5: adjusted default parameters based on test flight --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 3f5f79857d..bf5a87068f 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -15,19 +15,19 @@ then param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 + param set FW_PR_FF 0.3 param set FW_PR_I 0 param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.05 + param set FW_PR_P 0.03 param set FW_P_LIM_MAX 45 param set FW_P_LIM_MIN -45 param set FW_P_RMAX_NEG 0 param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 + param set FW_RR_FF 0.3 param set FW_RR_I 0 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.05 + param set FW_RR_P 0.03 param set FW_R_LIM 60 param set FW_R_RMAX 0 param set FW_T_HRATE_P 0.01 From 75d08826389680e55543eb017683c8cf9434bf7c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:16:02 +0100 Subject: [PATCH 039/154] fw_pos_control: added default for autoland parameters --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 ee8721ff92..a0447d90d2 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 @@ -168,9 +168,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 44bde0db912c78f5a9d7d71f974dd2f36873dd24 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:16:27 +0100 Subject: [PATCH 040/154] Navgitor: adjusted default loiter radius --- src/modules/navigator/navigator_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index d5e00e35db..1ba159a8e6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,7 +55,7 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing From d70d84c9a7be7f629542f40255396f6755239963 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 10 Feb 2014 13:35:11 +0100 Subject: [PATCH 041/154] Fixed wrong VEHICLE_TYPE for multicopters. --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 50d02d9e47..52584677b6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -1,6 +1,6 @@ #!nsh -set VEHICLE_TYPE fw +set VEHICLE_TYPE mc if [ $DO_AUTOCONFIG == yes ] then From aea135a9ced3f1c7e1c4e3be0e5f0e310f96c82d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:41:40 +0100 Subject: [PATCH 042/154] fw_pos_control: flare altitude back to 15m --- 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 a0447d90d2..512ca7b8ac 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 @@ -171,6 +171,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 36d1ec80ef264beb34604b3e2b9bb076fd78d52f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 15:12:32 +0100 Subject: [PATCH 043/154] Startup: don't configure anything if definitions are missing --- ROMFS/px4fmu_common/init.d/rcS | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 178bcaeba5..ede835ab71 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -464,16 +464,20 @@ then if [ $MIXER == none ] then - # Set default mixer for multicopter if not defined - set MIXER quad_x + echo "Default mixer for multicopter not defined" fi if [ $MAV_TYPE == none ] then - # Use MAV_TYPE = 2 (quadcopter) if not defined - set MAV_TYPE 2 - # Use mixer to detect vehicle type + if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ] + then + set MAV_TYPE 2 + fi + if [ $MIXER == FMU_quad_w ] + then + set MAV_TYPE 2 + fi if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then set MAV_TYPE 13 @@ -487,8 +491,14 @@ then set MAV_TYPE 14 fi fi - - param set MAV_TYPE $MAV_TYPE + + # Still no MAV_TYPE found + if [ $MAV_TYPE == none ] + then + echo "Unknown MAV_TYPE" + else + param set MAV_TYPE $MAV_TYPE + fi # Load mixer and configure outputs sh /etc/init.d/rc.interface @@ -502,10 +512,8 @@ then # if [ $VEHICLE_TYPE == none ] then - echo "[init] Vehicle type: GENERIC" + echo "[init] Vehicle type: No autostart ID found" - # Load mixer and configure outputs - sh /etc/init.d/rc.interface fi # Start any custom addons From 0388d9adefb33c98f1e4350e3f2ed59a7fdd5359 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Feb 2014 08:09:51 +0100 Subject: [PATCH 044/154] Hotfix and cleanup for system mixers --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 39 +++++ .../{12001_octo_cox_pwm => 12001_octo_cox} | 4 +- ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 2 +- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 2 +- ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 11 +- ROMFS/px4fmu_common/init.d/rcS | 12 +- ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 17 +- ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 17 +- ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix | 6 +- ROMFS/px4fmu_common/mixers/FMU_octo_x.mix | 6 +- ROMFS/px4fmu_common/mixers/README | 154 ------------------ ROMFS/px4fmu_common/mixers/hexa_cox.mix | 3 + 14 files changed, 70 insertions(+), 207 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/11001_hexa_cox rename ROMFS/px4fmu_common/init.d/{12001_octo_cox_pwm => 12001_octo_cox} (90%) delete mode 100644 ROMFS/px4fmu_common/mixers/README create mode 100644 ROMFS/px4fmu_common/mixers/hexa_cox.mix diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox new file mode 100644 index 0000000000..2dc83a517c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -0,0 +1,39 @@ +#!nsh +# +# UNTESTED UNTESTED! +# +# Generic 10†Hexa coaxial geometry +# +# Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER hexa_cox + +set PWM_OUTPUTS 12345678 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox similarity index 90% rename from ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm rename to ROMFS/px4fmu_common/init.d/12001_octo_cox index 5f3cec4e0a..a55fc5a30d 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10†Octo coaxial geometry # -# Maintainers: Lorenz Meier +# Lorenz Meier # if [ $DO_AUTOCONFIG == yes ] @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_octo_cox -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm index ddec8f36ef..4477967091 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_hexa_x -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm index 106e0fb54c..c4e9560d17 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_hexa_+ -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm index f0eea339b8..ea56195d4b 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_octo_x -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm index 992a7aeba6..f7693875c5 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_octo_+ -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 00baed6468..38b1cb57e6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -185,11 +185,20 @@ then sh /etc/init.d/10016_3dr_iris fi +# +# Hexa Coaxial +# + +if param compare SYS_AUTOSTART 11001 +then + sh /etc/init.d/11001_hexa_cox +fi + # # Octo Coaxial # if param compare SYS_AUTOSTART 12001 then - sh /etc/init.d/12001_octo_cox_pwm + sh /etc/init.d/12001_octo_cox fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6f4e1f3b56..50ac9759a5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -467,19 +467,23 @@ then # Use mixer to detect vehicle type if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] then - param set MAV_TYPE 13 + set MAV_TYPE 13 + fi + if [ $MIXER == hexa_cox ] + then + set MAV_TYPE 13 fi if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi if [ $MIXER == FMU_octo_cox ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi fi - param set MAV_TYPE $MAV_TYPE + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs sh /etc/init.d/rc.interface diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index f8f9f0e4dc..e608b459f8 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -1,18 +1,3 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the + configuration. All controls -are mixed 100%. +# Hexa + R: 6+ 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix index 26b40b9e95..16e6e22f90 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -1,18 +1,3 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the X configuration. All controls -are mixed 100%. +# Hexa X R: 6x 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix index 51cebb7858..f7845450d3 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix @@ -1,7 +1,3 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls -are mixed 100%. +# Octo coaxial R: 8c 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix index edc71f0139..c9a348aa42 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix @@ -1,7 +1,3 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a octocopter in the X configuration. All controls -are mixed 100%. +# Octo X R: 8x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README deleted file mode 100644 index 6649c53c20..0000000000 --- a/ROMFS/px4fmu_common/mixers/README +++ /dev/null @@ -1,154 +0,0 @@ -PX4 mixer definitions -===================== - -Files in this directory implement example mixers that can be used as a basis -for customisation, or for general testing purposes. - -Mixer basics ------------- - -Mixers combine control values from various sources (control tasks, user inputs, -etc.) and produce output values suitable for controlling actuators; servos, -motors, switches and so on. - -An actuator derives its value from the combination of one or more control -values. Each of the control values is scaled according to the actuator's -configuration and then combined to produce the actuator value, which may then be -further scaled to suit the specific output type. - -Internally, all scaling is performed using floating point values. Inputs and -outputs are clamped to the range -1.0 to 1.0. - -control control control - | | | - v v v - scale scale scale - | | | - | v | - +-------> mix <------+ - | - scale - | - v - out - -Scaling -------- - -Basic scalers provide linear scaling of the input to the output. - -Each scaler allows the input value to be scaled independently for inputs -greater/less than zero. An offset can be applied to the output, and lower and -upper boundary constraints can be applied. Negative scaling factors cause the -output to be inverted (negative input produces positive output). - -Scaler pseudocode: - -if (input < 0) - output = (input * NEGATIVE_SCALE) + OFFSET -else - output = (input * POSITIVE_SCALE) + OFFSET - -if (output < LOWER_LIMIT) - output = LOWER_LIMIT -if (output > UPPER_LIMIT) - output = UPPER_LIMIT - -Syntax ------- - -Mixer definitions are text files; lines beginning with a single capital letter -followed by a colon are significant. All other lines are ignored, meaning that -explanatory text can be freely mixed with the definitions. - -Each file may define more than one mixer; the allocation of mixers to actuators -is specific to the device reading the mixer definition, and the number of -actuator outputs generated by a mixer is specific to the mixer. - -A mixer begins with a line of the form - - : - -The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a -multirotor mixer, etc. - -Null Mixer -.......... - -A null mixer consumes no controls and generates a single actuator output whose -value is always zero. Typically a null mixer is used as a placeholder in a -collection of mixers in order to achieve a specific pattern of actuator outputs. - -The null mixer definition has the form: - - Z: - -Simple Mixer -............ - -A simple mixer combines zero or more control inputs into a single actuator -output. Inputs are scaled, and the mixing function sums the result before -applying an output scaler. - -A simple mixer definition begins with: - - M: - O: <-ve scale> <+ve scale> - -If is zero, the sum is effectively zero and the mixer will -output a fixed value that is constrained by and . - -The second line defines the output scaler with scaler parameters as discussed -above. Whilst the calculations are performed as floating-point operations, the -values stored in the definition file are scaled by a factor of 10000; i.e. an -offset of -0.5 is encoded as -5000. - -The definition continues with entries describing the control -inputs and their scaling, in the form: - - S: <-ve scale> <+ve scale> - -The value identifies the control group from which the scaler will read, -and the value an offset within that group. These values are specific to -the device reading the mixer definition. - -When used to mix vehicle controls, mixer group zero is the vehicle attitude -control group, and index values zero through three are normally roll, pitch, -yaw and thrust respectively. - -The remaining fields on the line configure the control scaler with parameters as -discussed above. Whilst the calculations are performed as floating-point -operations, the values stored in the definition file are scaled by a factor of -10000; i.e. an offset of -0.5 is encoded as -5000. - -Multirotor Mixer -................ - -The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) -into a set of actuator outputs intended to drive motor speed controllers. - -The mixer definition is a single line of the form: - -R: - -The supported geometries include: - - 4x - quadrotor in X configuration - 4+ - quadrotor in + configuration - 6x - hexcopter in X configuration - 6+ - hexcopter in + configuration - 8x - octocopter in X configuration - 8+ - octocopter in + configuration - -Each of the roll, pitch and yaw scale values determine scaling of the roll, -pitch and yaw controls relative to the thrust control. Whilst the calculations -are performed as floating-point operations, the values stored in the definition -file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. - -Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the -thrust input ranges from 0.0 to 1.0. Output for each actuator is in the -range -1.0 to 1.0. - -In the case where an actuator saturates, all actuator values are rescaled so that -the saturating actuator is limited to 1.0. diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/hexa_cox.mix new file mode 100644 index 0000000000..497786feb5 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/hexa_cox.mix @@ -0,0 +1,3 @@ +# Hexa coaxial + +R: 6c 10000 10000 10000 0 From 3c592e3a06aa6a0357b9540d8cc1f68333b1c256 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 15:35:26 +0100 Subject: [PATCH 045/154] Startup: fix merge mistake --- ROMFS/px4fmu_common/init.d/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 588a44a013..76f021e339 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -479,6 +479,7 @@ then set MAV_TYPE 2 fi if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] + then set MAV_TYPE 13 fi if [ $MIXER == hexa_cox ] From 6631ecf04a0592c816dfb832fa928a95877184d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 11:45:35 +0100 Subject: [PATCH 046/154] commander: reset blink_msg_end when blink message completed to set normal LED status immediately --- src/modules/commander/commander_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88e..04624275f9 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -161,6 +161,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { From 855944fb2eab33074537b5ccfc82e2462b662b5b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 00:24:40 +0100 Subject: [PATCH 047/154] commander: beeps and blinks cleanup --- .../commander/accelerometer_calibration.cpp | 2 +- .../commander/airspeed_calibration.cpp | 2 +- src/modules/commander/commander.cpp | 46 ++++------ src/modules/commander/commander_helper.cpp | 86 +++++++++++-------- src/modules/commander/commander_helper.h | 14 +-- 5 files changed, 76 insertions(+), 74 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 36b75dd58a..1cbdf9bf85 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (double)accel_ref[orient][2]); data_collected[orient] = true; - tune_neutral(); + tune_neutral(true); } close(sensor_combined_sub); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 1809f96888..6039d92a76 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd) } mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(); + tune_neutral(true); close(diff_pres_sub); return OK; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b85737..193a7473eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1101,7 +1101,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; - tune_positive(); + tune_positive(true); } } @@ -1196,8 +1196,9 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine according to mode switches */ res = set_main_state_rc(&status); + /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { - tune_positive(); + tune_positive(armed.armed); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1253,7 +1254,7 @@ int commander_thread_main(int argc, char *argv[]) /* flight termination in manual mode if assisted switch is on easy position */ if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(); + tune_positive(armed.armed); } } @@ -1308,21 +1309,18 @@ int commander_thread_main(int argc, char *argv[]) /* play arming and battery warning tunes */ if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ - if (tune_arm() == OK) - arm_tune_played = true; + set_tune(TONE_ARMING_WARNING_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ - if (tune_low_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ - if (tune_critical_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_FAST_TUNE); } else if (battery_tune_played) { - tune_stop(); + set_tune(TONE_STOP_TUNE); battery_tune_played = false; } @@ -1693,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg) sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - // only buzz if armed, because else we're driving people nuts indoors - // they really need to look at the leds as well. - if (status->arming_state == ARMING_STATE_ARMED) { - tune_negative(); - } else { - - // Always show the led indication - led_negative(); - } + /* only buzz if armed, because else we're driving people nuts indoors + they really need to look at the leds as well. */ + tune_negative(armed.armed); } } @@ -1715,7 +1707,7 @@ print_reject_arm(const char *msg) char s[80]; sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + tune_negative(true); } } @@ -1723,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); - tune_negative(); + tune_negative(true); break; default: @@ -1883,9 +1875,9 @@ void *commander_low_prio_loop(void *arg) } if (calib_ret == OK) - tune_positive(); + tune_positive(true); else - tune_negative(); + tune_negative(true); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 04624275f9..265c0134cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -82,10 +83,21 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) } static int buzzer; -static hrt_abstime blink_msg_end; +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 = 0; // currently playing tune, can be interrupted after tune_end +static int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { + tune_end = 0; + tune_current = 0; + memset(tune_durations, 0, sizeof(tune_durations)); + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { @@ -93,6 +105,8 @@ int buzzer_init() return ERROR; } + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + return OK; } @@ -101,58 +115,60 @@ void buzzer_deinit() close(buzzer); } -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +void set_tune(int tune) { + int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ + 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) { + ioctl(buzzer, TONE_SET_ALARM, tune); + } + tune_current = tune; + if (new_tune_duration > 0) { + tune_end = hrt_absolute_time() + new_tune_duration; + } else { + tune_end = 0; + } + } } -void tune_positive() +/** + * Blink green LED and play positive tune (if use_busser == true). + */ +void tune_positive(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } } -void tune_neutral() +/** + * Blink white LED and play neutral tune (if use_busser == true). + */ +void tune_neutral(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } } -void tune_negative() -{ - led_negative(); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); -} - -void led_negative() +/** + * Blink red LED and play negative tune (if use_busser == true). + */ +void tune_negative(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); -} - -int tune_critical_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); -} - -void tune_stop() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + } } int blink_msg_state() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index af25a5e979..e75f2592f5 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); -int tune_arm(void); -int tune_low_bat(void); -int tune_critical_bat(void); -void tune_stop(void); - -void led_negative(); +void set_tune(int tune); +void tune_positive(bool use_buzzer); +void tune_neutral(bool use_buzzer); +void tune_negative(bool use_buzzer); int blink_msg_state(); From 0613b299c033c21e1ddcbd8faeb9d3430d72386c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 09:13:51 +0100 Subject: [PATCH 048/154] commander: play warning tune (as for low battery) when in failsafe state --- src/modules/commander/commander.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 193a7473eb..20bfca4774 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,14 +1311,14 @@ int commander_thread_main(int argc, char *argv[]) /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* play tune on battery warning */ - set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* play tune on battery warning or failsafe */ + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); + } else if (battery_tune_played) { set_tune(TONE_STOP_TUNE); battery_tune_played = false; @@ -1420,11 +1420,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - rgbled_set_color(RGBLED_COLOR_AMBER); - } - + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { From 0ead560059fff0a31183f40a6b848e82aa2dae80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 18:24:20 +0100 Subject: [PATCH 049/154] commander: tunes cleanup and fixes --- src/modules/commander/commander.cpp | 9 ++---- src/modules/commander/commander_helper.cpp | 32 ++++++++++------------ 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20bfca4774..7284b38c91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -607,7 +607,6 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; - bool battery_tune_played = false; bool arm_tune_played = false; /* set parameters */ @@ -1020,14 +1019,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; if (armed.armed) { arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); @@ -1310,6 +1307,7 @@ int commander_thread_main(int argc, char *argv[]) if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); + arm_tune_played = true; } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ @@ -1319,13 +1317,12 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (battery_tune_played) { + } else { set_tune(TONE_STOP_TUNE); - battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { + if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 265c0134cb..fe6c9bfaa5 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,20 +82,20 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -static int buzzer; +static int buzzer = -1; 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 = 0; // currently playing tune, can be interrupted after tune_end -static int tune_durations[TONE_NUMBER_OF_TUNES]; +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]; int buzzer_init() { tune_end = 0; tune_current = 0; memset(tune_durations, 0, sizeof(tune_durations)); - tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); @@ -105,8 +105,6 @@ int buzzer_init() return ERROR; } - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - return OK; } @@ -116,15 +114,15 @@ void buzzer_deinit() } void set_tune(int tune) { - int new_tune_duration = tune_durations[tune]; + unsigned int new_tune_duration = tune_durations[tune]; /* don't interrupt currently playing non-repeating tune by repeating */ - if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + 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) { + if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; - if (new_tune_duration > 0) { + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; } else { tune_end = 0; @@ -133,7 +131,7 @@ void set_tune(int tune) { } /** - * Blink green LED and play positive tune (if use_busser == true). + * Blink green LED and play positive tune (if use_buzzer == true). */ void tune_positive(bool use_buzzer) { @@ -146,7 +144,7 @@ void tune_positive(bool use_buzzer) } /** - * Blink white LED and play neutral tune (if use_busser == true). + * Blink white LED and play neutral tune (if use_buzzer == true). */ void tune_neutral(bool use_buzzer) { @@ -159,7 +157,7 @@ void tune_neutral(bool use_buzzer) } /** - * Blink red LED and play negative tune (if use_busser == true). + * Blink red LED and play negative tune (if use_buzzer == true). */ void tune_negative(bool use_buzzer) { @@ -185,8 +183,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() { From 16908f9aff0e7ad0f967613adf2be9a00c1c6cce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 23:24:49 +0100 Subject: [PATCH 050/154] autostart for multicopters: frame-specific default parameters reverted and cleaned up --- .../px4fmu_common/init.d/10015_tbs_discovery | 21 ++++++++++++-- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 17 +++++++++-- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 28 ++---------------- ROMFS/px4fmu_common/init.d/12001_octo_cox | 29 ++----------------- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 19 +++++++++++- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 20 ++++++++++++- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 8 ----- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 5 ++-- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 5 ++-- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 ---- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 24 +++++++-------- 14 files changed, 92 insertions(+), 95 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index b09765e8e7..880e4899b1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,11 +2,28 @@ # # Team Blacksheep Discovery Quadcopter # -# Simon Wilks +# Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults +if [ $DO_AUTOCONFIG == yes ] +then + # TODO review MC_YAWRATE_I + param set MC_ROLL_P 8.0 + param set MC_ROLLRATE_P 0.07 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.0017 + param set MC_PITCH_P 8.0 + param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_I 0.1 + param set MC_PITCHRATE_D 0.0025 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.28 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 \ No newline at end of file +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 42d5169935..d691a6f2e6 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -9,9 +9,20 @@ sh /etc/init.d/rc.mc_defaults if [ $DO_AUTOCONFIG == yes ] then - # - # Default parameters for this platform - # + # TODO tune roll/pitch separately + param set MC_ROLL_P 9.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 9.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 0.5 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 fi diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 2dc83a517c..b98ab47748 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -7,33 +7,9 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi +sh /etc/init.d/rc.mc_defaults -set VEHICLE_TYPE mc set MIXER hexa_cox +# We only can run one channel group with one rate, so set all 8 channels set PWM_OUTPUTS 12345678 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index a55fc5a30d..655cb6226e 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -5,33 +5,8 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi +sh /etc/init.d/rc.mc_defaults -set VEHICLE_TYPE mc -set MIXER FMU_octo_cox +set MIXER octo_cox set PWM_OUTPUTS 12345678 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index fa751f1e31..345b0e3e4e 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_x -set PWM_OUTPUTS 1234 \ No newline at end of file +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 232af57b4a..cd4480c3eb 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -7,4 +7,21 @@ sh /etc/init.d/4001_quad_x -set PWM_MIN 1175 \ No newline at end of file +if [ $DO_AUTOCONFIG == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set PWM_MIN 1175 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 259636acc2..ac2ecc70aa 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -7,4 +7,22 @@ sh /etc/init.d/4001_quad_x -set PWM_MIN 1175 \ No newline at end of file +if [ $DO_AUTOCONFIG == yes ] +then + # TODO REVIEW + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set PWM_MIN 1175 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 deleted file mode 100644 index a5c4a86902..0000000000 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ /dev/null @@ -1,8 +0,0 @@ -#!nsh -# -# HobbyKing X550 Quadcopter -# -# Todd Stellanova -# - -sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 2f5ab44d77..55b31067d1 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ -set PWM_OUTPUTS 1234 \ No newline at end of file +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 73ef12569b..7714a508cf 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -9,6 +9,5 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x -# We only can run one channel group with one rate, -# so all 8 at 400 Hz -set PWM_OUTPUTS 12345678 \ No newline at end of file +# We only can run one channel group with one rate, so set all 8 channels +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index ef4b6297de..60db8c0690 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -9,6 +9,5 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ -# We only can run one channel group with one rate, -# so all 8 at 400 Hz -set PWM_OUTPUTS 12345678 \ No newline at end of file +# We only can run one channel group with one rate, so set all 8 channels +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index bb87f89fe7..411aee1140 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_x -set PWM_OUTPUTS 12345678 \ No newline at end of file +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 48c26aacd1..3968af58ea 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -116,11 +116,6 @@ then sh /etc/init.d/4011_dji_f450 fi -if param compare SYS_AUTOSTART 4012 12 -then - sh /etc/init.d/4012_hk_x550 -fi - # # Quad + # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 52584677b6..4db62607a9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -4,22 +4,20 @@ set VEHICLE_TYPE mc if [ $DO_AUTOCONFIG == yes ] then - # - # Default parameters for MC - # param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_P 0.1 param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 + param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.1 param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + param set MPC_THR_MAX 1.0 param set MPC_THR_MIN 0.1 param set MPC_XY_P 1.0 @@ -42,4 +40,4 @@ fi set PWM_RATE 400 set PWM_DISARMED 900 set PWM_MIN 1075 -set PWM_MAX 2000 \ No newline at end of file +set PWM_MAX 2000 From 36bd7a797b6116b2f4b1c3fe358ee56eb7982b1d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 11:46:26 +0100 Subject: [PATCH 051/154] navigator: use bearing to home for RTL --- src/modules/navigator/navigator_main.cpp | 30 +++++++++++++++++++----- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..d7f6fd16ab 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1343,7 +1343,14 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; // don't change altitude - _mission_item.yaw = NAN; // TODO set heading to home + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); + } _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -1409,17 +1416,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); + + } else { + /* else use current position */ + sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); + } + sp->loiter_radius = _parameters.loiter_radius; + sp->loiter_direction = 1; + sp->pitch_min = 0.0f; + } else { sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; } - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; From 179aa17a3842bb68fa3849e890d20cfb9a1a5392 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 12:21:23 +0100 Subject: [PATCH 052/154] sdlog2: TELE (telemetry status) message added, type for 'rssi' and 'remote_rssi' in 'telemetry_status' topic fixed to be consistent with 'noise'/'remote_noise' and mavlink message. --- src/modules/sdlog2/sdlog2.c | 24 ++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 +++++++++++++ src/modules/uORB/topics/telemetry_status.h | 10 ++++----- 3 files changed, 43 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3c218e21fb..68e6a7469f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -82,6 +82,7 @@ #include #include #include +#include #include #include @@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; + struct telemetry_status_s telemetry; } buf; memset(&buf, 0, sizeof(buf)); @@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; + int telemetry_sub; } subs; /* log message buffer: header + body */ @@ -811,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; + struct log_TELE_s log_TELE; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -946,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- TELEMETRY STATUS --- */ + subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + fds[fdsc_count].fd = subs.telemetry_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1347,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- TELEMETRY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index db87e3a6a2..16bfc355d6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -264,6 +264,18 @@ struct log_DIST_s { uint8_t flags; }; +/* --- TELE - TELEMETRY STATUS --- */ +#define LOG_TELE_MSG 22 +struct log_TELE_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc7..5192d4d589 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ + uint8_t rssi; /**< local signal strength */ + uint8_t remote_rssi; /**< remote signal strength */ + uint16_t rxerrors; /**< receive errors */ + uint16_t fixed; /**< count of error corrected packets */ uint8_t noise; /**< background noise level */ uint8_t remote_noise; /**< remote background noise level */ uint8_t txbuf; /**< how full the tx buffer is as a percentage */ @@ -73,4 +73,4 @@ struct telemetry_status_s { ORB_DECLARE(telemetry_status); -#endif /* TOPIC_TELEMETRY_STATUS_H */ \ No newline at end of file +#endif /* TOPIC_TELEMETRY_STATUS_H */ From 3d83c45f7585c71bee3f07ea414d798ab7e2bae5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 13:20:15 +0100 Subject: [PATCH 053/154] mavlink: bug in telemetry_status publication fixed --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a371a499ef..1dbe564955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -351,7 +351,7 @@ handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub == 0) { + if (telemetry_status_pub <= 0) { telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { From f6694c2cef62ee3284598ed1b4d8c6954effab4e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 00:03:51 +0100 Subject: [PATCH 054/154] rc.fw_defaults: increase acceptance radius which is used by navigator to generate virtual waypoints (RTL etc.) --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3e340699fc..3a50fcf56f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -10,4 +10,5 @@ then param set NAV_LAND_ALT 90 param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 + param set NAV_ACCEPT_RAD 50 fi \ No newline at end of file From 4a66e285adcd88cf42c3907753abd9b433815e45 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 00:05:27 +0100 Subject: [PATCH 055/154] navigator mavlink log info messages: add #audio tag --- src/modules/navigator/navigator_main.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..2711be24fc 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,7 +865,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] nav state: %s", nav_states_str[myState]); prevState = myState; /* reset time counter on state changes */ @@ -1073,11 +1073,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter at current altitude"); } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; @@ -1175,14 +1175,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to offboard WP %d", index); } } @@ -1331,7 +1331,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1357,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1384,12 +1384,12 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); break; } @@ -1516,7 +1516,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } @@ -1541,7 +1541,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff completed"); } else { /* advance by one mission item */ From 4982e819837195aa512fb977639ce1dd0c0cec3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 15:30:06 +0100 Subject: [PATCH 056/154] Navigator: set loiter WP correctly --- src/modules/navigator/navigator_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..260356eca9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1080,9 +1080,8 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } - + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; From 036ebdbe78defdcc8c1cf5955e8df773a16e7e8c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 16:08:49 +0100 Subject: [PATCH 057/154] Commander: add guard for parachute deployment --- src/modules/commander/commander.cpp | 14 +++++++++----- src/modules/navigator/navigator_params.c | 1 + 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b85737..8129dddb30 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0; static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static int parachute_enabled = 0; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + //XXX: to enable the parachute, a param needs to be set + //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); /* welcome user */ warnx("starting"); @@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); } + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + param_get(_param_enable_parachute, ¶chute_enabled); } orb_check(sp_man_sub, &updated); @@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1ba159a8e6..9e05bbffa8 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -61,3 +61,4 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment From 08f1e6a9dc86c03015cb2a83dc94c3412d6d9eb4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 13 Feb 2014 09:20:33 -0800 Subject: [PATCH 058/154] Fix base clock frequencies for timers 9/10/11 (not currently used). Thanks to xiazibin@gmail.com for pointing these out. --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 850043ddf5..3bede8a1f7 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -141,9 +141,9 @@ #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_PCLK1_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_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. From ccfe476326d8b01e33a3a7ea115054a31fa7a2b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 20:53:47 +0100 Subject: [PATCH 059/154] decrease MC_PITCHRATE_P for TBS Discovery --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 880e4899b1..fe85f7d35d 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Anton Babushkin , Simon Wilks +# Anton Babushkin , Simon Wilks , Thomas Gubler # sh /etc/init.d/rc.mc_defaults @@ -15,7 +15,7 @@ then param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.0017 param set MC_PITCH_P 8.0 - param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_P 0.1 param set MC_PITCHRATE_I 0.1 param set MC_PITCHRATE_D 0.0025 param set MC_YAW_P 2.8 From 7441efde4745c0dddc08a36a0bbf83307f82948a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Fri, 14 Feb 2014 01:48:00 +0100 Subject: [PATCH 060/154] Add a lot of MAVLink parameter documentation. --- .../launchdetection/launchdetection_params.c | 48 ++- src/modules/commander/commander_params.c | 32 ++ src/modules/mavlink/mavlink.c | 12 + src/modules/navigator/geofence_params.c | 16 +- src/modules/navigator/navigator_params.c | 63 +++- src/modules/sensors/sensor_params.c | 303 ++++++++++++++++-- src/modules/systemlib/system_params.c | 19 +- 7 files changed, 433 insertions(+), 60 deletions(-) diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 63a8981aa8..45d7957f11 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -45,28 +45,46 @@ #include /* - * Launch detection parameters, accessible via MAVLink + * Catapult launch detection parameters, accessible via MAVLink * */ -/* Catapult Launch detection */ - -// @DisplayName Switch to enable launchdetection -// @Description if set to 1 launchdetection is enabled -// @Range 0 or 1 +/** + * Enable launch detection. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); -// @DisplayName Catapult Accelerometer Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0 +/** + * Catapult accelerometer theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); -// @DisplayName Catapult Time Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0, in seconds +/** + * Catapult time theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); -// @DisplayName Throttle setting while detecting the launch -// @Description The throttle is set to this value while the system is waiting for the takeoff -// @Range 0 to 1 +/** + * Throttle setting while detecting launch. + * + * The throttle is set to this value while the system is waiting for the take-off. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index d3155f7bf5..80ca68f21f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,7 +48,39 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +/** + * Empty cell voltage. + * + * Defines the voltage where a single cell of the battery is considered empty. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); + +/** + * Full cell voltage. + * + * Defines the voltage where a single cell of the battery is considered full. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); + +/** + * Number of cells. + * + * Defines the number of cells the attached battery consists of. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); + +/** + * Battery capacity. + * + * Defines the capacity of the attached battery. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f7..ade4469c58 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -76,8 +76,20 @@ #include /* define MAVLink specific parameters */ +/** + * MAVLink system ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +/** + * MAVLink component ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +/** + * MAVLink type + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); __EXPORT int mavlink_main(int argc, char *argv[]); diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 20dd1fe2f0..5831a0ca94 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -45,11 +45,17 @@ #include /* - * geofence parameters, accessible via MAVLink - * + * Geofence parameters, accessible via MAVLink */ -// @DisplayName Switch to enable geofence -// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present -// @Range 0 or 1 +/** + * Enable geofence. + * + * Set to 1 to enable geofence. + * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * + * @min 0 + * @max 1 + * @group Geofence + */ PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9e05bbffa8..ec7a4e2296 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,15 +50,72 @@ /* * Navigator parameters, accessible via MAVLink - * */ +/** + * Minimum altitude + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); + +/** + * Waypoint acceptance radius. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); + +/** + * Loiter radius. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * @group Navigation + */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); + +/** + * Default take-off altitude. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude + +/** + * Landing altitude. + * + * Slowly descend from this altitude when landing. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing + +/** + * Return-to-land altitude. + * + * Minimum altitude for going home in RTL mode. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment + +/** + * Return-to-land delay. + * + * Delay after descend before landing. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); + +/** + * Enable parachute deployment. + * + * @group Navigation + */ +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a0..288c6e00a0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -42,13 +42,10 @@ */ #include - #include /** - * Gyro X offset - * - * This is an X-axis offset for the gyro. Adjust it according to the calibration data. + * Gyro X-axis offset * * @min -10.0 * @max 10.0 @@ -57,7 +54,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset + * Gyro Y-axis offset * * @min -10.0 * @max 10.0 @@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset + * Gyro Z-axis offset * * @min -5.0 * @max 5.0 @@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); /** - * Gyro X scaling - * - * X-axis scaling. + * Gyro X-axis scaling factor * * @min -1.5 * @max 1.5 @@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); /** - * Gyro Y scaling - * - * Y-axis scaling. + * Gyro Y-axis scaling factor * * @min -1.5 * @max 1.5 @@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); /** - * Gyro Z scaling - * - * Z-axis scaling. + * Gyro Z-axis scaling factor * * @min -1.5 * @max 1.5 @@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + /** - * Magnetometer X offset - * - * This is an X-axis offset for the magnetometer. + * Magnetometer X-axis offset * * @min -500.0 * @max 500.0 @@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); /** - * Magnetometer Y offset - * - * This is an Y-axis offset for the magnetometer. + * Magnetometer Y-axis offset * * @min -500.0 * @max 500.0 @@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); /** - * Magnetometer Z offset - * - * This is an Z-axis offset for the magnetometer. + * Magnetometer Z-axis offset * * @min -500.0 * @max 500.0 @@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +/** + * Differential pressure sensor offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Differential pressure sensor analog enabled + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +/** + * Board rotation + * + * This parameter defines the rotation of the FMU board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); + +/** + * External magnetometer rotation + * + * This parameter defines the rotation of the external magnetometer relative + * to the platform (not relative to the FMU). + * See SENS_BOARD_ROT for possible values. + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + /** * RC Channel 1 Minimum * @@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif -PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +/** + * DSM binding trigger. + * + * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + * + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); + + +/** + * Scaling factor for battery voltage sensor on PX4IO. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/** + * Scaling factor for battery voltage sensor on FMU v2. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ -/* PX4IOAR: 0.00838095238 */ -/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +/** + * Scaling factor for battery voltage sensor on FMU v1. + * + * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 + * FMUv1 with PX4IO: 0.00459340659 + * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238 + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif + +/** + * Scaling factor for battery current sensor. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ + /** * Roll control channel mapping. * @@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); + +/** + * Return switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); + +/** + * Assist switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); + +/** + * Mission switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Flaps channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ -PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ +/** + * Auxiliary switch 1 channel mapping. + * + * Default function: Camera pitch + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); +/** + * Auxiliary switch 2 channel mapping. + * + * Default function: Camera roll + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ + +/** + * Auxiliary switch 3 channel mapping. + * + * Default function: Camera azimuth / yaw + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); + + +/** + * Roll scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); + +/** + * Pitch scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); + +/** + * Yaw scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); -PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ + +/** + * Failsafe channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_CH, 0); + +/** + * Failsafe channel mode. + * + * 0 = too low means signal loss, + * 1 = too high means signal loss + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); + +/** + * Failsafe channel PWM threshold. + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 75be090f80..cb35a25413 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,8 +40,23 @@ #include #include -// Auto-start script with index #n +/** + * Auto-start script index. + * + * Defines the auto-start script used to bootstrap the system. + * + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); -// Automatically configure default values +/** + * Automatically configure default values. + * + * Set to 1 to set platform-specific parameters to their default + * values on next system startup. + * + * @min 0 + * @max 1 + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); From ed3ffc26d69f25dc087a23761cbf0655afeae8c0 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Fri, 14 Feb 2014 02:21:24 +0100 Subject: [PATCH 061/154] Layout fixes for wiki parameter documentation. - Replace newlines in names and comments with a space. - Right align min/max/default values. --- Tools/px4params/dokuwikiout.py | 46 +++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index c5cf65ea60..e02035423d 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -19,34 +19,40 @@ class DokuWikiOutput(output.Output): for group in groups: result += "==== %s ====\n\n" % group.GetName() result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" - result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" + result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") name = param.GetFieldValue("short_desc") - name = name.replace("\n", "") - result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") - if min_val is not None: - result += " | %s " % min_val - else: - result += " | " max_val = param.GetFieldValue("max") - if max_val is not None: - result += " | %s " % max_val - else: - result += " | " def_val = param.GetFieldValue("default") - if def_val is not None: - result += "| %s " % def_val - else: - result += " | " long_desc = param.GetFieldValue("long_desc") - if long_desc is not None: - long_desc = long_desc.replace("\n", "") - result += "| %s " % long_desc + + name = name.replace("\n", " ") + result += "| %s | %s |" % (code, name) + + if min_val is not None: + result += " %s |" % min_val else: - result += " | " - result += " |\n" + result += " |" + + if max_val is not None: + result += " %s |" % max_val + else: + result += " |" + + if def_val is not None: + result += " %s |" % def_val + else: + result += " |" + + if long_desc is not None: + long_desc = long_desc.replace("\n", " ") + result += " %s |" % long_desc + else: + result += " |" + + result += "\n" result += "\n" post_text = """ From 9e56652d3eda75a2738bcb9eab3ff99ac2aa455b Mon Sep 17 00:00:00 2001 From: Highlander-UA <315u448@ukrpost.net> Date: Sat, 15 Feb 2014 13:34:49 +0200 Subject: [PATCH 062/154] Added comments for L1 control parameters --- .../fw_pos_control_l1_params.c | 193 +++++++++++++++++- 1 file changed, 186 insertions(+), 7 deletions(-) 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 512ca7b8ac..9f15a93866 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 @@ -120,57 +120,236 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); +/** + * Controller roll limit + * + * The maximum roll the controller will output. + * + * @unit degrees + * @min 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); - -PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); - - +/* + * Throttle limit max + * + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that + * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * +*/ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +/* + * Throttle limit min + * + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide + * some aerodynamic drag from a turning prop to improve the descent rate. + * + * For aircraft with internal combustion engine this parameter should be set + * for desired idle rpm. +*/ +PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); + +/* + * Throttle limit value before flare + * + * This throttle value will be set as throttle limit at FW_LND_TLALT, + * before arcraft will flare. +*/ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); + +/* + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. +*/ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); +/* + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. +*/ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); +/* + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. +*/ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + + +/* + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. +*/ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); +/* + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. +*/ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); +/* + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. +*/ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); +/* + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second^2) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. +*/ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); +*/ + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + +*/ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); +/* + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. +*/ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); +/* + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. +*/ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); +/* + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots – set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). +*/ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); +/* + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. +*/ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - +/* + * Height rate P factor +*/ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/* + * Speed rate P factor +*/ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); +/* + * Landing slope angle +*/ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); + +/* + * Landing slope length +*/ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); + +/* + * +*/ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); + +/* + * Landing flare altitude (relative) +*/ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); + +/* + * Landing throttle limit altitude (relative) +*/ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); + +/* + * Landing heading hold horizontal distance +*/ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 480ba491f35019bfe781b1616bc8db71e71f2740 Mon Sep 17 00:00:00 2001 From: Highlander-UA <315u448@ukrpost.net> Date: Sat, 15 Feb 2014 17:27:38 +0200 Subject: [PATCH 063/154] Missing descriptions added for L1 control parameters --- 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 9f15a93866..e0b8d8771a 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 @@ -247,7 +247,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); -*/ +/* * Complementary filter "omega" parameter for height * * This is the cross-over frequency (in radians/second) of the complementary From 28536682aa664b5dae74b96688cc66b919fccffe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 21:00:48 +0100 Subject: [PATCH 064/154] Fix airspeed sensor --- src/drivers/airspeed/airspeed.cpp | 4 ++-- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 7 ++++--- src/drivers/meas_airspeed/meas_airspeed.cpp | 10 ++++++---- 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 771f2128f2..524151c90d 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -76,8 +76,8 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : - I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : + I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c27b1bcd80..186602eda8 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -90,7 +90,7 @@ static const int ERROR = -1; class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval); + Airspeed(int bus, int address, unsigned conversion_interval, const char* path); virtual ~Airspeed(); virtual int init(); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 8bbef5cfa0..b6e80ce1d0 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -77,6 +77,7 @@ /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +#define ETS_PATH "/dev/ets_airspeed" /* Register address */ #define READ_CMD 0x07 /* Read the data */ @@ -93,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); protected: @@ -112,8 +113,8 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f8..c5394d9b49 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -89,8 +89,10 @@ /* I2C bus address is 1010001x */ #define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ @@ -120,8 +122,8 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -304,7 +306,7 @@ start(int i2c_bus) errx(1, "already started"); /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ if (g_dev == nullptr) @@ -313,7 +315,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS4425); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) From 4bd83dcaeb48cb6629d205b28532f49ca9440e1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 21:20:02 +0100 Subject: [PATCH 065/154] Fix compile errors --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c5394d9b49..eb65c049be 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); protected: @@ -315,7 +315,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS4425); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) From 2e4c26c9577d11a3ff0e838ffa2c25c4f4ec1452 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:12:15 +0100 Subject: [PATCH 066/154] Rename parser.py to srcparser.py to prevent naming conflicts with built-in Python parser library. --- Tools/px4params/px_process_params.py | 4 ++-- Tools/px4params/{parser.py => srcparser.py} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename Tools/px4params/{parser.py => srcparser.py} (100%) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py index cdf5aba7c0..4f498026fa 100755 --- a/Tools/px4params/px_process_params.py +++ b/Tools/px4params/px_process_params.py @@ -40,12 +40,12 @@ # import scanner -import parser +import srcparser import xmlout import dokuwikiout # Initialize parser -prs = parser.Parser() +prs = srcparser.Parser() # Scan directories, and parse the files sc = scanner.Scanner() diff --git a/Tools/px4params/parser.py b/Tools/px4params/srcparser.py similarity index 100% rename from Tools/px4params/parser.py rename to Tools/px4params/srcparser.py From 1a7518a4224082cf707585a93eb6d934127e5d9e Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:18:26 +0100 Subject: [PATCH 067/154] Fixes for Python 3: Use sorted() with key parameter instead of deprecated (and removed) cmp() function. --- Tools/px4params/srcparser.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 251be672f2..1b2d301108 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -28,8 +28,7 @@ class ParameterGroup(object): state of the parser. """ return sorted(self.params, - cmp=lambda x, y: cmp(x.GetFieldValue("code"), - y.GetFieldValue("code"))) + key=lambda x: x.GetFieldValue("code")) class Parameter(object): """ @@ -61,9 +60,10 @@ class Parameter(object): """ Return list of existing field codes in convenient order """ - return sorted(self.fields.keys(), - cmp=lambda x, y: cmp(self.priority.get(y, 0), - self.priority.get(x, 0)) or cmp(x, y)) + keys = self.fields.keys() + keys = sorted(keys) + keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True) + return keys def GetFieldValue(self, code): """ @@ -197,7 +197,7 @@ class Parser(object): if tag == "group": group = tags[tag] elif tag not in self.valid_tags: - sys.stderr.write("Skipping invalid" + sys.stderr.write("Skipping invalid " "documentation tag: '%s'\n" % tag) else: param.SetField(tag, tags[tag]) @@ -214,7 +214,7 @@ class Parser(object): object. Note that returned object is not a copy. Modifications affect state of the parser. """ - return sorted(self.param_groups.values(), - cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), - self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), - y.GetName())) + groups = self.param_groups.values() + groups = sorted(groups, key=lambda x: x.GetName()) + groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True) + return groups From d30335cb3f141fe0abf7ea853d5e5b097452a0a3 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:24:53 +0100 Subject: [PATCH 068/154] Fixes for Python 3 and refactoring: Merge generic Output class into specialized output classes as some need to write files in binary mode. --- Tools/px4params/.gitignore | 1 + Tools/px4params/output.py | 5 -- ...istings.py => output_dokuwiki_listings.py} | 11 ++-- ...kuwikiout.py => output_dokuwiki_tables.py} | 63 ++++++++++--------- Tools/px4params/{xmlout.py => output_xml.py} | 11 ++-- Tools/px4params/px_process_params.py | 22 ++++--- Tools/px4params/xmlrpc.sh | 2 +- 7 files changed, 65 insertions(+), 50 deletions(-) delete mode 100644 Tools/px4params/output.py rename Tools/px4params/{dokuwikiout_listings.py => output_dokuwiki_listings.py} (84%) rename Tools/px4params/{dokuwikiout.py => output_dokuwiki_tables.py} (62%) rename Tools/px4params/{xmlout.py => output_xml.py} (78%) diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore index 5d0378b4a2..d78b71a6e8 100644 --- a/Tools/px4params/.gitignore +++ b/Tools/px4params/.gitignore @@ -1,3 +1,4 @@ parameters.wiki parameters.xml +parameters.wikirpc.xml cookies.txt \ No newline at end of file diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py deleted file mode 100644 index c092468713..0000000000 --- a/Tools/px4params/output.py +++ /dev/null @@ -1,5 +0,0 @@ -class Output(object): - def Save(self, groups, fn): - data = self.Generate(groups) - with open(fn, 'w') as f: - f.write(data) diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/output_dokuwiki_listings.py similarity index 84% rename from Tools/px4params/dokuwikiout_listings.py rename to Tools/px4params/output_dokuwiki_listings.py index 33f76b415e..117f4edf43 100644 --- a/Tools/px4params/dokuwikiout_listings.py +++ b/Tools/px4params/output_dokuwiki_listings.py @@ -1,7 +1,6 @@ -import output -class DokuWikiOutput(output.Output): - def Generate(self, groups): +class DokuWikiListingsOutput(): + def __init__(self, groups): result = "" for group in groups: result += "==== %s ====\n\n" % group.GetName() @@ -24,4 +23,8 @@ class DokuWikiOutput(output.Output): if def_val is not None: result += "* Default value: %s\n" % def_val result += "\n" - return result + self.output = result + + def Save(self, filename): + with open(filename, 'w') as f: + f.write(self.output) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/output_dokuwiki_tables.py similarity index 62% rename from Tools/px4params/dokuwikiout.py rename to Tools/px4params/output_dokuwiki_tables.py index e02035423d..dca3fd92d7 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/output_dokuwiki_tables.py @@ -1,21 +1,8 @@ -import output from xml.sax.saxutils import escape -class DokuWikiOutput(output.Output): - def Generate(self, groups): - pre_text = """ - - wiki.putPage - - - - :firmware:parameters - - - - - """ - result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values." +class DokuWikiTablesOutput(): + def __init__(self, groups): + result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values.\n\n" for group in groups: result += "==== %s ====\n\n" % group.GetName() result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" @@ -54,15 +41,35 @@ class DokuWikiOutput(output.Output): result += "\n" result += "\n" - post_text = """ - - - - - sum - Updated parameters automagically from code. - - - - """ - return pre_text + escape(result) + post_text + self.output = result; + + def Save(self, filename): + with open(filename, 'w') as f: + f.write(self.output) + + def SaveRpc(self, filename): + with open(filename, 'w') as f: + f.write(""" + + wiki.putPage + + + + :firmware:parameters + + + + + """) + f.write(escape(self.output)) + f.write(""" + + + + + sum + Updated parameters automagically from code. + + + +""") diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/output_xml.py similarity index 78% rename from Tools/px4params/xmlout.py rename to Tools/px4params/output_xml.py index d56802b158..5576954c09 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/output_xml.py @@ -1,8 +1,7 @@ -import output from xml.dom.minidom import getDOMImplementation -class XMLOutput(output.Output): - def Generate(self, groups): +class XMLOutput(): + def __init__(self, groups): impl = getDOMImplementation() xml_document = impl.createDocument(None, "parameters", None) xml_parameters = xml_document.documentElement @@ -19,4 +18,8 @@ class XMLOutput(output.Output): xml_param.appendChild(xml_field) xml_value = xml_document.createTextNode(value) xml_field.appendChild(xml_value) - return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") + self.output = xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") + + def Save(self, filename): + with open(filename, 'wb') as f: + f.write(self.output) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py index 4f498026fa..7799f63484 100755 --- a/Tools/px4params/px_process_params.py +++ b/Tools/px4params/px_process_params.py @@ -41,8 +41,9 @@ import scanner import srcparser -import xmlout -import dokuwikiout +import output_xml +import output_dokuwiki_tables +import output_dokuwiki_listings # Initialize parser prs = srcparser.Parser() @@ -50,12 +51,17 @@ prs = srcparser.Parser() # Scan directories, and parse the files sc = scanner.Scanner() sc.ScanDir("../../src", prs) -output = prs.GetParamGroups() +groups = prs.GetParamGroups() # Output into XML -out = xmlout.XMLOutput() -out.Save(output, "parameters.xml") +out = output_xml.XMLOutput(groups) +out.Save("parameters.xml") -# Output into DokuWiki -out = dokuwikiout.DokuWikiOutput() -out.Save(output, "parameters.wiki") +# Output to DokuWiki listings +#out = output_dokuwiki_listings.DokuWikiListingsOutput(groups) +#out.Save("parameters.wiki") + +# Output to DokuWiki tables +out = output_dokuwiki_tables.DokuWikiTablesOutput(groups) +out.Save("parameters.wiki") +out.SaveRpc("parameters.wikirpc.xml") diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh index 36c52ff714..efd177f46a 100644 --- a/Tools/px4params/xmlrpc.sh +++ b/Tools/px4params/xmlrpc.sh @@ -2,4 +2,4 @@ python px_process_params.py rm cookies.txt curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login -curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php" +curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wikirpc.xml "https://pixhawk.org/lib/exe/xmlrpc.php" From d811853d4463279556f596c7fb064ba016a83acd Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:47:13 +0100 Subject: [PATCH 069/154] Fixed Doxygen comments and added parameter documentation group. --- .../fw_pos_control_l1_params.c | 153 +++++++++++------- 1 file changed, 91 insertions(+), 62 deletions(-) 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 e0b8d8771a..416a6fcfce 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 @@ -40,12 +40,10 @@ */ #include - #include /* * Controller parameters, accessible via MAVLink - * */ /** @@ -119,7 +117,6 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); - /** * Controller roll limit * @@ -131,17 +128,18 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); -/* +/** * Throttle limit max * * This is the maximum throttle % that can be used by the controller. * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * -*/ + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); -/* +/** * Throttle limit min * * This is the minimum throttle % that can be used by the controller. @@ -152,19 +150,22 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * * For aircraft with internal combustion engine this parameter should be set * for desired idle rpm. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); -/* +/** * Throttle limit value before flare * * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); - -/* +/** * Maximum climb rate * * This is the best climb rate that the aircraft can achieve with @@ -179,21 +180,23 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * demand required to climb and maintain speed is noticeably less than * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); - -/* +/** * Minimum descent rate * * This is the sink rate of the aircraft with the throttle * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); - -/* +/** * Maximum descent rate * * This sets the maximum descent rate that the controller will use. @@ -201,41 +204,45 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * This should be set to a value that can be achieved without * exceeding the lower pitch angle limit and without over-speeding * the aircraft. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - -/* +/** * TECS time constant * * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - -/* +/** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); - -/* +/** * Integrator gain * * This is the integrator gain on the control loop. * Increasing this gain increases the speed at which speed * and height offsets are trimmed out, but reduces damping and * increases overshoot. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - -/* +/** * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second^2) @@ -243,11 +250,12 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - -/* +/** * Complementary filter "omega" parameter for height * * This is the cross-over frequency (in radians/second) of the complementary @@ -255,12 +263,12 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * an estimate of height rate and height. Increasing this frequency weights * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. - -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); - -/* +/** * Complementary filter "omega" parameter for speed * * This is the cross-over frequency (in radians/second) of the complementary @@ -268,11 +276,12 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * improved airspeed estimate. Increasing this frequency weights the solution * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); - -/* +/** * Roll -> Throttle feedforward * * Increasing this gain turn increases the amount of throttle that will @@ -283,11 +292,12 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft initially gains energy in turns. Efficient high aspect-ratio * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); - -/* +/** * Speed <--> Altitude priority * * This parameter adjusts the amount of weighting that the pitch control @@ -300,56 +310,75 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * control to simultaneously control height and speed. * Note to Glider Pilots – set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); - -/* +/** * Pitch damping factor * * This is the damping gain for the pitch demand loop. Increase to add * damping to correct for oscillations in height. The default value of 0.0 * will work well provided the pitch to servo controller has been tuned * properly. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); -/* +/** * Height rate P factor -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); -/* +/** * Speed rate P factor -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -/* +/** * Landing slope angle -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); -/* +/** * Landing slope length -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); -/* +/** * -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); -/* +/** * Landing flare altitude (relative) -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); -/* +/** * Landing throttle limit altitude (relative) -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); -/* +/** * Landing heading hold horizontal distance -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 387bfad1cacc2f9929e7590130ca93f27c688655 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 22:19:14 +0100 Subject: [PATCH 070/154] Fixed status printing for airspeed sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f8..6fe6c22212 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -50,6 +50,7 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ + #include #include @@ -386,7 +387,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -411,7 +412,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } From 1c13225b194583945324d247b83a5236e665d919 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 16 Feb 2014 01:45:10 +0100 Subject: [PATCH 071/154] Fixed illegal character 0x96. --- 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 416a6fcfce..62a340e905 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 @@ -308,7 +308,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * and ignore height errors. This will normally reduce airspeed errors, * but give larger height errors. The default value of 1.0 allows the pitch * control to simultaneously control height and speed. - * Note to Glider Pilots – set this parameter to 2.0 (The glider will + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group L1 Control From 49cc0bd162b63b188c341aab4be880776d21c388 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 16 Feb 2014 01:47:11 +0100 Subject: [PATCH 072/154] Explicitly treat all files as UTF-8. --- Tools/px4params/output_dokuwiki_listings.py | 3 ++- Tools/px4params/output_dokuwiki_tables.py | 5 +++-- Tools/px4params/output_xml.py | 7 ++++--- Tools/px4params/scanner.py | 3 ++- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/Tools/px4params/output_dokuwiki_listings.py b/Tools/px4params/output_dokuwiki_listings.py index 117f4edf43..83c50ae15d 100644 --- a/Tools/px4params/output_dokuwiki_listings.py +++ b/Tools/px4params/output_dokuwiki_listings.py @@ -1,3 +1,4 @@ +import codecs class DokuWikiListingsOutput(): def __init__(self, groups): @@ -26,5 +27,5 @@ class DokuWikiListingsOutput(): self.output = result def Save(self, filename): - with open(filename, 'w') as f: + with codecs.open(filename, 'w', 'utf-8') as f: f.write(self.output) diff --git a/Tools/px4params/output_dokuwiki_tables.py b/Tools/px4params/output_dokuwiki_tables.py index dca3fd92d7..aa04304df7 100644 --- a/Tools/px4params/output_dokuwiki_tables.py +++ b/Tools/px4params/output_dokuwiki_tables.py @@ -1,4 +1,5 @@ from xml.sax.saxutils import escape +import codecs class DokuWikiTablesOutput(): def __init__(self, groups): @@ -44,11 +45,11 @@ class DokuWikiTablesOutput(): self.output = result; def Save(self, filename): - with open(filename, 'w') as f: + with codecs.open(filename, 'w', 'utf-8') as f: f.write(self.output) def SaveRpc(self, filename): - with open(filename, 'w') as f: + with codecs.open(filename, 'w', 'utf-8') as f: f.write(""" wiki.putPage diff --git a/Tools/px4params/output_xml.py b/Tools/px4params/output_xml.py index 5576954c09..e845cd1b10 100644 --- a/Tools/px4params/output_xml.py +++ b/Tools/px4params/output_xml.py @@ -1,4 +1,5 @@ from xml.dom.minidom import getDOMImplementation +import codecs class XMLOutput(): def __init__(self, groups): @@ -18,8 +19,8 @@ class XMLOutput(): xml_param.appendChild(xml_field) xml_value = xml_document.createTextNode(value) xml_field.appendChild(xml_value) - self.output = xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") + self.xml_document = xml_document def Save(self, filename): - with open(filename, 'wb') as f: - f.write(self.output) + with codecs.open(filename, 'w', 'utf-8') as f: + self.xml_document.writexml(f, indent=" ", addindent=" ", newl="\n") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py index b5a1af47c3..8779b7bbf6 100644 --- a/Tools/px4params/scanner.py +++ b/Tools/px4params/scanner.py @@ -1,5 +1,6 @@ import os import re +import codecs class Scanner(object): """ @@ -29,6 +30,6 @@ class Scanner(object): Scans provided file and passes its contents to the parser using parser.Parse method. """ - with open(path, 'r') as f: + with codecs.open(path, 'r', 'utf-8') as f: contents = f.read() parser.Parse(contents) From 1be3ea4f4eac121a627c194fefbf202586f6f66f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 12:18:22 +0100 Subject: [PATCH 073/154] MPU6000: gyro topic was not initialized --- src/drivers/mpu6000/mpu6000.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bf80c9cff2..ac75682c47 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1353,6 +1353,7 @@ MPU6000::print_info() MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent), + _gyro_topic(-1), _gyro_class_instance(-1) { } From a6af669399b8f12e497aff6292ec35dfd541d903 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 14:43:00 +0100 Subject: [PATCH 074/154] navigator: switch to READY instead of LOITER if landed --- src/modules/navigator/navigator_main.cpp | 66 +++++++++++++----------- 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 260356eca9..5952d667fe 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -305,6 +305,12 @@ private: void start_land(); void start_land_home(); + /** + * Fork for state transitions + */ + void request_loiter_or_ready(); + void request_mission_if_available(); + /** * Guards offboard mission */ @@ -699,24 +705,17 @@ Navigator::task_main() } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); stick_mode = true; } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); + request_mission_if_available(); stick_mode = true; } } @@ -733,17 +732,11 @@ Navigator::task_main() break; case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); break; case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); break; case NAV_STATE_RTL: @@ -770,12 +763,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_mission_if_available(); } } } @@ -1397,6 +1385,28 @@ Navigator::set_rtl_item() _pos_sp_triplet_updated = true; } +void +Navigator::request_loiter_or_ready() +{ + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } +} + +void +Navigator::request_mission_if_available() +{ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + request_loiter_or_ready(); + } +} + void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { @@ -1555,13 +1565,7 @@ Navigator::on_mission_item_reached() /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - - if (_vstatus.condition_landed) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_loiter_or_ready(); } } else if (myState == NAV_STATE_RTL) { From 92578e1fc3a3b011aa361ecee65d1bcce1593f6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 19:34:44 +0100 Subject: [PATCH 075/154] navigator: ignore min altitude on MC, loiter always at current altitude --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5952d667fe..577c767a86 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1059,7 +1059,7 @@ Navigator::start_loiter() float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { + if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); From 44c095b9e5717c707faa3528b861bad8eb7ac94a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 19:46:57 +0100 Subject: [PATCH 076/154] commander: allow arming from RC with safety enabled in HIL mode --- 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 8129dddb30..6d14472f36 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -430,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -516,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -1156,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { + if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { From 7f7ce77d616679aed7a923c37fff8ef7a6261da2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 20:14:39 +0100 Subject: [PATCH 077/154] mc_att_control: parameters descriptions added --- .../mc_att_control/mc_att_control_params.c | 119 ++++++++++++++++++ 1 file changed, 119 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 27a45b6bbc..488107d585 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,16 +41,135 @@ #include +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw feed forward + * + * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); From 4b41a398e71d177661cc5127427448d55bab944e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 21:05:54 +0100 Subject: [PATCH 078/154] mc_pos_control: parameters descriptions added --- .../mc_pos_control/mc_pos_control_params.c | 146 +++++++++++++++++- 1 file changed, 145 insertions(+), 1 deletion(-) 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 9eb56545d7..0082a5e6a6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -39,20 +39,164 @@ #include -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); + +/** + * Maximum tilt + * + * Limits maximum tilt in AUTO and EASY modes. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); + +/** + * Landing descend rate + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); + +/** + * Maximum landing tilt + * + * Limits maximum tilt on landing. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); From 2c589ea374da5fc3b368d9195a80bc20365c86d5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 21:11:27 +0100 Subject: [PATCH 079/154] navigator: parameters descriptions cleanup --- src/modules/navigator/navigator_params.c | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ec7a4e2296..87be737c42 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,7 +53,7 @@ */ /** - * Minimum altitude + * Minimum altitude (fixed wing only) * * @group Navigation */ @@ -67,32 +67,36 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); /** - * Loiter radius. + * Loiter radius (fixed wing only) * * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** + * Enable onboard mission. + * * @group Navigation */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); /** - * Default take-off altitude. + * Take-off altitude. + * + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. * * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); /** * Landing altitude. * - * Slowly descend from this altitude when landing. + * Stay at this altitude after RTL descending. Slowly descend from this altitude if autolanding allowed. * * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); /** * Return-to-land altitude. @@ -101,12 +105,12 @@ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when * * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); /** * Return-to-land delay. * - * Delay after descend before landing. + * Delay after descend before landing in RTL mode. * If set to -1 the system will not land but loiter at NAV_LAND_ALT. * * @group Navigation From 9665d38940dec371be3dd1c5e711a670162e2a30 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 12:51:13 +0400 Subject: [PATCH 080/154] navigator: parameters descriptions cleanup --- src/modules/navigator/navigator_params.c | 37 +++++++++++++++++------- 1 file changed, 26 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 87be737c42..9ef359c6dc 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,70 +55,85 @@ /** * Minimum altitude (fixed wing only) * + * Minimum altitude above home for LOITER. + * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); /** - * Waypoint acceptance radius. + * Waypoint acceptance radius * + * Default value of acceptance radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); /** - * Loiter radius (fixed wing only) + * Loiter radius (fixed wing only) * + * Default value of loiter radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** - * Enable onboard mission. + * Enable onboard mission * * @group Navigation */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); /** - * Take-off altitude. + * Take-off altitude * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); /** - * Landing altitude. + * Landing altitude * - * Stay at this altitude after RTL descending. Slowly descend from this altitude if autolanding allowed. + * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); /** - * Return-to-land altitude. + * Return-To-Launch altitude * - * Minimum altitude for going home in RTL mode. + * Minimum altitude above home position for going home in RTL mode. * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); /** - * Return-to-land delay. + * Return-To-Launch delay * * Delay after descend before landing in RTL mode. * If set to -1 the system will not land but loiter at NAV_LAND_ALT. * + * @unit seconds * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); /** - * Enable parachute deployment. + * Enable parachute deployment * * @group Navigation */ From 7d80f05b4d857b933cf7aca106ac55a7111e3b35 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 12:53:06 +0400 Subject: [PATCH 081/154] mc_att_control: more strict conditions for integrating --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index db5e2e9bb0..8de0f0b39f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -86,7 +86,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.05f -#define RATES_I_LIMIT 0.5f +#define RATES_I_LIMIT 0.3f class MulticopterAttitudeControl { @@ -658,7 +658,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f) { + if (_thrust_sp > 0.2f) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; From 838290fa63cf5592e5c9d7590d0f359f5f81aac3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 17:12:25 +0400 Subject: [PATCH 082/154] mc_att_control: pref counter name fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8de0f0b39f..38fde0cace 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -262,7 +262,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc att control")) { memset(&_v_att, 0, sizeof(_v_att)); From 8bc0287eccc7871db17c199a3b330e74c92c068a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 22:45:54 +0400 Subject: [PATCH 083/154] commander: disarm system when safety enabled --- src/modules/commander/commander.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6d14472f36..29a930ebb3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,11 +902,13 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); - // XXX this would be the right approach to do it, but do we *WANT* this? - // /* disarm if safety is now on and still armed */ - // if (safety.safety_switch_available && !safety.safety_off) { - // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - // } + /* disarm if safety is now on and still armed */ + if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + } + } } /* update global position estimate */ From 71d4c7fed8bc809fc38ba54147156fc834d38846 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 19:03:26 +0100 Subject: [PATCH 084/154] Startup: Hex vs Hexa --- ROMFS/px4fmu_common/init.d/rcS | 2 +- ROMFS/px4fmu_common/mixers/{FMU_hex_+.mix => FMU_hexa_+.mix} | 0 ROMFS/px4fmu_common/mixers/{FMU_hex_x.mix => FMU_hexa_x.mix} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename ROMFS/px4fmu_common/mixers/{FMU_hex_+.mix => FMU_hexa_+.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_hex_x.mix => FMU_hexa_x.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 50ac9759a5..d127853683 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -465,7 +465,7 @@ then set MAV_TYPE 2 # Use mixer to detect vehicle type - if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hex_+.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hex_x.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix From acf680389e95c82d2e0df1253f16842d320f3504 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Feb 2014 19:48:45 +0100 Subject: [PATCH 085/154] launchdetector: add reset, #663 --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 8 +++++++- src/lib/launchdetection/CatapultLaunchMethod.h | 3 +-- src/lib/launchdetection/LaunchDetector.cpp | 6 ++++++ src/lib/launchdetection/LaunchDetector.h | 1 + src/lib/launchdetection/LaunchMethod.h | 1 + src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 6 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index d5c759b17b..1039b4a091 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -42,7 +42,7 @@ #include CatapultLaunchMethod::CatapultLaunchMethod() : - last_timestamp(0), + last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), threshold_accel(NULL, "LAUN_CAT_A", false), @@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams() threshold_accel.update(); threshold_time.update(); } + +void CatapultLaunchMethod::reset() +{ + integrator = 0.0f; + launchDetected = false; +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index e943f11e9d..b8476b4c87 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -55,11 +55,10 @@ public: void update(float accel_x); bool getLaunchDetected(); void updateParams(); + void reset(); private: hrt_abstime last_timestamp; -// float threshold_accel_raw; -// float threshold_time; float integrator; bool launchDetected; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index df9f2fe95c..4109a90bad 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector() } +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 7c2ff826cf..05708c5262 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -53,6 +53,7 @@ class __EXPORT LaunchDetector public: LaunchDetector(); ~LaunchDetector(); + void reset(); void update(float accel_x); bool getLaunchDetected(); diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index bfb5f8cb4c..0cfbab3e09 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -47,6 +47,7 @@ public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; virtual void updateParams() = 0; + virtual void reset() = 0; protected: private: }; 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 45fdaa355e..a4142266b1 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 @@ -1055,6 +1055,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; + launchDetector.reset(); } if (was_circle_mode && !_l1_control.circle_mode()) { From e407766cc7c9f2c2d06deb064b9a1c89cb17a203 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:56:31 +0400 Subject: [PATCH 086/154] mc_att_control: minor cleanup and refactoring, move some class attributes to local variables --- .../mc_att_control/mc_att_control_main.cpp | 43 +++++++++---------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 38fde0cace..76ee2c311c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,7 +71,6 @@ #include #include #include -#include #include #include #include @@ -84,8 +83,8 @@ */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); -#define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f class MulticopterAttitudeControl @@ -135,15 +134,13 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ - math::Matrix<3, 3> _R; /**< rotation matrix for current state */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ math::Vector<3> _att_control; /**< attitude control vector */ - math::Matrix<3, 3> I; /**< identity matrix */ + math::Matrix<3, 3> _I; /**< identity matrix */ bool _reset_yaw_sp; /**< reset yaw setpoint flag */ @@ -262,7 +259,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc att control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -276,15 +273,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_i.zero(); _params.rate_d.zero(); - _R_sp.identity(); - _R.identity(); _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); - I.identity(); + _I.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); @@ -535,16 +530,17 @@ MulticopterAttitudeControl::control_attitude(float dt) _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - _R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0][0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -561,23 +557,24 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* rotation matrix for current state */ - _R.set(_v_att.R); + math::Matrix<3, 3> R; + R.set(_v_att.R); /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); - math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ - float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; @@ -600,15 +597,15 @@ MulticopterAttitudeControl::control_attitude(float dt) e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ - R_rp = _R; + R_rp = R; } - /* R_rp and _R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; @@ -616,7 +613,7 @@ MulticopterAttitudeControl::control_attitude(float dt) /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; - q.from_dcm(_R.transposed() * _R_sp); + q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); @@ -658,7 +655,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.2f) { + if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; From 55f5f1815f6a8f2efb969cea2eb061bdc2d9b92a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:57:23 +0400 Subject: [PATCH 087/154] mc_att_control: remove rate limiting to run at 250Hz --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 76ee2c311c..e92b7f3758 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -692,9 +692,6 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ - orb_set_interval(_v_att_sub, 5); - /* initialize parameters cache */ parameters_update(); From 2b4af6635708be2248abf53edb65a9223fedcd6a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:58:13 +0400 Subject: [PATCH 088/154] mc_att_control: poll vehicle_rates_setpoint if attitude controller disabled --- src/modules/mc_att_control/mc_att_control_main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index e92b7f3758..b5df83d859 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -761,10 +761,12 @@ MulticopterAttitudeControl::task_main() } } else { - /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; } if (_v_control_mode.flag_control_rates_enabled) { From 969f564a132e7b14e94028798ca5a4d6c5f13244 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:58:58 +0400 Subject: [PATCH 089/154] mc_att_control: code style fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b5df83d859..01902ed0cf 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -531,6 +531,7 @@ MulticopterAttitudeControl::control_attitude(float dt) /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; + if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ R_sp.set(&_v_att_sp.R_body[0][0]); @@ -762,10 +763,10 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } From 3eca9f9b6b866e1c7ad8b57d16ad16cb073d6085 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 17 Feb 2014 21:11:03 +0100 Subject: [PATCH 090/154] Updated logging/conv.zip with latest script versions from Tools directory. --- ROMFS/px4fmu_common/logging/conv.zip | Bin 10087 -> 9922 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip index 7cb837e5666f51eca7ed803dfef4581f01bec1f3..7f485184c6d453a422f795489fe64684e9b179e3 100644 GIT binary patch delta 9806 zcmZ{~MNpmr)200o+}(q_yE_DT2u^T!certP4Nh=};O_3h-QC?~>iz%4`_0r$Z+g*v zs(NuYSJydG=#v*zk_Csr0DS|&fXF;G#JFwb1yQg;AagSi2o?ka+1Q&K+uOM@+Fl4) z+pkF_ZbAS4nyH5lF%lj1T2>&HOu|Ma1AL_N(Rsqar+!Oew70jFx0SaM$}2?3rKTz} z-uEopk1k(Vx(Xv~kTg@m`Z`N|OVg}qJcA_H%-COaa=A)azBB@}b#Eic5Oh3RemrBM zaC#ZuSsBFq`yGd44xHff47H{40tzg8mf*c)EpBT<>tRXXKsTaeeU3 z#&wtc7?*}0ssGN3Oh8tv3;lL(F9?>RaK>oI7$B2dmyjsY)>P&_(81O@Y+%U~r4O*! zfD2tW*{yk;yi&HWGg(ovUdmH`^oHH|?Wl}UqK7vU(kn|4Zp@H<769gW0cUFV30tYQ ziA5&?Cff%Tk2dI#CMD_>#>fZcN!&=3fMUCw=(D~hX>VT>I2`$mhBX9x{d;4TkkyYT zFgam6qLY2%bHen+k@W|V^E7g}WhUJm>~ zA4G$$)qA2ZwVWPzW`7@D`7D+cr7YXDf9=W5Ei}$s1%)_dL!Nz+8s0w?dMOPe0k|_&b=*v< zoJ->VQXDu(Gqmrc^#>F5{Sd*Wz>mrG>qXA-zu)9vu{$$a#x@{U)l_`bjZUP=U%7-e z@tMY48{^R?Hc*tlO~kV?bEBQ(poRbOZ_QupK`BB`4nuf7-tZ5&Y;%=8 z??#k?q#5`qS|Rd8?WN}{Ss5|lSQ7w6rMnTxksW~RC=A)((*`aZsCtU*BRC)?H>BZk zmdt(aO*tT7a?yADdU4{)4rqJ^Hx&(|&rXKLM7t$A$cMnB!3Dz|!tpPe>0n+?m7NgN!%X{mY3OkfwqdYP=loU*v&%Vv$`=i2 zD~ilXy!x8_y>?G4GYvZ<)XeF@NWMs8hmi0JQf1{Q!_NNnXQqk?2M$H-`A@PF>nlXu zXNizTEUzksdMa3W@DZ6thBr$kL&3qU>;)%3VK4M`ktER%V{_an2~^yA>;j=+a+NpO zP(hX8j7>qcI>DZg*F=#VPdftG_eef)mj{)X%4|nD?cb<6WvnKHB&s$mRitjWP=uwx zTab79q3SiiDcx;}0q}Oyl`rdsL)p`*0sSRU6<^brxl@S=kceWP54T6Ogl=W0H#PPF|1ypxee+sPSJ^sZ6tXQu zfX~oSsTqHv@=$;ZZ|8OhHUPr{{b{*`gDMt3c@laYc?=aHhWfdvl=dtH5JZ3iSeZ3U zcWVTe$&XA;5k;uO{e{3^@wn<88mp^`MssC8bEQnDy1^egjP;THXz+?O$!4QjEb+*6 zqh{12#u5N1Ar%;GgDzO>e<9T%M3+^VakdAobshi023R3nUZ{G{x`NPma~+LyiD%g~ z@o&zVw5Ucz&8pg>Q-5`lv%Chjr)_L%#-EivU>G5qh=@{o-qT#`ow81vWghO3W35|z zp2fuxyrpBURl1+y`loK$weQ&^Sm}AvLvp4j)GUET+DR$gj#Fml53IPppCL@=kwDO# zF}-VzZcW*ebWWYLl5#1=_#X-ntIydiwUm8LT9hVxGXr9OEM@X+e{1Y4X%tsVcF%aF zh8CX-Upm}-s*N23R=gZluIpAVv_n5UgIfNL*evxdyda;YKdEv~=IA2x0+W$SjR70b z5Ffy1LY42>#(|q%w;MLgLzTUM2v3O}@45Ok^S|>QR8evj?Esd#d|XD?;RYRu4M(zT zo6Utqdy&P}hft-2bbB4P_!+kXtPNK4?SIW%;ajqcMH?gK3r;ch^J`%!2r0{r>oXQy z1}N4RBZ!l3cNEZXr`HrH*X0qp9dwr;H{XG$MS9I9p2Hf5t{R+FAD5t5sB%vq$>D2Q z`JZ>3!}pky(E!(q2kNhg9UHlfWdT`9>~m*fu{v`QQ^<%_=C9Ngx_A*Hw}8e6P~`x8 zj*Lct7ipXR=gqg)t#&YWC#%;A>be5l<5 z#`aUC&+{F^Czzg#YkOfm06VQcv7rq!#~D-IM6R()ye-iZ^9M^zW#qcLg(IJ?9*(p8 zO^~FFj)5S=%h`mQ_A<7x+xMQ$j(8w7Qv256RKFP`JsEjymH;{Tr|x!D2iDxIJsCUiY&e)*kPH4V!?`|i4e{MJ(3d5bxa1@pI@hhra_au z5wFd5>vTv%{Ef}Ve0~L`nbS!a1yC*Cf5O(ea5%?Ogd zD}1cw6AcQMkRFF66JP3+AqU7kpf^G&xWyeO*VxEso?kGSG1w+q2 z$$Ib?`k=0A$~aEIp{cgI63vs}p}G1K&|@fM?Rn8vLZ{c0>sK^F^#VZS>6-DYs;~=`s?Zc*S+f!0DwlfVXd2tgA|0JE(zN)?^6!McVB34Pa zdixRe_?$YhO#>5^==c0Ws{G8~r{)t?1_BS}FsT^=Wp;hYabc zpC6vh@mFoQ1&}a?i$eG@hkuXXa{L(cB3zt+l|ieik>B z*137Y&1rT+AEuS#`fZ4M_Rdi9N0f&!FZG*8G;9sPFs+|?IHBl#>%m8*Z-2C(d6DJQ zo|!aU)!drJzdkkEkHtgeN4zI(&ZMO!@RbUl83!9eV zA?5^-btR)Czv}|&S>1QaDLbY>D=zTF$C1|7;A!HFNbr`B?JVY_DQ%>1&$sdIXq2-+ z)L;c!<6t743&+{H8rsxxzJemwtE~LBt2hyxNlJ@%iF$$e#B26t*E?K7weDOo{nGSx zxj>@HOZTPr*U^eWfAwDiP8*4gU9}Vw5|&y(-m@mDSy~iQU5|l-UzJ_7|% z6-Emj2r-7bgj4+SX7Vb6>??!?wWiy{I`+X5bOZC%MdNA01)sFR5>09%*32~6hDp^P zA!C4p^4A-6qXPV^n=4^sUHZ|`uuh2I%MP{e?^asiN2ID>xErj*b7*_$hU@7`eNaq= zZ%fj7!KBHsW~_J64WifHi3sotF|WH!zv3`4Rg2e{OtVs+G6M;&eqv zoNJE$N@Ea0TGB$B^cp{A9rB*c_E#T~1_$lZ)D7t2@89ybj>@>ox+=TN)Q~e!7hVlN zj%}-(z-J?gVLQ%{ey2gVM6cgu;0F@?BOmSZRf|FQQi%9r3_WXFE|U~)U}@xODA3D`U~u%YG-Qh{ z=_a(uBL4ALL@4wAF5Zvu-?Z`TONL*UvQr`=D41G*swyR4err2DOQOn+06u!;&;;_} z0!t(z83y4|ek(l&Rb~^4c9zVh{_~F|rJe;=rsR1)g&gHJCw@zSov~|>$~aNl1U)8Y zJcMK^S&OA>ePdl&Ot{W zD|0ro&V$(`&{^|*8$-_JTRoN!-mi{s=Ybz9-z?UKQ2q1IeRfouS!y!IEAVP(j4sqJn&Rt zF>)K(i4r}(Rm5x@2b?|GI_4F}HbP7zn?SovOOiN`rw23g!!Fg2)UTZZ9c>~dFL7;) zuhVWmj2-()-OvDXGBxVFim*dT-kq(ubDvs2atDJ}Th>o3gfAw3ziy5{MZ(kB=mwYP z?NmKf>F#pE1?4~c5d6iiNg}-iEo0>6+GnsoU3!yHdzzswr#jvv%CTtIXz@JPp-#qC ziGA0gPV6xf6De9-BTO=Y#A|U%Bf3d?&pPA`Ek9Et_IeCC!O1XuSMVfMWjrIgNq*-# zw18X-e*3)C)! z31s!gIE(wN6i5LJKf|??SJ;_XVKT!4x8T1URm9|3UjGKms;{o0O3;v2m&Y94x`HBn zwn8CpGKxbM$OIbBs`|exslI3kI+3amH7?yIZ4h7sS>m4c#>%0j_kkAY+xmW+s~w{O z+$q3;5^2c9%ex0!@~d7R`{TEwR&44p?*OL;Z0cd}fLpKTfy18hgyrTJN{t^;>=>qt zSfms0F(jDbsvRRH*7h_x3YXeWwaLy(EJ(k(Kg+sO19ED zgVi;Go_UMwNre)T5tR+CWJN5pO3!p?zN>-}9SREm$LsRgIOI0X-ld4{zS1X7-HEC& z7evfBfrjpaw$J<7Cv@5KX=ahS=R}&?tw6>sa6Z935e3L;qy*}Y>FKu_Ow3-(_dt9b z=Lv7^96QBa4Ce%a`>DI$YU2bjj-rQ`mcVMD%bH8XZeoGgV_n5!I7yECZIhwfqF`hfYPaRryuIqViUg2uZ9RWWOaW*FigOL$Jj z9WR|v(`t}|=52NQ-qn|y5MVXWq;zYcPbj$@J{pxR1OB;Oh}j+$CUzW3~({`>*Q00u0q$p z%mO}7OXHvshL5aeNba4mtgzBQStlEzc+f-?OW*RMOEElxT@9rUp z>#qiU&?&d7?~{uE#_R3QXa)9KOQ7#Z;{^UpTemI;b)|%M@(d%pT*oBZBOgwfq;Li1 z5d5iWxP|BTiL$j(zw;TAl!9>MXKh0aMu{efQ4cZ;cpHl0{r06Wi*o90` zfS<;~NHnyvd%i6MsExvDKn__@jW z61_AA%da*mj8vJQ>MN?mF1t^Y>$3a&6)Y(2wD!RYL0DV|me5g}4H-IwIe)9*@{oLM zEtLm6B+^gggj}cIN{IM^=I%D9lc;j4d(k~d2M|AuDj=aUI(zOEp1%8Ha+F_LTU6T4 zX+ezF9Z-eMQa!1Z{V_p4j%S~4asF5Q)M(Wf)5L}`rz~>6sKW*$K(!E$f_@|B1Mg4T z0u(oXj4yZqqC;ZWG(*_iKjY&%@5VQ&z2+Vi*@+l`FL9!t!E8m3pEn&;_WN_rn~J3A z+edr;dIWsI9`e49a9$Uu!2PIhgmnJweL?^CJ#F|$Q!FY-K@dOyf!jMl5M%q>+ystBcaXkGEVNb5ChCPQir3+%ibdQfR42+~pMl@J{U% zY2BU6bW(poeZhr1JgptXA|eVkpKtpt0nXu`^Vu(ni1E)4(XSq78S0ny$2BT4ST9--T5Bzt0}64Eq5 z+Q(ckd58wqcijeJ^i|EbFRiNp5 z@xU3vxUWvplx#mEKp&C=ap#cYIK$qTEhC97-!CSSp4KzPYN_PmR=IX7K;d5wBbQQ& zgkjQ382hK^h&Bnb$tm!kY4)4hpS${#a+yyUp)w=N=DOx;hxg9{krl*vtf1 za^d>XJ?{K3CGL8|P#%4K=}H?6Xf$)Q%cjBYZI;|Dj&92l^%|9CvMt1W7HmXRQ>HBy>3zp+Hg zZV_R!N3`K@oaf91re=3N^H(3W(g_14n^dq7kX(W+6bs~1+6R$|-$Dv`4mqTpNzq2m=MsE$#8LI<)))%+`H`E* zu*sv$wd5w9=cTSDqEYvD*0#zP^*sx$DxPV&_dVCl+{5F>txK~ zMg6o#C0bO9ZWbG;MqM>}g)W!8+;tLv>~e(pNEZ2&q>oR8Y5s+VP2pBS;(yY+-EEM; z7U9>z7?vvK*svJ zgyJaNfI_<$Ctiq_v9B1_!WRAWs$6Bn`dgZK9^QP$77Wf${6#{qss7A{LaaNRNA{rn zO#=ili6VQ$IQSF!iR3^c*fJ|-TfZnq$9c|#5?7Oyb6TO=VqDXLVm9H1_74FK7d55y zQT1WKYHnLACln>3@U+r6npd#hmM&kZ{~b=TGmQlt#rf+b+W{CShJ$fW*?f6;?DJi@ z5it(l6!0SxpE4YZoJkdkyvyIek!&%7kK<`&VZIAb+TtCXU?Ij9M9xRiU3+ZD)W$i$ zu?nq-^c@%^Btvh$X$h&U-o!o6=xNCsrWgj815Ia}iH{35ZHf^uUW(;m=6F_WIO=Yr zlK;$|rb|a~e{ivXv+wB(YCe@(%fE)B^kdIkSf66WHCPd0uMX1ywN)?Oq*v&>H%<%*6eBO7Qyy|C(C!;%7B z)T4HC5@ES+fOCn-lfy0kSD>c82wZ!SZIYPVsMO7|uPGGa355gx5} z$+3;-r2SnQQK1~{U)7|Jt}j^??-kG(-dA%_V}o=0GFs$}l5jymGOtSTPHXZ`wZ;D+G&(54eh zgpinx(mAuviLorZ0 zi)6DhW!iPoo0))sa>!Sh>9C1h8d46r&etQ~u-Gs`(Nxhf?IRI<39)+_#Ur$NT7XQ= z$SY;_>5Z^gvpx&0AP$@LKl1hrMN%x-SUw1`I;MHfD2%DEiIu7eaLW zkvj&PU_{ciPvklyi|4WW*qATL2`hOnC_%Vvbi0tm=x+2hT5jpFr|`H{R5BQP5bAQ=C=5njI_y)8zt9*=6n$vg#nh5seTklx+lZ%y{>ShA%- zeSP^MGaS0f5L+(qQRoyQ-MVpWh$2ayj=u#SbAMmOD0~%=dQtk5q*+#wOH3PiroBWE z9A#I8w{_zj)3|U7ys40ko>G})sSz4;J;IeW_J~n$9SrF7>I*+MRXq61fLs+)?>rpF z_?Xl`{iDWB^W_Y`?vsjSKJ|>qisii&GEWNrrstnV$?G85Rg()YJvSeR=p;1e5euc# zJ&C1n)JsZBw~m}?u==Ub^HSd1+sh+TnK4L6YA@EVtN7Es@>S^#w^6^u%^uF$aLP}) zss<5yzXZtk=O>toS>&$h{=%Cve7<+*9y{ZyM`YBzzFBWU>XF><8$~Z?;5O}A- znZ|auFrL0QnWaY~CnKUW(X~x!wgu{J%I=-1zXfEOOcqt3n2fa-z7^9kT92n`bl-xj zZL=y?$a#16*+cEPl{zYRnB+;})S}Igo#Y438YzoZ;TqkSYjiD9|D*Wa=aRcV7k7-$ z*9Fz=tuiTyBm9e7#*uH_aBjCgwo7|>{e6(1{vea#l+-V>7h(2h%c6;Nk-l|4n#SKS z76Hf=JAYU}r=Lsfps@U7uo>!`1VIDe3!14e@^WRqU9?^Pp1LiBQU%{{onvQ}NdB=Mx#YP_~f`Y0;T<8Vu%-b<=OK4t8rj{Ai#pDdSORCTM6xtqn+APrOh zgu-pAj&t|q`ONw^>nkArS4M^HMNw01-Ks0cUVg@+*5-QTZ@N!s|7pp=FKN)wJr7WL z>uYHnP@BX5DfC~6`A^1X=ci5b6Br1j2Mz+kfj}S?agiVL;*2gHE^3MT_N&YogIjbD zeBTdX$zgsqQ;`!=SMZWkTg=$4j=Lmr48cRx!LNPnYzebUb67p?g3O4wmRXyUg zA#%iC_ypz!p~V^5FXuZuvOn=Q2x(II)%>zTnTJb7-|w)4)`4U7<=-O*>YSSaRK9~4 zz~UPW7qTinKB4>>PwWmQ0K0bJSnq=e<+!7%7AI+LZ*|K`H3-oe_Enm%JU>Y4MT@Z? zG3+xK$z*S>@T)JU@$${6Ko3AO&oXG-?PsMTD37PIrGLT{{?%I=2Shn<6v{uPyKTLk zVzFK7_lKvW0iB+2IACivdnBX&7f||Y3h=0*^`If>x?etIGR8b~9folWI|6R0yB5UN zF|dE_HI%hgCAlttZ`1bvit&^F^*gHH>WyG%*V6O3H#0P5{)oqR>C zB~ze>zMP@ZywT)pwGv=ACtIk`^KopEWMsjj2|fj6?9~2;J1@a?$ljm*+Vh5&H+CZf zl@{{X8f|kMU}ThyNFY&7Wk;TnV4_?O2F6%au-fc+0ZpG>&T;lqW){Fp^Fk6-JsQcP z9gLxuxPjmP{`i`dm@GpyDXAX6FaeL2dT_YpCjpr=7sjGHXUtV`fkg-Q3v@mDd}9qa zDiEI;bP3K+SUM)5xNfy2^JvoCOKHOO;xRp>}EV#S$1HjA1+c7>qN?fdixHtZrcGkiM+*}7O92A$z-w%7qJYb zhA4$fD8X!W7GSO-^5JgdG@6@OyS92Y0u+-F+$W8Y{3n2i!c&CwqL36dC5aS;aXnaD z=Fi-kQ_3+xUryq%T%=t5{`<3=^eENEXmT5uG20UyfIb!*{#;WL--8D6tau?6;zXiu zb~jjNCqh&rX2Dk!$F=_F58dw1%O7vPUJqN*t%0sVxzh?W)7voi)7#*OT9vQid4{Ez z!#MWuR{|BcOP$YHr7L}hrxUp|wdB*S+iM4v=g)-t`h{ltUGDk>J}SQVT@R|I=U;j` z6FEOVH$>zUyH>7&e{-#dx^%A7qjwi5?ODYsdrG=?lChA+K`P5g;|f`KNkNGY(tCMb zN69Nl(v#HnLH(Ri6x-z8hh(!cK~H$4KKlnzi$zGKVGth=zFJNVM~`I7e`fgmE9R7mvn1f0h?;Hrwsbb^Ixi!M!-CS9%20i6MV7o= z>gWtqg9OW0Ig$MX{Hk96&%k zG-)X`BGDE_l_9C_2j^kMgYF4cL%xq_oR#2DHFVrxsVH8cdR-!yo`chH%i`s`@DG&U zy|KBc@J$kB)#zlw$1y$S>anC+USWYl6dGn5GqPd&R0`9OEzu^MOnPj3i^Hlhls2%x zqL*tm^qCu4K5j-mA^C`$n-_M#Q)o~vepQs?W2Aysfn{@>8_1FPO#{Q1oygJe@dlj#03C7KSpMdyK$J{MomA~zpdw6;HdaIuUaSzgr?7Pio3ihpliJ|V=~ zwIXRks*-5{x1{THM*~+MdN; zCPa7mvpyZx*p%;d z0)=nC&TZljG$*v$7tEJ8fH&8VhS`qQei9uXBRa>&O05kRKJ(`D!EqA3gD#6^Wfes&u-yn z!rQ>5#c!FL>h}3*W5=4_San9B$jQSak+c>r#oBOTZ0_HuB9i{__hLHTjW=e#s;FUz z3g+>Sn^u^BN~gy}80kU_$AN@(*25x+ARQ+*!9%e8J;mH=7`rChLXfFxz!`6Jj7?97 zpxC7D*zA~AJtaETn7UX&SOe4ONJl+#XkymOM00^D?7)JH;Jgd*!IblpaqQF~Diim> zdKaZaVWNP_-f}gp-+7N^9EKuhYSEoFH}YyR1%+0aIOf4dcV8g_%l&4o{Y}+Ybt$HX zO?$;!bLlSTs*_1U2lo=!w0EDMai;Zr6>|8@>CG#(?w7`B1eJq4f(3msPX(WY|Jdj% zR0(7KOu$1xsy2N$()t_1MT?D&tsH~TdP&D6ax!LOMO)Zoe|yExu4aUm`k?B?L4lU^ zuZMLlnI&nD4!cSprfmZ0ocHinum#Ye(ZeqrO0&J9jcYkNqe?!~k!m_0qIMkwxeuc| z)uET%L@9Vvlle`S7~t4sD&aPzj-9FgkIORSKOz$ghNBDcSxgcc^xk)5bcJLdEE&Sq ziE-Vp`Ns_K2RM0spBKazXAbK10@p^lsU9Fz*w?aZO>;B8Vy)|O&1 zQzU&;TXfrLE!4PbZVIBevsMTSTb0t3L#11xTHJirr9xLVO$;74Tzn4SIbS^&^!6lQ z5ot-i{hoC}*eYQP?54ueJq-BQjH{P5!kqeNRjydL(kdr!kqAc75amJ_O~L#D^vok5 zp>ND{5^QZV>9%CBDmf5GGvu0?hY9o|<_C-&F5cEPJAw7ej$xaG~!vhF(0n= z5{mIQyxOyDY5zvnrd^(7cV)a6Vb&Z zk^3V0D)vr{-^YIx|f-e?D};PyCQGk5D9qoMaMP~mf6e&BKGHbxDf|cks(uUxQ3Sa0w zz^Z?Q&a8ObO|}BHLS=B@va4nwRN9=}6`d3kZ*iIu8($5Syjl_-x4ZT{u|EuhCxLNO zYL)U=^Ns-3E1U4`S7XbckT1M5@4US#xE81PJb9QR0eer9!eMt?&nH?yz?IcB#4`7OEc;fngr&L~q%gEn|&pUGx#Bn}18kp~zCb#-n8 zkv5>~Hq+s@sOz>CO-n*>Cfmea3+h8l7kEX?|2ec}PWpA>KF1v)jA>4l3XC?A84+rt zrUvNT4jP*AO*-xcb)^KYD2@=>+uZWAu(6M}zh1O=zJiK8guynlv>;;#B|DZ;wsU?w zy5BByy0b62eG81+--0BFZT`cjS^tLxmDz*mJt)tDqbxe8S_H4;(Jc^brTAxQ&0%Zn zm^qW~6Xg+D30Lni3*XpC=!-5i(JQ-%H-l4(_Yz93Z3VQ!MGUCS=7N)G^7zPt{x@@4 z(5*D)R*c7-4h2*od$axVsh7u={(Db$ho$-4BR=;&Th>x(J9??zfR?k1o9s4jqi0#B z!y8f-z4^Isrc2}0#fI)%{>!%n|Nni(rt5^LOPL%vzCTg#5t|Bu*oexr?i_inEF{ zPaqdLi-#$_;O=deu>*~1hLtnSb1+iMeixpPf-qOqOPGrOcCXE~- zUIN(QKwQaqUHSb99#j{nE)pJE__=;G?hNiIXjp}{qx&Xuq>EOgkTbE2u97i*0dhQCM36-;V;cLIXGzt~ z#wN5f0ZcM&F0F^^KapBd1p&?;Euiz%T1#!tkkgFmZ-kTTanqp#PYTRJ0iiwycM>7 zs2E{FG1Lw{j$A5)*el*_eRwwRgc^?!pV+?sZUbKOQd-A<2xojhxPWhhjUhrjEgMMc z(UO&6n(mL1QkH@DXWfiTJx}VZz>QVBJyXytWKq9F(-SS?6A`6kSFBx$x(L#V12P%F zdVqis3&|4-`IE}f7X4OEa%)y3m<_g-OkVuNgX(Nm+R({xKBfa4Tx|Cw)!hA!mq@rH zzEmYjM3p}FqS+Oix2rA-{sXM^f;BXfZS^`8+zZx2h+p?{u1H_+3hT3%rx7#X{B3;> z_H)U@e?lC;Casyvnwn!>sWwu*HXn*6AtC}i9FK+0+3>XGQLfYr^=yRi4>{K5L?sDP|892BVZ=XJ$&p1&Mq4b#&;&TEv!mUQ{Ny{ zU&6^$!9vHAmw3y2b<~0qc11PZp+7{y{RFCD1HNv;62k`zLs4M5iS6RW)RCF~0NucPbF2w8mXIdX!S+#Z@(PvaXPT+xAoFVQJ{=wQ9vJ3o28b;_liBY)*< zIuZVH61wg!-O-;uBVcp7YNvIWw=Vj_ZLy43jT=Jd7YY!@?e+Oa$euog<`4QJ(01@L zn>waoyYg;9gh{oG7i(npF%K;*5fnI* zof-e0qbWMwYU^`}yVArjO6BbPZU2wS)wm#mNNH;I2%xbnX#AT7X%lTxe*(0C@;5BQ zoUrB-VGVz1ZruJmSs%=2Od@LAeudE3KAW72GLo;gWsFYL&w0~?-pvx};MI}KAuj_x+Sxyu>2Kr2Z zyrp(KPWEJzAHZHij}CmUYECxj*BTM}jDu;4z%nloM!pq9C-S$qZc?4U$(<9In>~u= z>mliH%N)J^St=a(u9fEL7Ed?dh|#F4d$G0V5Yjq~*KGr@+ECruudU8oiLA8?sZ~vm zHkJ{{RIQf_b&e2tJuhQkyv8~#rsSHnYksAMx_;|ZjL@4vl&gjf(iBm#v#_jA`*Be@ zkS5q$iRpV^)Al^#w5ii*SpI9$0M^@pneO$30v}QJ{v|ec7ade~ggf$pArbx6vg(ei z7qM$23tHuqk-)le5)^4L2QY?=V#MgBrqIR<5xNF6KLV--;d7)F7pdt4`uvievcOg< zSO8xDZdc>Prk$mYi z)o<8_4a=zM1>?y>W#Jc+sNMy#(n>|a7fi|QbJka)_EXJFFs!P|{T}a--obROjyG19 zy-^c-QrcTIGwpD+EG6s96nbM#a0Th&%c6F)O>OuLjM42CFMXuN^h|_-7cNIM^cV0% zJV=K2`jRqY^d3x)wHuML(t+6(cXN+b_DrpIZiWhO>P=}sGw%h_)a^K2^Cvp`#IPBK zrB%v?P)AjSIWN)^wW~38kfFL@#V{hcO^YD}bE;J^wCNJH5w}|GUG^&rK5<%D&uqeV z+JB&k2C8%}W11=c$x~NjFkF^qt3^fmF8F@IAd51br`sDSUhV6jltAkH3;#X?bf~90 zEH72lVp1cno>QL|IEj+6OFy+#HJDOYWxenE zYBttInI}E`S77TYtYGbsG|7_sQz;|QH}{I3B1x~nBg1*#a=we$S)zQUdB=Xqx#l`F zSlB;8Zbq>3sJgPLuLlBT6Wb!*+C}t~GDT}!r4i0ie=Ws=*Bx1cwf)GvzMox^@v4<^ zgl`mTn%f`NkzsweVgFFTxEP(x1%>J5Y_)BRe?R^*85Bhr=p;8mLI?iuv4z~;sEPY=jh? zd$Z;AJCHK2)#>ihRla9!daSjR%VX0wb8KwjGK<$H>3$c6CC8a!ALtcJ)AW*TGTYus z)W#aX+!~F@06b$ZLCipGcwgC=rH5ZB9B;gacRv98{L7W1!+Y2J1A*5?)akg1F3PO{ z4f(_cdobjiPw26FpL%L~Mit9FoY}NnaW%!S4ifG4kKLY)#D*kkrI`=;sbU6Of;C-{ z5nq$zO%y`)#B}rz`PN>KhN&_X&8Go1kNru!=ytA^pa#yfB}|EKnw0Uso*`ZCLgVx8 z;^oUyD@oH>+af^iL{q_ zOfpr&!=(x`XaU7%4Fbe2W9W>WS6OG@%?pI!qsPydIUSow(cdoDvX0FBBMW5_Lhv&n zR9-wPJIQ$_mfJe|9Hc0Fir5U;FmW0MfyPL?+wVCBV*7lVps&#Z#Z#>rIWFbW`X;bt z4y?DmxGFo7!i}__Tc4?O^y5_&gWuzWiGJ;-2XywM&+%vY&Wdcuh4isw)kQn{O&ap{ z|F)OLT&wOkaYHElwl-V+&*faBR|OyN58sM9(Z_%J%9e1ord$gP8T-6BX%{;g_7(d! z;yXkwAleB@>OG)0aK-)juqJe6MR-N_mn!!Hd*YWpQ;(26??zQcWSHL- zWpTQ00g_MjQUZ(4`FUndt;47oyvkriiNkX5;T_hfC=@8v!aETjh+<{B))U{B;^S$I zl{Y9A6BEL}Zg=b#srH#~VY=OBn|{eDNM#EtkeTS4#i$`V?~wOwcCh}0G&E$Frfx_F z|M0h9+q|5kyxXR$d<99ndDv+G#I~dbVm^`xw(Fc|p-}RS7wOg?0MdbgRQh=IA~(Z` zgB8nI+csG`@2@(;JWy+;YV6?JTt8vg^32aIeqA%WXWL@pJlsp4Z32ymuKTbMZ;hQ~ z5k<_lDpht{<-JKDCYMZN5|P~V4}fIPs}8B7?n+h*5NatNGmWqp-I$aftiNBZ(5=?0 z)omN~`AR_j#mBmM?x-8>N)S7`yN99{K~bx2Gpa*^SoQcy>Ac(_bLzUVY$Kd?>;!N9)4^!%rG8$ggbsszCik_ryq&eprj)`;Jjt@m{gM>cDjHI3SBT zrq}na#k&VAP~5HWVm8a2Bz4}EYReJJ+0s8= zLgb$E11>)KS>3kZBr1Foyz?m*h$`xU^<@W6TSuM+SC$aE>ohpyl+$5<8o0mvS_{hr zf!Gpzv$kItYeNDF|2 z>L8CJ6GGAZgK_?{qt`T>hB(JGP}mWW0H$J}=(k*hne>sOLqkd=LG0#V7`9tZ2#t{8 zxMnKnq6E@LW9(%U7ILHj+H#a;szyu0WcbXmz-{;+KPz=-f$p1ttooYT1Pv*5IgGLG zYiNQOD-@y@<2Yo#Oh1d5CnM;x@^=k(M-uhnW{pNv8c4CaYnVPJ^$#Kx)DX(uWRh!J z_`zr~k49sqP?Cp0i;Eot-{tkLF@MgKukk!lxQxcvTQaJcZb8c{C~0d(MWko&LkmX5 z*k|y=N4tnA|1{!y`$oB%a7;_8@hW=J^ff#Y8kAD^gt6s6%DmZ|%?F0m`*}ZqF#UR0 zfuVakRY}tv!!O*aJignI`R=`Q>Q;i7kAcaghU?w9V2RSO=!$umH1y3{|EO>JFV$U5 zJQ{(Q`%>=+j9%5w`JjQ(q8A>+(XvQKXygb%({)*mK=%W^A6)Wku2G=Lt!ZJeFMNjs z(#1OA3k7&USKhL#@hAL#~ExcXb6!W#Ehju1E_0jn5WRj=(tKmxhs6S}_&0ANf zwrfF~h~vl#pVyAMXmwE*9fn88V#SMh#R<{^w&3|<4gTKOT2@E$6+$q5Pn9HP> z)dm)ow!cm>#{&|T6+je5DrkE`=n|$Sk#r(E>BtX0Pz5kms$`;jRj33?*%>|g0Y+?s zaNpa=Uz6xaL(P`bXfsm%N;j6Hvq|VDM)VHd;1v0oZ;Oq`7$}Pd?a-5-URLQCXfz;R z0rb9n^!4$^296Do8+@1jxOs5u6pbqp$31)WVcYT6wKENux_5)L0$WiMhen@+MjkYr z@720fXR_1!R~Q*xsunf8O4$(X8uQ9zxbi=zhGq(FtGz$-43f2q*@8qPcwj}1CF)*1 zQ9ixo7G*hKMr*0A3Ues*S%qN8{3!`=4C6m%N}UxOm9a4yBLN8J6@eClEw<4L44_NPcD58jRfhkQL2kkS&q z$S#Xhou5x*6!edsAkZts#_k9zRkgn*uZ{{i-R;!C516t)K=;x(=j)*b|BEx*(d*`H z@@;pR7)_enFNO@`agZxsnbp&u;V4M5g0d=b63&P*Db$lu)M@s21pM2T+$#Ir@0|s# zLOcQCWwK2bKILm(+2#_DG$xE>pX!r@^>CkSr}mMUVG?10*PK#<4pbQqvh|k9gV{J9zUw0 z*1&;yL2``%I}+RwcIC0%Ea&@61iW=s=BC`%P4L}9H)-eD{$QTm#eY~!C<mkR>LYR!`s%Mm*b4^ayBo(km9X`UmFGP81sl@-XDvFcdb9P z2utE(%i_MS6?go1*07nXjq;<_=4S>9%CW0%Gi16z?|S}1$qhDN^bo|V_F76Jy8&(Y z02`)a7Dr)7N2Scqn`o9LLcg=DX9WRQ=+fivTuKFAy&%fF#0b2%0d;r;pjW_g_7&F~ zja5}wZDDD*13;?P9CJdt_c{b{kxKP%R!?CaBtvJy~x`x!zfT;-v` z>EQx{wE(KXzvkK-zjb3MO?V-5e|m$-L%pu@{Jw}O;L!ULz26P=SPIvacrn#AYo#~G zhi=LzhVOXyG@d+f_&SF#gZU5`nQe6?xZ#QTT7$#?`nO&CS~r470WI9-Ef@fx3;_VZ z0e%3K#e_uV#2B32o&U9LAJc$UejEOel8~CRj{Q2*ci)-Xmt@L#BIm7~c=x=jv-rF` zp-xN21*tt0nOOWad7>|0O8LkRDy;2kHy*9(H~5k5?K++=Lnn71zfX^K0)13=btUIn zl;jk}BXG*BByr=!j0zTQ7l)iGEH24oUUrwM{U(St=`$J4sbW0yp6?c7)U4zsL>9lq zh*M4y>|1pbr4di#wS=P64J(xruHsP_o`%$SXi|h}P}*XUn!!X=h9-Hv++Wb*DV|9~ z64T}tlBl?05cWn45LpU!jDiT$w6wfnYVOIcUmE$KEhngcj(!>{)Bn0^eWAEmIo^7+ zq#H<~PYD8Ab(N1!M1OBYM))KR)mFk1#{N!Bsi1o#Bkt|7(XbERGUquO83FySRrV`z z?hbi8+LM42G0W|$4+J^XevL}$4`;GRl? zcRx=2hr}HA2sRPz?6~6empqD=rvW2YQ~*8m6N||D3yO)1vJxR4*Fiy^_^bYP9;*p< z5#<*^Q3ed05aNI8mth0`IbZ+)peW#<*MD!DLH(b$*?()A{X6qNW1oL#b|V1(Jp_UR u{3`_dUvU4a?0?5mlL7u8okIcsrSt#bK|%jB3 Date: Tue, 18 Feb 2014 00:13:55 +0400 Subject: [PATCH 091/154] Unused includes removed --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- .../position_estimator_inav/position_estimator_inav_main.c | 3 --- 3 files changed, 7 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 01902ed0cf..24226880ed 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,11 +53,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include 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 25d34c872d..b06655385a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -68,7 +67,6 @@ #include #include #include -#include #include #include #include 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 bf4f7ae974..ad363efe06 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -42,14 +42,11 @@ #include #include #include -#include #include #include #include #include #include -#include -#include #include #include #include From 98ee73463f428b420d4aeb44af4a7a90d8d40e41 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 17 Feb 2014 21:14:40 +0100 Subject: [PATCH 092/154] Removed obsolete ROMFS folder for removed logging build. The logging makefile was removed in 9a54c7c6. --- ROMFS/px4fmu_logging/init.d/rcS | 88 -------------------------- ROMFS/px4fmu_logging/logging/conv.zip | Bin 10087 -> 0 bytes 2 files changed, 88 deletions(-) delete mode 100644 ROMFS/px4fmu_logging/init.d/rcS delete mode 100644 ROMFS/px4fmu_logging/logging/conv.zip diff --git a/ROMFS/px4fmu_logging/init.d/rcS b/ROMFS/px4fmu_logging/init.d/rcS deleted file mode 100644 index 7b88567197..0000000000 --- a/ROMFS/px4fmu_logging/init.d/rcS +++ /dev/null @@ -1,88 +0,0 @@ -#!nsh -# -# PX4FMU startup script for logging purposes -# - -# -# Try to mount the microSD card. -# -echo "[init] looking for microSD..." -if mount -t vfat /dev/mmcsd0 /fs/microsd -then - echo "[init] card mounted at /fs/microsd" - # Start playing the startup tune - tone_alarm start -else - echo "[init] no microSD card found" - # Play SOS - tone_alarm error -fi - -uorb start - -# -# Start sensor drivers here. -# - -ms5611 start -adc start - -# mag might be external -if hmc5883 start -then - echo "using HMC5883" -fi - -if mpu6000 start -then - echo "using MPU6000" -fi - -if l3gd20 start -then - echo "using L3GD20(H)" -fi - -if lsm303d start -then - set BOARD fmuv2 -else - set BOARD fmuv1 -fi - -# Start airspeed sensors -if meas_airspeed start -then - echo "using MEAS airspeed sensor" -else - if ets_airspeed start - then - echo "using ETS airspeed sensor (bus 3)" - else - if ets_airspeed start -b 1 - then - echo "Using ETS airspeed sensor (bus 1)" - fi - fi -fi - -# -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. -# -if sensors start -then - echo "SENSORS STARTED" -fi - -sdlog2 start -r 250 -e -b 16 - -if sercon -then - echo "[init] USB interface connected" - - # Try to get an USB console - nshterm /dev/ttyACM0 & -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_logging/logging/conv.zip b/ROMFS/px4fmu_logging/logging/conv.zip deleted file mode 100644 index 7cb837e5666f51eca7ed803dfef4581f01bec1f3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10087 zcmaKyV{j$hx~*5-u{!S9){1T0PC9lvwryj@w#|-hvt!%n;MBKo)vbNczUR!USvCL6 zr|PX5f5tn8q6`G&cK`tJ4S+6!D1z1=V&H-R05G!v07!rz04Eb0dvg{86BkqR>C zB~ze>zMP@ZywT)pwGv=ACtIk`^KopEWMsjj2|fj6?9~2;J1@a?$ljm*+Vh5&H+CZf zl@{{X8f|kMU}ThyNFY&7Wk;TnV4_?O2F6%au-fc+0ZpG>&T;lqW){Fp^Fk6-JsQcP z9gLxuxPjmP{`i`dm@GpyDXAX6FaeL2dT_YpCjpr=7sjGHXUtV`fkg-Q3v@mDd}9qa zDiEI;bP3K+SUM)5xNfy2^JvoCOKHOO;xRp>}EV#S$1HjA1+c7>qN?fdixHtZrcGkiM+*}7O92A$z-w%7qJYb zhA4$fD8X!W7GSO-^5JgdG@6@OyS92Y0u+-F+$W8Y{3n2i!c&CwqL36dC5aS;aXnaD z=Fi-kQ_3+xUryq%T%=t5{`<3=^eENEXmT5uG20UyfIb!*{#;WL--8D6tau?6;zXiu zb~jjNCqh&rX2Dk!$F=_F58dw1%O7vPUJqN*t%0sVxzh?W)7voi)7#*OT9vQid4{Ez z!#MWuR{|BcOP$YHr7L}hrxUp|wdB*S+iM4v=g)-t`h{ltUGDk>J}SQVT@R|I=U;j` z6FEOVH$>zUyH>7&e{-#dx^%A7qjwi5?ODYsdrG=?lChA+K`P5g;|f`KNkNGY(tCMb zN69Nl(v#HnLH(Ri6x-z8hh(!cK~H$4KKlnzi$zGKVGth=zFJNVM~`I7e`fgmE9R7mvn1f0h?;Hrwsbb^Ixi!M!-CS9%20i6MV7o= z>gWtqg9OW0Ig$MX{Hk96&%k zG-)X`BGDE_l_9C_2j^kMgYF4cL%xq_oR#2DHFVrxsVH8cdR-!yo`chH%i`s`@DG&U zy|KBc@J$kB)#zlw$1y$S>anC+USWYl6dGn5GqPd&R0`9OEzu^MOnPj3i^Hlhls2%x zqL*tm^qCu4K5j-mA^C`$n-_M#Q)o~vepQs?W2Aysfn{@>8_1FPO#{Q1oygJe@dlj#03C7KSpMdyK$J{MomA~zpdw6;HdaIuUaSzgr?7Pio3ihpliJ|V=~ zwIXRks*-5{x1{THM*~+MdN; zCPa7mvpyZx*p%;d z0)=nC&TZljG$*v$7tEJ8fH&8VhS`qQei9uXBRa>&O05kRKJ(`D!EqA3gD#6^Wfes&u-yn z!rQ>5#c!FL>h}3*W5=4_San9B$jQSak+c>r#oBOTZ0_HuB9i{__hLHTjW=e#s;FUz z3g+>Sn^u^BN~gy}80kU_$AN@(*25x+ARQ+*!9%e8J;mH=7`rChLXfFxz!`6Jj7?97 zpxC7D*zA~AJtaETn7UX&SOe4ONJl+#XkymOM00^D?7)JH;Jgd*!IblpaqQF~Diim> zdKaZaVWNP_-f}gp-+7N^9EKuhYSEoFH}YyR1%+0aIOf4dcV8g_%l&4o{Y}+Ybt$HX zO?$;!bLlSTs*_1U2lo=!w0EDMai;Zr6>|8@>CG#(?w7`B1eJq4f(3msPX(WY|Jdj% zR0(7KOu$1xsy2N$()t_1MT?D&tsH~TdP&D6ax!LOMO)Zoe|yExu4aUm`k?B?L4lU^ zuZMLlnI&nD4!cSprfmZ0ocHinum#Ye(ZeqrO0&J9jcYkNqe?!~k!m_0qIMkwxeuc| z)uET%L@9Vvlle`S7~t4sD&aPzj-9FgkIORSKOz$ghNBDcSxgcc^xk)5bcJLdEE&Sq ziE-Vp`Ns_K2RM0spBKazXAbK10@p^lsU9Fz*w?aZO>;B8Vy)|O&1 zQzU&;TXfrLE!4PbZVIBevsMTSTb0t3L#11xTHJirr9xLVO$;74Tzn4SIbS^&^!6lQ z5ot-i{hoC}*eYQP?54ueJq-BQjH{P5!kqeNRjydL(kdr!kqAc75amJ_O~L#D^vok5 zp>ND{5^QZV>9%CBDmf5GGvu0?hY9o|<_C-&F5cEPJAw7ej$xaG~!vhF(0n= z5{mIQyxOyDY5zvnrd^(7cV)a6Vb&Z zk^3V0D)vr{-^YIx|f-e?D};PyCQGk5D9qoMaMP~mf6e&BKGHbxDf|cks(uUxQ3Sa0w zz^Z?Q&a8ObO|}BHLS=B@va4nwRN9=}6`d3kZ*iIu8($5Syjl_-x4ZT{u|EuhCxLNO zYL)U=^Ns-3E1U4`S7XbckT1M5@4US#xE81PJb9QR0eer9!eMt?&nH?yz?IcB#4`7OEc;fngr&L~q%gEn|&pUGx#Bn}18kp~zCb#-n8 zkv5>~Hq+s@sOz>CO-n*>Cfmea3+h8l7kEX?|2ec}PWpA>KF1v)jA>4l3XC?A84+rt zrUvNT4jP*AO*-xcb)^KYD2@=>+uZWAu(6M}zh1O=zJiK8guynlv>;;#B|DZ;wsU?w zy5BByy0b62eG81+--0BFZT`cjS^tLxmDz*mJt)tDqbxe8S_H4;(Jc^brTAxQ&0%Zn zm^qW~6Xg+D30Lni3*XpC=!-5i(JQ-%H-l4(_Yz93Z3VQ!MGUCS=7N)G^7zPt{x@@4 z(5*D)R*c7-4h2*od$axVsh7u={(Db$ho$-4BR=;&Th>x(J9??zfR?k1o9s4jqi0#B z!y8f-z4^Isrc2}0#fI)%{>!%n|Nni(rt5^LOPL%vzCTg#5t|Bu*oexr?i_inEF{ zPaqdLi-#$_;O=deu>*~1hLtnSb1+iMeixpPf-qOqOPGrOcCXE~- zUIN(QKwQaqUHSb99#j{nE)pJE__=;G?hNiIXjp}{qx&Xuq>EOgkTbE2u97i*0dhQCM36-;V;cLIXGzt~ z#wN5f0ZcM&F0F^^KapBd1p&?;Euiz%T1#!tkkgFmZ-kTTanqp#PYTRJ0iiwycM>7 zs2E{FG1Lw{j$A5)*el*_eRwwRgc^?!pV+?sZUbKOQd-A<2xojhxPWhhjUhrjEgMMc z(UO&6n(mL1QkH@DXWfiTJx}VZz>QVBJyXytWKq9F(-SS?6A`6kSFBx$x(L#V12P%F zdVqis3&|4-`IE}f7X4OEa%)y3m<_g-OkVuNgX(Nm+R({xKBfa4Tx|Cw)!hA!mq@rH zzEmYjM3p}FqS+Oix2rA-{sXM^f;BXfZS^`8+zZx2h+p?{u1H_+3hT3%rx7#X{B3;> z_H)U@e?lC;Casyvnwn!>sWwu*HXn*6AtC}i9FK+0+3>XGQLfYr^=yRi4>{K5L?sDP|892BVZ=XJ$&p1&Mq4b#&;&TEv!mUQ{Ny{ zU&6^$!9vHAmw3y2b<~0qc11PZp+7{y{RFCD1HNv;62k`zLs4M5iS6RW)RCF~0NucPbF2w8mXIdX!S+#Z@(PvaXPT+xAoFVQJ{=wQ9vJ3o28b;_liBY)*< zIuZVH61wg!-O-;uBVcp7YNvIWw=Vj_ZLy43jT=Jd7YY!@?e+Oa$euog<`4QJ(01@L zn>waoyYg;9gh{oG7i(npF%K;*5fnI* zof-e0qbWMwYU^`}yVArjO6BbPZU2wS)wm#mNNH;I2%xbnX#AT7X%lTxe*(0C@;5BQ zoUrB-VGVz1ZruJmSs%=2Od@LAeudE3KAW72GLo;gWsFYL&w0~?-pvx};MI}KAuj_x+Sxyu>2Kr2Z zyrp(KPWEJzAHZHij}CmUYECxj*BTM}jDu;4z%nloM!pq9C-S$qZc?4U$(<9In>~u= z>mliH%N)J^St=a(u9fEL7Ed?dh|#F4d$G0V5Yjq~*KGr@+ECruudU8oiLA8?sZ~vm zHkJ{{RIQf_b&e2tJuhQkyv8~#rsSHnYksAMx_;|ZjL@4vl&gjf(iBm#v#_jA`*Be@ zkS5q$iRpV^)Al^#w5ii*SpI9$0M^@pneO$30v}QJ{v|ec7ade~ggf$pArbx6vg(ei z7qM$23tHuqk-)le5)^4L2QY?=V#MgBrqIR<5xNF6KLV--;d7)F7pdt4`uvievcOg< zSO8xDZdc>Prk$mYi z)o<8_4a=zM1>?y>W#Jc+sNMy#(n>|a7fi|QbJka)_EXJFFs!P|{T}a--obROjyG19 zy-^c-QrcTIGwpD+EG6s96nbM#a0Th&%c6F)O>OuLjM42CFMXuN^h|_-7cNIM^cV0% zJV=K2`jRqY^d3x)wHuML(t+6(cXN+b_DrpIZiWhO>P=}sGw%h_)a^K2^Cvp`#IPBK zrB%v?P)AjSIWN)^wW~38kfFL@#V{hcO^YD}bE;J^wCNJH5w}|GUG^&rK5<%D&uqeV z+JB&k2C8%}W11=c$x~NjFkF^qt3^fmF8F@IAd51br`sDSUhV6jltAkH3;#X?bf~90 zEH72lVp1cno>QL|IEj+6OFy+#HJDOYWxenE zYBttInI}E`S77TYtYGbsG|7_sQz;|QH}{I3B1x~nBg1*#a=we$S)zQUdB=Xqx#l`F zSlB;8Zbq>3sJgPLuLlBT6Wb!*+C}t~GDT}!r4i0ie=Ws=*Bx1cwf)GvzMox^@v4<^ zgl`mTn%f`NkzsweVgFFTxEP(x1%>J5Y_)BRe?R^*85Bhr=p;8mLI?iuv4z~;sEPY=jh? zd$Z;AJCHK2)#>ihRla9!daSjR%VX0wb8KwjGK<$H>3$c6CC8a!ALtcJ)AW*TGTYus z)W#aX+!~F@06b$ZLCipGcwgC=rH5ZB9B;gacRv98{L7W1!+Y2J1A*5?)akg1F3PO{ z4f(_cdobjiPw26FpL%L~Mit9FoY}NnaW%!S4ifG4kKLY)#D*kkrI`=;sbU6Of;C-{ z5nq$zO%y`)#B}rz`PN>KhN&_X&8Go1kNru!=ytA^pa#yfB}|EKnw0Uso*`ZCLgVx8 z;^oUyD@oH>+af^iL{q_ zOfpr&!=(x`XaU7%4Fbe2W9W>WS6OG@%?pI!qsPydIUSow(cdoDvX0FBBMW5_Lhv&n zR9-wPJIQ$_mfJe|9Hc0Fir5U;FmW0MfyPL?+wVCBV*7lVps&#Z#Z#>rIWFbW`X;bt z4y?DmxGFo7!i}__Tc4?O^y5_&gWuzWiGJ;-2XywM&+%vY&Wdcuh4isw)kQn{O&ap{ z|F)OLT&wOkaYHElwl-V+&*faBR|OyN58sM9(Z_%J%9e1ord$gP8T-6BX%{;g_7(d! z;yXkwAleB@>OG)0aK-)juqJe6MR-N_mn!!Hd*YWpQ;(26??zQcWSHL- zWpTQ00g_MjQUZ(4`FUndt;47oyvkriiNkX5;T_hfC=@8v!aETjh+<{B))U{B;^S$I zl{Y9A6BEL}Zg=b#srH#~VY=OBn|{eDNM#EtkeTS4#i$`V?~wOwcCh}0G&E$Frfx_F z|M0h9+q|5kyxXR$d<99ndDv+G#I~dbVm^`xw(Fc|p-}RS7wOg?0MdbgRQh=IA~(Z` zgB8nI+csG`@2@(;JWy+;YV6?JTt8vg^32aIeqA%WXWL@pJlsp4Z32ymuKTbMZ;hQ~ z5k<_lDpht{<-JKDCYMZN5|P~V4}fIPs}8B7?n+h*5NatNGmWqp-I$aftiNBZ(5=?0 z)omN~`AR_j#mBmM?x-8>N)S7`yN99{K~bx2Gpa*^SoQcy>Ac(_bLzUVY$Kd?>;!N9)4^!%rG8$ggbsszCik_ryq&eprj)`;Jjt@m{gM>cDjHI3SBT zrq}na#k&VAP~5HWVm8a2Bz4}EYReJJ+0s8= zLgb$E11>)KS>3kZBr1Foyz?m*h$`xU^<@W6TSuM+SC$aE>ohpyl+$5<8o0mvS_{hr zf!Gpzv$kItYeNDF|2 z>L8CJ6GGAZgK_?{qt`T>hB(JGP}mWW0H$J}=(k*hne>sOLqkd=LG0#V7`9tZ2#t{8 zxMnKnq6E@LW9(%U7ILHj+H#a;szyu0WcbXmz-{;+KPz=-f$p1ttooYT1Pv*5IgGLG zYiNQOD-@y@<2Yo#Oh1d5CnM;x@^=k(M-uhnW{pNv8c4CaYnVPJ^$#Kx)DX(uWRh!J z_`zr~k49sqP?Cp0i;Eot-{tkLF@MgKukk!lxQxcvTQaJcZb8c{C~0d(MWko&LkmX5 z*k|y=N4tnA|1{!y`$oB%a7;_8@hW=J^ff#Y8kAD^gt6s6%DmZ|%?F0m`*}ZqF#UR0 zfuVakRY}tv!!O*aJignI`R=`Q>Q;i7kAcaghU?w9V2RSO=!$umH1y3{|EO>JFV$U5 zJQ{(Q`%>=+j9%5w`JjQ(q8A>+(XvQKXygb%({)*mK=%W^A6)Wku2G=Lt!ZJeFMNjs z(#1OA3k7&USKhL#@hAL#~ExcXb6!W#Ehju1E_0jn5WRj=(tKmxhs6S}_&0ANf zwrfF~h~vl#pVyAMXmwE*9fn88V#SMh#R<{^w&3|<4gTKOT2@E$6+$q5Pn9HP> z)dm)ow!cm>#{&|T6+je5DrkE`=n|$Sk#r(E>BtX0Pz5kms$`;jRj33?*%>|g0Y+?s zaNpa=Uz6xaL(P`bXfsm%N;j6Hvq|VDM)VHd;1v0oZ;Oq`7$}Pd?a-5-URLQCXfz;R z0rb9n^!4$^296Do8+@1jxOs5u6pbqp$31)WVcYT6wKENux_5)L0$WiMhen@+MjkYr z@720fXR_1!R~Q*xsunf8O4$(X8uQ9zxbi=zhGq(FtGz$-43f2q*@8qPcwj}1CF)*1 zQ9ixo7G*hKMr*0A3Ues*S%qN8{3!`=4C6m%N}UxOm9a4yBLN8J6@eClEw<4L44_NPcD58jRfhkQL2kkS&q z$S#Xhou5x*6!edsAkZts#_k9zRkgn*uZ{{i-R;!C516t)K=;x(=j)*b|BEx*(d*`H z@@;pR7)_enFNO@`agZxsnbp&u;V4M5g0d=b63&P*Db$lu)M@s21pM2T+$#Ir@0|s# zLOcQCWwK2bKILm(+2#_DG$xE>pX!r@^>CkSr}mMUVG?10*PK#<4pbQqvh|k9gV{J9zUw0 z*1&;yL2``%I}+RwcIC0%Ea&@61iW=s=BC`%P4L}9H)-eD{$QTm#eY~!C<mkR>LYR!`s%Mm*b4^ayBo(km9X`UmFGP81sl@-XDvFcdb9P z2utE(%i_MS6?go1*07nXjq;<_=4S>9%CW0%Gi16z?|S}1$qhDN^bo|V_F76Jy8&(Y z02`)a7Dr)7N2Scqn`o9LLcg=DX9WRQ=+fivTuKFAy&%fF#0b2%0d;r;pjW_g_7&F~ zja5}wZDDD*13;?P9CJdt_c{b{kxKP%R!?CaBtvJy~x`x!zfT;-v` z>EQx{wE(KXzvkK-zjb3MO?V-5e|m$-L%pu@{Jw}O;L!ULz26P=SPIvacrn#AYo#~G zhi=LzhVOXyG@d+f_&SF#gZU5`nQe6?xZ#QTT7$#?`nO&CS~r470WI9-Ef@fx3;_VZ z0e%3K#e_uV#2B32o&U9LAJc$UejEOel8~CRj{Q2*ci)-Xmt@L#BIm7~c=x=jv-rF` zp-xN21*tt0nOOWad7>|0O8LkRDy;2kHy*9(H~5k5?K++=Lnn71zfX^K0)13=btUIn zl;jk}BXG*BByr=!j0zTQ7l)iGEH24oUUrwM{U(St=`$J4sbW0yp6?c7)U4zsL>9lq zh*M4y>|1pbr4di#wS=P64J(xruHsP_o`%$SXi|h}P}*XUn!!X=h9-Hv++Wb*DV|9~ z64T}tlBl?05cWn45LpU!jDiT$w6wfnYVOIcUmE$KEhngcj(!>{)Bn0^eWAEmIo^7+ zq#H<~PYD8Ab(N1!M1OBYM))KR)mFk1#{N!Bsi1o#Bkt|7(XbERGUquO83FySRrV`z z?hbi8+LM42G0W|$4+J^XevL}$4`;GRl? zcRx=2hr}HA2sRPz?6~6empqD=rvW2YQ~*8m6N||D3yO)1vJxR4*Fiy^_^bYP9;*p< z5#<*^Q3ed05aNI8mth0`IbZ+)peW#<*MD!DLH(b$*?()A{X6qNW1oL#b|V1(Jp_UR u{3`_dUvU4a?0?5mlL7u8okIcsrSt#bK|%jB3 Date: Mon, 17 Feb 2014 21:15:35 +0100 Subject: [PATCH 093/154] Cleanup: Moved sdlog2 file conversion scripts to separate folder. --- Tools/{ => sdlog2}/README.txt | 0 Tools/{ => sdlog2}/logconv.m | 1070 ++++++++++++++--------------- Tools/{ => sdlog2}/sdlog2_dump.py | 0 3 files changed, 535 insertions(+), 535 deletions(-) rename Tools/{ => sdlog2}/README.txt (100%) rename Tools/{ => sdlog2}/logconv.m (97%) rename Tools/{ => sdlog2}/sdlog2_dump.py (100%) diff --git a/Tools/README.txt b/Tools/sdlog2/README.txt similarity index 100% rename from Tools/README.txt rename to Tools/sdlog2/README.txt diff --git a/Tools/logconv.m b/Tools/sdlog2/logconv.m similarity index 97% rename from Tools/logconv.m rename to Tools/sdlog2/logconv.m index c416b8095e..e19c97fa3c 100644 --- a/Tools/logconv.m +++ b/Tools/sdlog2/logconv.m @@ -1,535 +1,535 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -%% ************************************************************************ -% PX4LOG_PLOTSCRIPT: Main function -% ************************************************************************ -function PX4Log_Plotscript - -% Clear everything -clc -clear all -close all - -% ************************************************************************ -% SETTINGS -% ************************************************************************ - -% Set the path to your sysvector.bin file here -filePath = 'log001.bin'; - -% Set the minimum and maximum times to plot here [in seconds] -mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] -maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] - -%Determine which data to plot. Not completely implemented yet. -bDisplayGPS=true; - -%conversion factors -fconv_gpsalt=1; %[mm] to [m] -fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg] -fconv_timestamp=1E-6; % [microseconds] to [seconds] - -% ************************************************************************ -% Import the PX4 logs -% ************************************************************************ -ImportPX4LogData(); - -%Translate min and max plot times to indices -time=double(sysvector.TIME_StartTime) .*fconv_timestamp; -mintime_log=time(1); %The minimum time/timestamp found in the log -maxtime_log=time(end); %The maximum time/timestamp found in the log -CurTime=mintime_log; %The current time at which to draw the aircraft position - -[imintime,imaxtime]=FindMinMaxTimeIndices(); - -% ************************************************************************ -% PLOT & GUI SETUP -% ************************************************************************ -NrFigures=5; -NrAxes=10; -h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively -h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively -h.pathpoints=[]; % Temporary initiliazation of path points - -% Setup the GUI to control the plots -InitControlGUI(); -% Setup the plotting-GUI (figures, axes) itself. -InitPlotGUI(); - -% ************************************************************************ -% DRAW EVERYTHING -% ************************************************************************ -DrawRawData(); -DrawCurrentAircraftState(); - -%% ************************************************************************ -% *** END OF MAIN SCRIPT *** -% NESTED FUNCTION DEFINTIONS FROM HERE ON -% ************************************************************************ - - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -function ImportPX4LogData() - - % ************************************************************************ - % RETRIEVE SYSTEM VECTOR - % ************************************************************************* - % //All measurements in NED frame - - % Convert to CSV - %arg1 = 'log-fx61-20130721-2.bin'; - arg1 = filePath; - delim = ','; - time_field = 'TIME'; - data_file = 'data.csv'; - csv_null = ''; - - if not(exist(data_file, 'file')) - s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) ); - end - - if exist(data_file, 'file') - - %data = csvread(data_file); - sysvector = tdfread(data_file, ','); - - % shot the flight time - time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1); - time_s = uint64(time_us*1e-6); - time_m = uint64(time_s/60); - time_s = time_s - time_m * 60; - - disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]); - - disp(['logfile conversion finished.' char(10)]); - else - disp(['file: ' data_file ' does not exist' char(10)]); - end -end - -%% ************************************************************************ -% INITCONTROLGUI (nested function) -% ************************************************************************ -%Setup central control GUI components to control current time where data is shown -function InitControlGUI() - %********************************************************************** - % GUI size definitions - %********************************************************************** - dxy=5; %margins - %Panel: Plotctrl - dlabels=120; - dsliders=200; - dedits=80; - hslider=20; - - hpanel1=40; %panel1 - hpanel2=220;%panel2 - hpanel3=3*hslider+4*dxy+3*dxy;%panel3. - - width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width - height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height - - %********************************************************************** - % Create GUI - %********************************************************************** - h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); - h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); - h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); - h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); - - %%Control GUI-elements - %Slider: Current time - h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... - 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); - temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); - set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); - h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... - 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); - - %Slider: MaxTime - h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %Slider: MinTime - h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %%Current data/state GUI-elements (Multiline-edit-box) - h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... - 'HorizontalAlignment','left','parent',h.aircraftstatepanel); - - h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... - 'HorizontalAlignment','left','parent',h.guistatepanel); - -end - -%% ************************************************************************ -% INITPLOTGUI (nested function) -% ************************************************************************ -function InitPlotGUI() - - % Setup handles to lines and text - h.markertext=[]; - templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array - h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively - h.markerline(1:NrAxes)=0.0; - - % Setup all other figures and axes for plotting - % PLOT WINDOW 1: GPS POSITION - h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); - h.axes(1)=axes(); - set(h.axes(1),'Parent',h.figures(2)); - - % PLOT WINDOW 2: IMU, baro altitude - h.figures(3)=figure('Name', 'IMU / Baro Altitude'); - h.axes(2)=subplot(4,1,1); - h.axes(3)=subplot(4,1,2); - h.axes(4)=subplot(4,1,3); - h.axes(5)=subplot(4,1,4); - set(h.axes(2:5),'Parent',h.figures(3)); - - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); - h.axes(6)=subplot(4,1,1); - h.axes(7)=subplot(4,1,2); - h.axes(8)=subplot(4,1,3); - h.axes(9)=subplot(4,1,4); - set(h.axes(6:9),'Parent',h.figures(4)); - - % PLOT WINDOW 4: LOG STATS - h.figures(5) = figure('Name', 'Log Statistics'); - h.axes(10)=subplot(1,1,1); - set(h.axes(10:10),'Parent',h.figures(5)); - -end - -%% ************************************************************************ -% DRAWRAWDATA (nested function) -% ************************************************************************ -%Draws the raw data from the sysvector, but does not add any -%marker-lines or so -function DrawRawData() - % ************************************************************************ - % PLOT WINDOW 1: GPS POSITION & GUI - % ************************************************************************ - figure(h.figures(2)); - % Only plot GPS data if available - if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS) - %Draw data - plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ... - double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ... - double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.'); - title(h.axes(1),'GPS Position Data(if available)'); - xlabel(h.axes(1),'Latitude [deg]'); - ylabel(h.axes(1),'Longitude [deg]'); - zlabel(h.axes(1),'Altitude above MSL [m]'); - grid on - - %Reset path - h.pathpoints=0; - end - - % ************************************************************************ - % PLOT WINDOW 2: IMU, baro altitude - % ************************************************************************ - figure(h.figures(3)); - plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]); - title(h.axes(2),'Magnetometers [Gauss]'); - legend(h.axes(2),'x','y','z'); - plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]); - title(h.axes(3),'Accelerometers [m/s²]'); - legend(h.axes(3),'x','y','z'); - plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]); - title(h.axes(4),'Gyroscopes [rad/s]'); - legend(h.axes(4),'x','y','z'); - plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue'); - if(bDisplayGPS) - hold on; - plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red'); - hold off - legend('Barometric Altitude [m]','GPS Altitude [m]'); - else - legend('Barometric Altitude [m]'); - end - title(h.axes(5),'Altitude above MSL [m]'); - - % ************************************************************************ - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - % ************************************************************************ - figure(h.figures(4)); - %Attitude Estimate - plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159); - title(h.axes(6),'Estimated attitude [deg]'); - legend(h.axes(6),'roll','pitch','yaw'); - %Actuator Controls - plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]); - title(h.axes(7),'Actuator control [-]'); - legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]'); - %Actuator Controls - plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]); - title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); - legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); - set(h.axes(8), 'YLim',[800 2200]); - %Airspeeds - plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime)); - hold on - plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime)); - hold off - %add GPS total airspeed here - title(h.axes(9),'Airspeed [m/s]'); - legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); - %calculate time differences and plot them - intervals = zeros(0,imaxtime - imintime); - for k = imintime+1:imaxtime - intervals(k) = time(k) - time(k-1); - end - plot(h.axes(10), time(imintime:imaxtime), intervals); - - %Set same timescale for all plots - for i=2:NrAxes - set(h.axes(i),'XLim',[mintime maxtime]); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% DRAWCURRENTAIRCRAFTSTATE(nested function) -% ************************************************************************ -function DrawCurrentAircraftState() - %find current data index - i=find(time>=CurTime,1,'first'); - - %********************************************************************** - % Current aircraft state label update - %********************************************************************** - acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',... - 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',... - 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; - acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),... - ', y=',num2str(sysvector.IMU_MagY(i)),... - ', z=',num2str(sysvector.IMU_MagZ(i)),']']; - acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),... - ', y=',num2str(sysvector.IMU_AccY(i)),... - ', z=',num2str(sysvector.IMU_AccZ(i)),']']; - acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),... - ', y=',num2str(sysvector.IMU_GyroY(i)),... - ', z=',num2str(sysvector.IMU_GyroZ(i)),']']; - acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; - acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),... - ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),... - ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']']; - acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); - %for j=1:4 - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),',']; - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),',']; - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),',']; - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),',']; - %end - acstate{7,:}=[acstate{7,:},']']; - acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); - %for j=1:8 - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),',']; - %end - acstate{8,:}=[acstate{8,:},']']; - acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']']; - - set(h.edits.AircraftState,'String',acstate); - - %********************************************************************** - % GPS Plot Update - %********************************************************************** - %Plot traveled path, and and time. - figure(h.figures(2)); - hold on; - if(CurTime>mintime+1) %the +1 is only a small bugfix - h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ... - double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ... - double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2); - end; - hold off - %Plot current position - newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt]; - if(numel(h.pathpoints)<=3) %empty path - h.pathpoints(1,1:3)=newpoint; - else %Not empty, append new point - h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; - end - axes(h.axes(1)); - line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); - - - % Plot current time (small label next to current gps position) - textdesc=strcat(' t=',num2str(time(i)),'s'); - if(isvalidhandle(h.markertext)) - delete(h.markertext); %delete old text - end - h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,... - double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc); - set(h.edits.CurTime,'String',CurTime); - - %********************************************************************** - % Plot the lines showing the current time in the 2-d plots - %********************************************************************** - for i=2:NrAxes - if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end - ylims=get(h.axes(i),'YLim'); - h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); - set(h.markerline(i),'parent',h.axes(i)); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% MINMAXTIME CALLBACK (nested function) -% ************************************************************************ -function minmaxtime_callback(hObj,event) %#ok - new_mintime=get(h.sliders.MinTime,'Value'); - new_maxtime=get(h.sliders.MaxTime,'Value'); - - %Safety checks: - bErr=false; - %1: mintime must be < maxtime - if((new_mintime>maxtime) || (new_maxtimeCurTime) - set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); - mintime=new_mintime; - CurTime=mintime; - bErr=true; - end - %3: MaxTime must be >CurTime - if(new_maxtime - %find current time - if(hObj==h.sliders.CurTime) - CurTime=get(h.sliders.CurTime,'Value'); - elseif (hObj==h.edits.CurTime) - temp=str2num(get(h.edits.CurTime,'String')); - if(tempmintime) - CurTime=temp; - else - %Error - set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); - end - else - %Error - set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); - end - - set(h.sliders.CurTime,'Value',CurTime); - set(h.edits.CurTime,'String',num2str(CurTime)); - - %Redraw time markers, but don't have to redraw the whole raw data - DrawCurrentAircraftState(); -end - -%% ************************************************************************ -% FINDMINMAXINDICES (nested function) -% ************************************************************************ -function [idxmin,idxmax] = FindMinMaxTimeIndices() - for i=1:size(sysvector.TIME_StartTime,1) - if time(i)>=mintime; idxmin=i; break; end - end - for i=1:size(sysvector.TIME_StartTime,1) - if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end - if time(i)>=maxtime; idxmax=i; break; end - end - mintime=time(idxmin); - maxtime=time(idxmax); -end - -%% ************************************************************************ -% ISVALIDHANDLE (nested function) -% ************************************************************************ -function isvalid = isvalidhandle(handle) - if(exist(varname(handle))>0 && length(ishandle(handle))>0) - if(ishandle(handle)>0) - if(handle>0.0) - isvalid=true; - return; - end - end - end - isvalid=false; -end - -%% ************************************************************************ -% JUST SOME SMALL HELPER FUNCTIONS (nested function) -% ************************************************************************ -function out = varname(var) - out = inputname(1); -end - -%This is the end of the matlab file / the main function -end +% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +%% ************************************************************************ +% PX4LOG_PLOTSCRIPT: Main function +% ************************************************************************ +function PX4Log_Plotscript + +% Clear everything +clc +clear all +close all + +% ************************************************************************ +% SETTINGS +% ************************************************************************ + +% Set the path to your sysvector.bin file here +filePath = 'log001.bin'; + +% Set the minimum and maximum times to plot here [in seconds] +mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] +maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] + +%Determine which data to plot. Not completely implemented yet. +bDisplayGPS=true; + +%conversion factors +fconv_gpsalt=1; %[mm] to [m] +fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg] +fconv_timestamp=1E-6; % [microseconds] to [seconds] + +% ************************************************************************ +% Import the PX4 logs +% ************************************************************************ +ImportPX4LogData(); + +%Translate min and max plot times to indices +time=double(sysvector.TIME_StartTime) .*fconv_timestamp; +mintime_log=time(1); %The minimum time/timestamp found in the log +maxtime_log=time(end); %The maximum time/timestamp found in the log +CurTime=mintime_log; %The current time at which to draw the aircraft position + +[imintime,imaxtime]=FindMinMaxTimeIndices(); + +% ************************************************************************ +% PLOT & GUI SETUP +% ************************************************************************ +NrFigures=5; +NrAxes=10; +h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively +h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively +h.pathpoints=[]; % Temporary initiliazation of path points + +% Setup the GUI to control the plots +InitControlGUI(); +% Setup the plotting-GUI (figures, axes) itself. +InitPlotGUI(); + +% ************************************************************************ +% DRAW EVERYTHING +% ************************************************************************ +DrawRawData(); +DrawCurrentAircraftState(); + +%% ************************************************************************ +% *** END OF MAIN SCRIPT *** +% NESTED FUNCTION DEFINTIONS FROM HERE ON +% ************************************************************************ + + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +function ImportPX4LogData() + + % ************************************************************************ + % RETRIEVE SYSTEM VECTOR + % ************************************************************************* + % //All measurements in NED frame + + % Convert to CSV + %arg1 = 'log-fx61-20130721-2.bin'; + arg1 = filePath; + delim = ','; + time_field = 'TIME'; + data_file = 'data.csv'; + csv_null = ''; + + if not(exist(data_file, 'file')) + s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) ); + end + + if exist(data_file, 'file') + + %data = csvread(data_file); + sysvector = tdfread(data_file, ','); + + % shot the flight time + time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1); + time_s = uint64(time_us*1e-6); + time_m = uint64(time_s/60); + time_s = time_s - time_m * 60; + + disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]); + + disp(['logfile conversion finished.' char(10)]); + else + disp(['file: ' data_file ' does not exist' char(10)]); + end +end + +%% ************************************************************************ +% INITCONTROLGUI (nested function) +% ************************************************************************ +%Setup central control GUI components to control current time where data is shown +function InitControlGUI() + %********************************************************************** + % GUI size definitions + %********************************************************************** + dxy=5; %margins + %Panel: Plotctrl + dlabels=120; + dsliders=200; + dedits=80; + hslider=20; + + hpanel1=40; %panel1 + hpanel2=220;%panel2 + hpanel3=3*hslider+4*dxy+3*dxy;%panel3. + + width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width + height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height + + %********************************************************************** + % Create GUI + %********************************************************************** + h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); + h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); + h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); + h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); + + %%Control GUI-elements + %Slider: Current time + h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... + 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); + temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); + set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); + h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... + 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); + + %Slider: MaxTime + h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %Slider: MinTime + h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %%Current data/state GUI-elements (Multiline-edit-box) + h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... + 'HorizontalAlignment','left','parent',h.aircraftstatepanel); + + h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... + 'HorizontalAlignment','left','parent',h.guistatepanel); + +end + +%% ************************************************************************ +% INITPLOTGUI (nested function) +% ************************************************************************ +function InitPlotGUI() + + % Setup handles to lines and text + h.markertext=[]; + templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array + h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively + h.markerline(1:NrAxes)=0.0; + + % Setup all other figures and axes for plotting + % PLOT WINDOW 1: GPS POSITION + h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); + h.axes(1)=axes(); + set(h.axes(1),'Parent',h.figures(2)); + + % PLOT WINDOW 2: IMU, baro altitude + h.figures(3)=figure('Name', 'IMU / Baro Altitude'); + h.axes(2)=subplot(4,1,1); + h.axes(3)=subplot(4,1,2); + h.axes(4)=subplot(4,1,3); + h.axes(5)=subplot(4,1,4); + set(h.axes(2:5),'Parent',h.figures(3)); + + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); + h.axes(6)=subplot(4,1,1); + h.axes(7)=subplot(4,1,2); + h.axes(8)=subplot(4,1,3); + h.axes(9)=subplot(4,1,4); + set(h.axes(6:9),'Parent',h.figures(4)); + + % PLOT WINDOW 4: LOG STATS + h.figures(5) = figure('Name', 'Log Statistics'); + h.axes(10)=subplot(1,1,1); + set(h.axes(10:10),'Parent',h.figures(5)); + +end + +%% ************************************************************************ +% DRAWRAWDATA (nested function) +% ************************************************************************ +%Draws the raw data from the sysvector, but does not add any +%marker-lines or so +function DrawRawData() + % ************************************************************************ + % PLOT WINDOW 1: GPS POSITION & GUI + % ************************************************************************ + figure(h.figures(2)); + % Only plot GPS data if available + if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS) + %Draw data + plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ... + double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ... + double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.'); + title(h.axes(1),'GPS Position Data(if available)'); + xlabel(h.axes(1),'Latitude [deg]'); + ylabel(h.axes(1),'Longitude [deg]'); + zlabel(h.axes(1),'Altitude above MSL [m]'); + grid on + + %Reset path + h.pathpoints=0; + end + + % ************************************************************************ + % PLOT WINDOW 2: IMU, baro altitude + % ************************************************************************ + figure(h.figures(3)); + plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]); + title(h.axes(2),'Magnetometers [Gauss]'); + legend(h.axes(2),'x','y','z'); + plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]); + title(h.axes(3),'Accelerometers [m/s²]'); + legend(h.axes(3),'x','y','z'); + plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]); + title(h.axes(4),'Gyroscopes [rad/s]'); + legend(h.axes(4),'x','y','z'); + plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue'); + if(bDisplayGPS) + hold on; + plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red'); + hold off + legend('Barometric Altitude [m]','GPS Altitude [m]'); + else + legend('Barometric Altitude [m]'); + end + title(h.axes(5),'Altitude above MSL [m]'); + + % ************************************************************************ + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + % ************************************************************************ + figure(h.figures(4)); + %Attitude Estimate + plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159); + title(h.axes(6),'Estimated attitude [deg]'); + legend(h.axes(6),'roll','pitch','yaw'); + %Actuator Controls + plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]); + title(h.axes(7),'Actuator control [-]'); + legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]'); + %Actuator Controls + plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]); + title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); + legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); + set(h.axes(8), 'YLim',[800 2200]); + %Airspeeds + plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime)); + hold on + plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime)); + hold off + %add GPS total airspeed here + title(h.axes(9),'Airspeed [m/s]'); + legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); + %calculate time differences and plot them + intervals = zeros(0,imaxtime - imintime); + for k = imintime+1:imaxtime + intervals(k) = time(k) - time(k-1); + end + plot(h.axes(10), time(imintime:imaxtime), intervals); + + %Set same timescale for all plots + for i=2:NrAxes + set(h.axes(i),'XLim',[mintime maxtime]); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% DRAWCURRENTAIRCRAFTSTATE(nested function) +% ************************************************************************ +function DrawCurrentAircraftState() + %find current data index + i=find(time>=CurTime,1,'first'); + + %********************************************************************** + % Current aircraft state label update + %********************************************************************** + acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',... + 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',... + 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; + acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),... + ', y=',num2str(sysvector.IMU_MagY(i)),... + ', z=',num2str(sysvector.IMU_MagZ(i)),']']; + acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),... + ', y=',num2str(sysvector.IMU_AccY(i)),... + ', z=',num2str(sysvector.IMU_AccZ(i)),']']; + acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),... + ', y=',num2str(sysvector.IMU_GyroY(i)),... + ', z=',num2str(sysvector.IMU_GyroZ(i)),']']; + acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; + acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),... + ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),... + ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']']; + acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); + %for j=1:4 + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),',']; + %end + acstate{7,:}=[acstate{7,:},']']; + acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); + %for j=1:8 + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),',']; + %end + acstate{8,:}=[acstate{8,:},']']; + acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']']; + + set(h.edits.AircraftState,'String',acstate); + + %********************************************************************** + % GPS Plot Update + %********************************************************************** + %Plot traveled path, and and time. + figure(h.figures(2)); + hold on; + if(CurTime>mintime+1) %the +1 is only a small bugfix + h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ... + double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ... + double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2); + end; + hold off + %Plot current position + newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt]; + if(numel(h.pathpoints)<=3) %empty path + h.pathpoints(1,1:3)=newpoint; + else %Not empty, append new point + h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; + end + axes(h.axes(1)); + line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); + + + % Plot current time (small label next to current gps position) + textdesc=strcat(' t=',num2str(time(i)),'s'); + if(isvalidhandle(h.markertext)) + delete(h.markertext); %delete old text + end + h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,... + double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc); + set(h.edits.CurTime,'String',CurTime); + + %********************************************************************** + % Plot the lines showing the current time in the 2-d plots + %********************************************************************** + for i=2:NrAxes + if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end + ylims=get(h.axes(i),'YLim'); + h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); + set(h.markerline(i),'parent',h.axes(i)); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% MINMAXTIME CALLBACK (nested function) +% ************************************************************************ +function minmaxtime_callback(hObj,event) %#ok + new_mintime=get(h.sliders.MinTime,'Value'); + new_maxtime=get(h.sliders.MaxTime,'Value'); + + %Safety checks: + bErr=false; + %1: mintime must be < maxtime + if((new_mintime>maxtime) || (new_maxtimeCurTime) + set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); + mintime=new_mintime; + CurTime=mintime; + bErr=true; + end + %3: MaxTime must be >CurTime + if(new_maxtime + %find current time + if(hObj==h.sliders.CurTime) + CurTime=get(h.sliders.CurTime,'Value'); + elseif (hObj==h.edits.CurTime) + temp=str2num(get(h.edits.CurTime,'String')); + if(tempmintime) + CurTime=temp; + else + %Error + set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); + end + else + %Error + set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); + end + + set(h.sliders.CurTime,'Value',CurTime); + set(h.edits.CurTime,'String',num2str(CurTime)); + + %Redraw time markers, but don't have to redraw the whole raw data + DrawCurrentAircraftState(); +end + +%% ************************************************************************ +% FINDMINMAXINDICES (nested function) +% ************************************************************************ +function [idxmin,idxmax] = FindMinMaxTimeIndices() + for i=1:size(sysvector.TIME_StartTime,1) + if time(i)>=mintime; idxmin=i; break; end + end + for i=1:size(sysvector.TIME_StartTime,1) + if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end + if time(i)>=maxtime; idxmax=i; break; end + end + mintime=time(idxmin); + maxtime=time(idxmax); +end + +%% ************************************************************************ +% ISVALIDHANDLE (nested function) +% ************************************************************************ +function isvalid = isvalidhandle(handle) + if(exist(varname(handle))>0 && length(ishandle(handle))>0) + if(ishandle(handle)>0) + if(handle>0.0) + isvalid=true; + return; + end + end + end + isvalid=false; +end + +%% ************************************************************************ +% JUST SOME SMALL HELPER FUNCTIONS (nested function) +% ************************************************************************ +function out = varname(var) + out = inputname(1); +end + +%This is the end of the matlab file / the main function +end diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py similarity index 100% rename from Tools/sdlog2_dump.py rename to Tools/sdlog2/sdlog2_dump.py From a8b3381ca9675890b0d36e98a4453ac18d19df3e Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 17 Feb 2014 21:29:14 +0100 Subject: [PATCH 094/154] BL-Ctrl 3.0 fix --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index ec5f77d747..705e98eea4 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; foundMotorCount++; - if (Motor[i].MaxPWM == 250) { + if ((Motor[i].MaxPWM & 252) == 248) { Motor[i].Version = BLCTRL_NEW; } else { From 4cfaf928e12ac8927b3980506f31c05a1ab899ce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 16:11:46 +0400 Subject: [PATCH 095/154] px4io: bug in failsafe fixed --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec6..6a44294619 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,7 +179,7 @@ mixer_tick(void) ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); From 4b519e4d5daee4da8e619414d4b1cd697198b6a3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:04:11 +0100 Subject: [PATCH 096/154] Navigator: Get rid of warnings first --- src/modules/navigator/navigator_main.cpp | 46 ++++++++++++------------ 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 577c767a86..f59663ca00 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -154,17 +154,16 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ - bool _mission_item_valid; /**< current mission item valid */ + struct mission_item_s _mission_item; /**< current mission item */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -178,21 +177,22 @@ private: class Mission _mission; - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ + bool _mission_item_valid; /**< current mission item valid */ + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ bool _pos_sp_triplet_updated; - char *nav_states_str[NAV_STATE_MAX]; + const char *nav_states_str[NAV_STATE_MAX]; struct { float min_altitude; @@ -382,11 +382,11 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _control_mode_sub(-1), /* publications */ _pos_sp_triplet_pub(-1), @@ -395,22 +395,22 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), -/* states */ - _rtl_state(RTL_STATE_NONE), + _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _mission_item_valid(false), _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0), - _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _set_nav_state_timestamp(0), _pos_sp_triplet_updated(false), - _geofence_violation_warning_sent(false) +/* states */ + _rtl_state(RTL_STATE_NONE) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); @@ -1162,7 +1162,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); } else { if (onboard) { @@ -1318,7 +1318,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } @@ -1344,7 +1344,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1362,7 +1362,7 @@ Navigator::set_rtl_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; @@ -1371,7 +1371,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1525,7 +1525,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } } From 3eaa892ee42f7de84698d46ef8cf3b29b96a2b34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:35:41 +0100 Subject: [PATCH 097/154] fw_pos_control_l1: indentation only --- .../fw_pos_control_l1_main.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 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 45fdaa355e..10aa890885 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 @@ -130,23 +130,23 @@ private: int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _sensor_combined_sub; /**< for body frame accelerations */ + int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _control_mode; /**< vehicle status */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ - struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ + struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -192,7 +192,7 @@ private: uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures 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 + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -406,8 +406,8 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - position_setpoint_triplet_s _pos_sp_triplet = {0}; - sensor_combined_s _sensor_combined = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -587,8 +587,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7f; - _launch_lon = _global_pos.lon / 1e7f; + _launch_lat = _global_pos.lat / 1e7d; + _launch_lon = _global_pos.lon / 1e7d; _launch_alt = _global_pos.alt; _launch_valid = true; } From 83f66e45999c7db8734534ba1cfb9f9e023f47a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:40:11 +0100 Subject: [PATCH 098/154] fw_pos_control_l1: use double everywhere for lat and lon --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 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 10aa890885..c1aa8d39c8 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 @@ -154,13 +154,13 @@ private: /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; + double _loiter_hold_lat; + double _loiter_hold_lon; float _loiter_hold_alt; bool _loiter_hold; - float _launch_lat; - float _launch_lon; + double _launch_lat; + double _launch_lon; float _launch_alt; bool _launch_valid; @@ -587,8 +587,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7d; - _launch_lon = _global_pos.lon / 1e7d; + _launch_lat = _global_pos.lat; + _launch_lon = _global_pos.lon; _launch_alt = _global_pos.alt; _launch_valid = true; } From a43c7c488eaba5022a18f05e590059cad6f580da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 18:54:58 +0400 Subject: [PATCH 099/154] navigator: "reached" flags reset fixed --- src/modules/navigator/navigator_main.cpp | 39 +++++++++++++++++------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ed12d1062..af1b9aac02 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -321,6 +321,11 @@ private: */ bool onboard_mission_available(unsigned relative_index); + /** + * Reset all "reached" flags. + */ + void reset_reached(); + /** * Check if current mission item has been reached. */ @@ -855,9 +860,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - - /* reset time counter on state changes */ - _time_first_inside_orbit = 0; } perf_end(_loop_perf); @@ -1009,6 +1011,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { void Navigator::start_none() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -1024,6 +1028,8 @@ Navigator::start_none() void Navigator::start_ready() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; @@ -1046,6 +1052,8 @@ Navigator::start_ready() void Navigator::start_loiter() { + reset_reached(); + _do_takeoff = false; /* set loiter position if needed */ @@ -1091,6 +1099,8 @@ Navigator::start_mission() void Navigator::set_mission_item() { + reset_reached(); + /* copy current mission to previous item */ memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); @@ -1104,9 +1114,6 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - /* reset time counter for new item */ - _time_first_inside_orbit = 0; - _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1224,6 +1231,8 @@ Navigator::start_rtl() void Navigator::start_land() { + reset_reached(); + /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ _do_takeoff = false; @@ -1255,6 +1264,8 @@ Navigator::start_land() void Navigator::start_land_home() { + reset_reached(); + /* land to home position, should be called when hovering above home, from RTL state */ _do_takeoff = false; _reset_loiter_pos = true; @@ -1285,8 +1296,7 @@ Navigator::start_land_home() void Navigator::set_rtl_item() { - /*reset time counter for new RTL item */ - _time_first_inside_orbit = 0; + reset_reached(); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -1550,9 +1560,7 @@ Navigator::check_mission_item_reached() /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - _time_first_inside_orbit = 0; - _waypoint_yaw_reached = false; - _waypoint_position_reached = false; + reset_reached(); return true; } } @@ -1561,6 +1569,15 @@ Navigator::check_mission_item_reached() } +void +Navigator::reset_reached() +{ + _time_first_inside_orbit = 0; + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + +} + void Navigator::on_mission_item_reached() { From 7d8f08ad9a733111a9d8b1512a85592a2fb9b045 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 19:05:32 +0400 Subject: [PATCH 100/154] navigator: check if yaw reached only when position reached --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index af1b9aac02..4cd60d8d89 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1533,7 +1533,7 @@ Navigator::check_mission_item_reached() } } - if (!_waypoint_yaw_reached) { + if (_waypoint_position_reached && !_waypoint_yaw_reached) { if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); From 84d9e3479f3d2bb29b22906be0dfe65d73b5dd0f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 23:42:02 +0400 Subject: [PATCH 101/154] mc_pos_control: unused variables removed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 - 1 file changed, 1 deletion(-) 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 b06655385a..b4bac53d66 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -730,7 +730,6 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err; - float err_x, err_y; get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); pos_err(2) = -(_alt_sp - alt); From 79d2e5de6cf00186359e5b436c602055e4110249 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:48:31 +0100 Subject: [PATCH 102/154] TECS: use constant from geo --- src/lib/ecl/ecl.h | 3 +-- src/lib/external_lgpl/tecs/tecs.cpp | 5 +---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index e0f207696a..aa3c5000a4 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,7 +38,6 @@ */ #include -#include #define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file +#define ecl_elapsed_time hrt_elapsed_time diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0f28bccadd..3730b19204 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -3,13 +3,10 @@ #include "tecs.h" #include #include +#include using namespace math; -#ifndef CONSTANTS_ONE_G -#define CONSTANTS_ONE_G GRAVITY -#endif - /** * @file tecs.cpp * From 84df8ee78d28c51fc0f6272771d3b7c12b281cfe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:48:46 +0100 Subject: [PATCH 103/154] TECS: warning fixes --- src/lib/external_lgpl/tecs/tecs.h | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index d1ebacda16..5cafb1c791 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,16 +28,7 @@ class __EXPORT TECS { public: TECS() : - - _airspeed_enabled(false), - _throttle_slewrate(0.0f), - _climbOutDem(false), - _hgt_dem_prev(0.0f), - _hgt_dem_adj_last(0.0f), - _hgt_dem_in_old(0.0f), - _TAS_dem_last(0.0f), - _TAS_dem_adj(0.0f), - _TAS_dem(0.0f), + _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), _integ3_state(0.0f), @@ -45,8 +36,16 @@ public: _integ5_state(0.0f), _integ6_state(0.0f), _integ7_state(0.0f), - _pitch_dem(0.0f), _last_pitch_dem(0.0f), + _vel_dot(0.0f), + _TAS_dem(0.0f), + _TAS_dem_last(0.0f), + _hgt_dem_in_old(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_prev(0.0f), + _TAS_dem_adj(0.0f), + _STEdotErrLast(0.0f), + _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), _SPEdot_dem(0.0f), @@ -55,9 +54,9 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), - _vel_dot(0.0f), - _STEdotErrLast(0.0f) { - + _airspeed_enabled(false), + _throttle_slewrate(0.0f) + { } bool airspeed_sensor_enabled() { From 18f71e6bc41e9823f8c82c49c03ee8c7261b8053 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:49:03 +0100 Subject: [PATCH 104/154] fw_pos_control_l1: warning fixes --- .../fw_pos_control_l1_main.cpp | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 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 c1aa8d39c8..62c69d3ebe 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 @@ -362,6 +362,7 @@ FixedwingPositionControl *g_control; FixedwingPositionControl::FixedwingPositionControl() : + _mavlink_fd(-1), _task_should_exit(false), _control_task(-1), @@ -380,36 +381,34 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + /* states */ _setpoint_valid(false), _loiter_hold(false), - _airspeed_error(0.0f), - _airspeed_valid(false), - _groundspeed_undershoot(0.0f), - _global_pos_valid(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), land_onslope(false), - flare_curve_alt_last(0.0f), - _mavlink_fd(-1), - launchDetector(), launch_detected(false), - usePreTakeoffThrust(false) + usePreTakeoffThrust(false), + flare_curve_alt_last(0.0f), + launchDetector(), + _airspeed_error(0.0f), + _airspeed_valid(false), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false) { /* safely initialize structs */ - vehicle_attitude_s _att = {0}; - vehicle_attitude_setpoint_s _att_sp = {0}; - navigation_capabilities_s _nav_capabilities = {0}; - manual_control_setpoint_s _manual = {0}; - airspeed_s _airspeed = {0}; - vehicle_control_mode_s _control_mode = {0}; - vehicle_global_position_s _global_pos = {0}; - position_setpoint_triplet_s _pos_sp_triplet = {0}; - sensor_combined_s _sensor_combined = {0}; - - + memset(&_att, 0, sizeof(vehicle_attitude_s)); + memset(&_att_sp, 0, sizeof(vehicle_attitude_setpoint_s)); + memset(&_nav_capabilities, 0, sizeof(navigation_capabilities_s)); + memset(&_manual, 0, sizeof(manual_control_setpoint_s)); + memset(&_airspeed, 0, sizeof(airspeed_s)); + memset(&_control_mode, 0, sizeof(vehicle_control_mode_s)); + memset(&_global_pos, 0, sizeof(vehicle_global_position_s)); + memset(&_pos_sp_triplet, 0, sizeof(position_setpoint_triplet_s)); + memset(&_sensor_combined, 0, sizeof(sensor_combined_s)); _nav_capabilities.turn_distance = 0.0f; @@ -916,7 +915,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) { flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; @@ -1074,13 +1073,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _seatbelt_hold_heading = _att.yaw + _manual.yaw; } - /* climb out control */ - bool climb_out = false; + //XXX not used - /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { - climb_out = true; - } + /* climb out control */ +// bool climb_out = false; +// +// /* user wants to climb out */ +// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { +// climb_out = true; +// } /* if in seatbelt mode, set airspeed based on manual control */ @@ -1287,7 +1288,7 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; From 6480c6a2cee7a237f9310f6986567e91c54c1247 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 21:48:54 +0100 Subject: [PATCH 105/154] fw autoland: remove confusing slopelength parameter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +----- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ------- 2 files changed, 1 insertion(+), 12 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 45fdaa355e..020d1c119e 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 @@ -233,7 +233,6 @@ private: float speedrate_p; float land_slope_angle; - float land_slope_length; float land_H1_virt; float land_flare_alt_relative; float land_thrust_lim_alt_relative; @@ -278,7 +277,6 @@ private: param_t speedrate_p; param_t land_slope_angle; - param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; @@ -431,7 +429,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); @@ -520,7 +517,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); - param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); @@ -889,7 +885,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * 0.9f; float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _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 62a340e905..0909135e15 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 @@ -348,13 +348,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); -/** - * Landing slope length - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); - /** * * From b7488da441cf945ec1e5f0a1a4c4b6295707a8b0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 22:06:05 +0100 Subject: [PATCH 106/154] fw autoland: completely remove the virtual wp between the last wp and the landing wp. autoland starts now from the last normal wp. --- .../fw_pos_control_l1_main.cpp | 32 +++++++------------ 1 file changed, 11 insertions(+), 21 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 020d1c119e..e1869328a1 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 @@ -885,7 +885,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * 0.9f; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); @@ -931,34 +931,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi //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_last = flare_curve_alt; - - } else if (wp_distance < L_wp_distance) { - - /* minimize speed to approach speed, stay on landing slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); - - if (!land_onslope) { - - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); - land_onslope = true; - } - } else { /* intersect glide slope: - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope - * */ + * minimize speed to approach speed + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + + if (!land_onslope) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + land_onslope = true; + } + } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); From 9830f668c548ed63131722cdbba4753e1c06f647 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 22:55:40 +0100 Subject: [PATCH 107/154] fw autoland: fix warning --- 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 e1869328a1..b0d2bcbc6d 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 @@ -912,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) { flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; From ae3e625ce8cec56527dc6809c0b48081773b47a3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:20:17 +0100 Subject: [PATCH 108/154] fw pos ctrl: initialize uorb structs in initialization list --- .../fw_pos_control_l1_main.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 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 62c69d3ebe..1feb539b98 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 @@ -397,20 +397,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined() { - /* safely initialize structs */ - memset(&_att, 0, sizeof(vehicle_attitude_s)); - memset(&_att_sp, 0, sizeof(vehicle_attitude_setpoint_s)); - memset(&_nav_capabilities, 0, sizeof(navigation_capabilities_s)); - memset(&_manual, 0, sizeof(manual_control_setpoint_s)); - memset(&_airspeed, 0, sizeof(airspeed_s)); - memset(&_control_mode, 0, sizeof(vehicle_control_mode_s)); - memset(&_global_pos, 0, sizeof(vehicle_global_position_s)); - memset(&_pos_sp_triplet, 0, sizeof(position_setpoint_triplet_s)); - memset(&_sensor_combined, 0, sizeof(sensor_combined_s)); - - _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); From 60de15ea9d753eef3361778633f7f91fb92aa748 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:26:50 +0100 Subject: [PATCH 109/154] fw autoland: remove old comments --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ---- 1 file changed, 4 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 b0d2bcbc6d..3367aee9aa 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 @@ -942,17 +942,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } - } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), From b7899056f5454d8508a20dff87661c424ad98340 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:32:55 +0100 Subject: [PATCH 110/154] fw autoland: add very short comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 ++ 1 file changed, 2 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 3367aee9aa..64d2b70197 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 @@ -885,8 +885,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); From 2d97dff35c2976c7ba98e97a66b4a141cfb8960e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 06:52:40 +0100 Subject: [PATCH 111/154] Added support for DX10t, which apparently outputs 13 channels --- src/modules/px4iofirmware/dsm.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 60eda23192..70f31240a2 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -162,6 +162,7 @@ dsm_guess_format(bool reset) 0xff, /* 8 channels (DX8) */ 0x1ff, /* 9 channels (DX9, etc.) */ 0x3ff, /* 10 channels (DX10) */ + 0x1fff, /* 13 channels (DX10t) */ 0x3fff /* 18 channels (DX10) */ }; unsigned votes10 = 0; From 9828d5ede474723d83b189c394397d78c7050b7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 07:50:36 +0100 Subject: [PATCH 112/154] Fix copyright, do not return junk channel #13 that Spektrum gives us. --- src/modules/px4iofirmware/dsm.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 70f31240a2..0d3eb26063 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -401,6 +401,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } + /* + * Spektrum likes to send junk in higher channel numbers to fill + * their packets. We don't know about a 13 channel model in their TX + * lines, so if we get a channel count of 13, we'll return 12 (the last + * data index that is stable). + */ + if (*num_values == 13) + *num_values = 12; + if (dsm_channel_shift == 11) { /* Set the 11-bit data indicator */ *num_values |= 0x8000; From b58fa2dffc22b3ba27ac87f1f514c298024cea4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 10:37:54 +0100 Subject: [PATCH 113/154] Change bit mask to allow for 10 channels. --- 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 1335f52e13..97d25bbfa8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_DSM: - dsm_bind(value & 0x0f, (value >> 4) & 7); + dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; default: From f1f1ecd096f2ad7b791127c9ee943a7dfc0ab3f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 10:36:46 +0100 Subject: [PATCH 114/154] Fix mavlink feedback FD handling --- src/drivers/px4io/px4io.cpp | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index efcf4d12b9..13326e9625 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,8 +244,7 @@ private: volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag - int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. - int _thread_mavlink_fd; ///< mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. perf_counter_t _perf_update; /// Date: Tue, 18 Feb 2014 10:37:11 +0100 Subject: [PATCH 115/154] Code style --- 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 c3eea310ff..7a93e513ed 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) From a91b68d8a52709c5ce3ce465cad3b7e5a5e34f66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 10:37:32 +0100 Subject: [PATCH 116/154] Fix DSM pairing error handling. --- src/drivers/px4io/px4io.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 13326e9625..7c7b3dcb7d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2112,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); - usleep(500000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(72000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - usleep(50000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(72000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + usleep(50000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + + ret = OK; + } else { + ret = -EINVAL; + } break; case DSM_BIND_POWER_UP: @@ -2612,7 +2622,7 @@ bind(int argc, char *argv[]) #endif if (argc < 3) - errx(0, "needs argument, use dsm2 or dsmx"); + errx(0, "needs argument, use dsm2, dsmx or dsmx8"); if (!strcmp(argv[2], "dsm2")) pulses = DSM2_BIND_PULSES; @@ -2621,7 +2631,7 @@ bind(int argc, char *argv[]) else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; else - errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); From 06b69b70a5d73ba32bc08cacedbeb9016a32195b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 12:27:04 +0100 Subject: [PATCH 117/154] Scaling Spektrum inputs into normalized value same as the servo outputs do --- src/modules/px4iofirmware/dsm.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 0d3eb26063..aac2087b22 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -369,11 +369,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) if (channel >= *num_values) *num_values = channel + 1; - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (dsm_channel_shift == 11) - value /= 2; + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ + if (dsm_channel_shift == 10) + value *= 2; - value += 998; + /* + * Spektrum scaling is special. There are these basic considerations + * + * * Midpoint is 1520 us + * * 100% travel channels are +- 400 us + * + * We obey the original Spektrum scaling (so a default setup will scale from + * 1100 - 1900 us), but we do not obey the weird 1520 us center point + * and instead (correctly) center the center around 1500 us. This is in order + * to get something useful without requiring the user to calibrate on a digital + * link for no reason. + */ + + /* scaled integer for decent accuracy while staying efficient */ + value = (((value - 1024) * 1000) / 1700) + 1500; /* * Store the decoded channel into the R/C input buffer, taking into From 983cff56b3854aecf645773a1863ac01903b8c5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 12:50:29 +0100 Subject: [PATCH 118/154] Added int cast to handle arithmetic correctly --- src/modules/px4iofirmware/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index aac2087b22..d3f3658228 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -387,7 +387,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) */ /* scaled integer for decent accuracy while staying efficient */ - value = (((value - 1024) * 1000) / 1700) + 1500; + value = ((((int)value - 1024) * 1000) / 1700) + 1500; /* * Store the decoded channel into the R/C input buffer, taking into From f4bac7e7e802abf671d0ee87ad84b6f281fd81be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 21 Feb 2014 20:22:05 +0100 Subject: [PATCH 119/154] navigator: remove all but one [navigator] tag from mavlink audio messages --- src/modules/navigator/navigator_main.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2711be24fc..0268c47e73 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,7 +865,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] nav state: %s", nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); prevState = myState; /* reset time counter on state changes */ @@ -1073,11 +1073,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter at current altitude"); + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; @@ -1175,14 +1175,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); } } @@ -1331,7 +1331,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1357,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1384,7 +1384,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1516,7 +1516,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", _mission_item.time_inside); } } @@ -1541,7 +1541,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); } else { /* advance by one mission item */ From ff8f92af31fe1eadb52dbe1e612fd1f31b00ff82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:24:03 -0800 Subject: [PATCH 120/154] Remove Ubuntu code style script - people should use the reference one --- Tools/fix_code_style_ubuntu.sh | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100755 Tools/fix_code_style_ubuntu.sh diff --git a/Tools/fix_code_style_ubuntu.sh b/Tools/fix_code_style_ubuntu.sh deleted file mode 100755 index 90ab57b895..0000000000 --- a/Tools/fix_code_style_ubuntu.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh -astyle \ - --style=linux \ - --indent=force-tab=8 \ - --indent-cases \ - --indent-preprocessor \ - --break-blocks=all \ - --pad-oper \ - --pad-header \ - --unpad-paren \ - --keep-one-line-blocks \ - --keep-one-line-statements \ - --align-pointer=name \ - --suffix=none \ - --lineend=linux \ - $* - #--ignore-exclude-errors-x \ - #--exclude=EASTL \ - #--align-reference=name \ From 99b376089f6a35c977174ae8e01bc4686b405cc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:26:23 -0800 Subject: [PATCH 121/154] Replace single if lines with brackets in astyle --- Tools/fix_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index 832ee79da6..0b6743013f 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -16,4 +16,5 @@ astyle \ --ignore-exclude-errors-x \ --lineend=linux \ --exclude=EASTL \ + --add-brackets \ $* From 20618a611b5cd719cab803823b2f3a08d9a498f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:44:05 -0800 Subject: [PATCH 122/154] Fixed a number of compile warnings in mount tests. --- src/systemcmds/tests/test_mount.c | 38 ++++++++++++++----------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 44e34d9ef3..4b6303cfb1 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -141,8 +141,8 @@ test_mount(int argc, char *argv[]) /* announce mode switch */ if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); } @@ -162,7 +162,7 @@ test_mount(int argc, char *argv[]) } char buf[64]; - int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + (void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); lseek(cmd_fd, 0, SEEK_SET); write(cmd_fd, buf, strlen(buf) + 1); fsync(cmd_fd); @@ -174,8 +174,8 @@ test_mount(int argc, char *argv[]) printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); for (unsigned a = 0; a < alignments; a++) { @@ -185,22 +185,20 @@ test_mount(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - hrt_abstime start, end; int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); - if (wret != chunk_sizes[c]) { + if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) @@ -214,8 +212,8 @@ test_mount(int argc, char *argv[]) fsync(fd); } else { printf("#"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); } } @@ -224,12 +222,10 @@ test_mount(int argc, char *argv[]) } printf("."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(200000); - end = hrt_absolute_time(); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -237,7 +233,7 @@ test_mount(int argc, char *argv[]) for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, chunk_sizes[c]); - if (rret != chunk_sizes[c]) { + if (rret != (int)chunk_sizes[c]) { warnx("READ ERROR!"); return 1; } @@ -245,7 +241,7 @@ test_mount(int argc, char *argv[]) /* compare value */ bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { + for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j + a]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); compare_ok = false; @@ -271,16 +267,16 @@ test_mount(int argc, char *argv[]) } } - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); /* we always reboot for the next test if we get here */ warnx("Iteration done, rebooting.."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); systemreset(false); From e1f60e770948d28ec1152f66b9c3ad57eae0c2e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:53:20 -0800 Subject: [PATCH 123/154] Code style fixes (authors should be in the doxygen section), using lseek to attempt to work around log corruption --- src/modules/sdlog2/logbuffer.c | 3 +-- src/modules/sdlog2/sdlog2.c | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index b3243f7b5a..6a29d7e5c9 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c4fafb5a66..16c37459b5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -95,6 +93,7 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ + } else { \ log_msgs_skipped++; \ } @@ -450,6 +449,7 @@ static void *logwriter_thread(void *arg) n = available; } + lseek(log_fd, 0, SEEK_CUR); n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; From ab8ece3961f50348cf4bf464cb7d953b182a0a44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 12:42:48 +0100 Subject: [PATCH 124/154] l1_pos_control: prevent NaNs for roll setpoint --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 3b68a0a4e8..d1c864d782 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -38,6 +38,8 @@ * */ +#include + #include "ecl_l1_pos_controller.h" float ECL_L1_Pos_Controller::nav_roll() @@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* calculate the vector from waypoint A to current position */ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - /* store the normalized vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } /* calculate eta angle towards the loiter center */ From 75c6706278119e50ce56ecbb1620025a9b9b936d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:19:57 +0100 Subject: [PATCH 125/154] l1_pos_control: set gndspeed_undershoot to 0 in loiter --- 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 ed57e0d349..2e8d038d0d 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 @@ -695,7 +695,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid) { + if (_global_pos_valid && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); From 909e138182000350df19d47367d6f68d0a6b7fd4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:27:24 +0100 Subject: [PATCH 126/154] l1_pos_control: fix stupid 'if not' mistake --- 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 2e8d038d0d..ed6d8792ce 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 @@ -695,7 +695,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); From ba0bf456b9b0cf6cdc58ce2fd80192da3c1744d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:37:35 +0100 Subject: [PATCH 127/154] gitignore: ignore pydev project file created in eclipse --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 3e94cf6205..71326517f2 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/ /Documentation/doxygen*objdb*tmp .tags .tags_sorted_by_file +.pydevproject From 5ed5e04cb24c261c9a8b40857bd1d733238f847e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:43:23 +0100 Subject: [PATCH 128/154] sdlog2: code style fixes broke compilation --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 16c37459b5..41e2248bbe 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -93,7 +93,6 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ - } else { \ log_msgs_skipped++; \ } From a79eef05bcb23c9be386a2980437a67151cfa3ea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Feb 2014 23:48:00 +0400 Subject: [PATCH 129/154] perf_counter: added include --- src/modules/systemlib/perf_counter.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index e1e3cbe95e..d8f69fdbf7 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -39,6 +39,8 @@ #ifndef _SYSTEMLIB_PERF_COUNTER_H #define _SYSTEMLIB_PERF_COUNTER_H value +#include + /** * Counter types. */ From f8187650083b361f61c07ceb712c21a25d1c80c4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:32:55 +0400 Subject: [PATCH 130/154] top: CPU % indication bug fixed --- src/systemcmds/top/top.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 1ca3fc9281..37e913040d 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -233,8 +233,8 @@ top_main(void) system_load.tasks[i].tcb->pid, CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100), - (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + (int)(curr_loads[i] * 100.0f), + (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), stack_size - stack_free, stack_size, system_load.tasks[i].tcb->sched_priority, From f2b46389ee8c8e9dd73ad9ac1fc8170c759e8b1c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Feb 2014 21:42:41 +0100 Subject: [PATCH 131/154] fw: reset landing and takeoff state if switched to manual --- .../fw_pos_control_l1_main.cpp | 55 +++++++++++++++---- 1 file changed, 45 insertions(+), 10 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 a4142266b1..ab2a2439bb 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 @@ -176,6 +176,8 @@ private: bool launch_detected; bool usePreTakeoffThrust; + bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + /* Landingslope object */ Landingslope landingslope; @@ -346,6 +348,16 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + /* + * Reset takeoff state + */ + int reset_takeoff_state(); + + /* + * Reset landing state + */ + int reset_landing_state(); }; namespace l1_control @@ -396,7 +408,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false) + usePreTakeoffThrust(false), + last_manual(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -1042,20 +1055,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // mission is active _loiter_hold = false; - /* reset land state */ + /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; + reset_landing_state(); } /* reset takeoff/launch state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { - launch_detected = false; - usePreTakeoffThrust = false; - launchDetector.reset(); + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1150,6 +1157,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; + + /* reset landing and takeoff state */ + if (!last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } } if (usePreTakeoffThrust) { @@ -1160,6 +1173,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _att_sp.pitch_body = _tecs.get_pitch_demand(); + if (_control_mode.flag_control_position_enabled) { + last_manual = false; + } else { + last_manual = true; + } + return setpoint; } @@ -1310,6 +1329,22 @@ FixedwingPositionControl::task_main() _exit(0); } +int FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +int FixedwingPositionControl::reset_landing_state() +{ + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; +} + int FixedwingPositionControl::start() { From eac00a71ea8f0b10d446be752f6fef737f850956 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 11:05:07 +0100 Subject: [PATCH 132/154] navigator: don't enter AUTO_READY for fixedwing because we don't have a land detector yet --- src/modules/navigator/navigator_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 04f87d4caf..8d0710e915 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1386,7 +1386,8 @@ Navigator::set_rtl_item() void Navigator::request_loiter_or_ready() { - if (_vstatus.condition_landed) { + /* XXX workaround: no landing detector for fixedwing yet */ + if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { dispatch(EVENT_READY_REQUESTED); } else { From b264352c179dafd42066ca8ffbdf855c7f111207 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 15:47:09 +0100 Subject: [PATCH 133/154] startup: phantom script was missing the default line --- ROMFS/px4fmu_common/init.d/3031_phantom | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 2e2434bb85..a3004d1e13 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -5,4 +5,6 @@ # Simon Wilks # +sh /etc/init.d/rc.fw_defaults + set MIXER FMU_Q From e06263f76f9e38ea9624f1f1ab21af8731a3cd7f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:43:56 +0400 Subject: [PATCH 134/154] position_estimator_inav: failsafe against NaN estimate --- .../position_estimator_inav_main.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 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 ad363efe06..fb68dd31c0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -708,6 +708,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); + float x_est_prev[3], y_est_prev[3]; + + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -715,7 +720,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); - thread_should_exit = true; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ @@ -739,7 +745,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); - thread_should_exit = true; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_flow, 0, sizeof(corr_flow)); } } From d11235585cf1014c5391b63e394151c171ed286d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:56:02 +0400 Subject: [PATCH 135/154] position_estimator_inav: log writing on NaN estimate fixed --- .../position_estimator_inav_main.c | 9 +++++---- 1 file changed, 5 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 fb68dd31c0..d6d03367b6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); - fputs(f, s); - snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); - fputs(f, s); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fwrite(s, 1, n, f); + n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); } From db157b68c2f3dba4209f8075f6ae7ae9e81a448c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 19:42:10 +0400 Subject: [PATCH 136/154] mc_pos_control: remove some debug logs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 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 b4bac53d66..78d06ba5b5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -791,7 +791,6 @@ MulticopterPositionControl::task_main() } thrust_int(2) = -i; - mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } } else { @@ -803,7 +802,6 @@ MulticopterPositionControl::task_main() reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; - mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); } } else { From 3674aae4a746099a162cbf943311b587cd46822f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 21:19:56 +0400 Subject: [PATCH 137/154] commander: ignore battery voltage in HIL mode --- 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 d5af63120b..483437029b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -962,7 +962,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; From b76e26c5e5f37c4fb086a68e0427d9d297e8d225 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 21:24:59 +0400 Subject: [PATCH 138/154] commander: allow arming with safety enabled in HIL mode --- 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 483437029b..d114a2e5ce 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); From a7012a54180386b5dc7f86fe6a1564690ecc8240 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Mar 2014 08:34:55 +1100 Subject: [PATCH 139/154] tests: added "tests file2" filesystem test this is useful for testing for filesystem corruption on cluster boundaries --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_file2.c | 196 ++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 199 insertions(+) create mode 100644 src/systemcmds/tests/test_file2.c diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index acf28c35b2..622a0faf3d 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -26,6 +26,7 @@ SRCS = test_adc.c \ test_mixer.cpp \ test_mathlib.cpp \ test_file.c \ + test_file2.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c new file mode 100644 index 0000000000..ef555f6c34 --- /dev/null +++ b/src/systemcmds/tests/test_file2.c @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * 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 test_file2.c + * + * File write test. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define FLAG_FSYNC 1 +#define FLAG_LSEEK 2 + +/* + return a predictable value for any file offset to allow detection of corruption + */ +static uint8_t get_value(uint32_t ofs) +{ + union { + uint32_t ofs; + uint8_t buf[4]; + } u; + u.ofs = ofs; + return u.buf[ofs % 4]; +} + +static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags) +{ + printf("Testing on %s with write_chunk=%u write_size=%u\n", + filename, (unsigned)write_chunk, (unsigned)write_size); + + uint32_t ofs = 0; + int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + if (fd == -1) { + perror(filename); + exit(1); + } + + // create a file of size write_size, in write_chunk blocks + uint8_t counter = 0; + while (ofs < write_size) { + uint8_t buffer[write_chunk]; + for (uint16_t j=0; j 0) { + filename = argv[0]; + } + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + test_corruption(filename, write_chunk, write_size, flags); + return 0; +} + diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ac64ad33d3..ad55e14102 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -107,6 +107,7 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_file2(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 73827b7cf0..fe22f61773 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -104,6 +104,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"file2", test_file2, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, From ea70e967565a15ffc06aeb95df87f47440df97c1 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 4 Mar 2014 16:16:39 +0100 Subject: [PATCH 140/154] Added ARDrone to autostart --- ROMFS/px4fmu_common/init.d/rc.autostart | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 3968af58ea..7aaf7133ef 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -106,6 +106,15 @@ then sh /etc/init.d/4001_quad_x fi +# +# ARDrone +# + +if param compare SYS_AUTOSTART 4008 8 +then + sh /etc/init.d/4008_ardrone +fi + if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 From 958f7597e7d13f82e41b9c8e0aa4c3ac552cdf1c Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 4 Mar 2014 16:33:18 +0100 Subject: [PATCH 141/154] Added actual ARDrone start up script. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 40 +++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4008_ardrone diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone new file mode 100644 index 0000000000..fa9b9d18a1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -0,0 +1,40 @@ +#!nsh +# +# ARDrone +# + +# Just use the default multicopter settings. +sh /etc/init.d/rc.mc_defaults + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 + + param set SYS_AUTOCONFIG 0 + param save +fi + +set FMU_MODE gpio + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +set MAV_TYPE 2 +set VEHICLE_TYPE mc +param set BAT_V_SCALING 0.008381 From 9817922bf96611f59552b6a6fd2aebab790135f5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Mar 2014 20:34:20 +1100 Subject: [PATCH 142/154] FMUv2: switch debug console and 2nd GPS this allows a normal GPS cable to be used for the 2nd GPS, making dual-GPS setups easier --- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2a734c27e5..26222c255d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set -# CONFIG_UART7_RXDMA is not set +CONFIG_UART7_RXDMA=y # CONFIG_UART8_RS485 is not set CONFIG_UART8_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y @@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # 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 is not set -CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set # From 3639c7ae91f5226b71cdba8d8cc98eae4bba7e07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Mar 2014 21:36:09 +0100 Subject: [PATCH 143/154] Fixed warning --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6a44294619..dfa98d425c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -327,7 +327,7 @@ mixer_handle_text(const void *buffer, size_t length) return 1; } - px4io_mixdata *msg = (px4io_mixdata *)buffer; + px4io_mixdata *msg = static_cast(buffer); isr_debug(2, "mix txt %u", length); From fabef6c889f0fc347e0059ee6f8ede83bf0f4f32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Mar 2014 22:52:34 +0100 Subject: [PATCH 144/154] Revert "Fixed warning" This reverts commit 3639c7ae91f5226b71cdba8d8cc98eae4bba7e07. --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index dfa98d425c..6a44294619 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -327,7 +327,7 @@ mixer_handle_text(const void *buffer, size_t length) return 1; } - px4io_mixdata *msg = static_cast(buffer); + px4io_mixdata *msg = (px4io_mixdata *)buffer; isr_debug(2, "mix txt %u", length); From 1d9d24f605aeb13b87efe2f6cf82232768b148ac Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 10:50:16 +0100 Subject: [PATCH 145/154] Added flag for ARDrone interface. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 14 +++++--------- ROMFS/px4fmu_common/init.d/rcS | 12 +++++++++++- src/drivers/px4fmu/fmu.cpp | 2 +- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index fa9b9d18a1..d6b2319f94 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -3,6 +3,8 @@ # ARDrone # +echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" + # Just use the default multicopter settings. sh /etc/init.d/rc.mc_defaults @@ -29,12 +31,6 @@ then param save fi -set FMU_MODE gpio - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -set MAV_TYPE 2 -set VEHICLE_TYPE mc -param set BAT_V_SCALING 0.008381 +set FMU_MODE gpio_serial +set OUTPUT_MODE fmu +set ARDRONE yes \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 76f021e339..af78162ed5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -397,8 +397,10 @@ then set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - mavlink start + mavlink start -d /dev/ttyS0 usleep 5000 + + set EXIT_ON_END yes fi fi @@ -427,6 +429,14 @@ then gps start fi + # + # Start up ARDrone Motor interface + # + if [ $ARDRONE == yes ] + then + ardrone_interface start -d /dev/ttyS1 + fi + # # Fixed wing setup # diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0fbd849243..a70ff6c5c5 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[]) } - fprintf(stderr, "FMU: unrecognised command, try:\n"); + fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #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\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) From 9ffa5a665a2ac8b887cbd9383eab1947137f0def Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 10:56:58 +0100 Subject: [PATCH 146/154] Set the mavlink port settings back to ttyS1 by default. --- ROMFS/px4fmu_common/init.d/rcS | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index af78162ed5..1baac0eb3a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes ] + if [ $TTYS1_BUSY == yes -o $ARDRONE == yes] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 @@ -397,10 +397,8 @@ then set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - mavlink start -d /dev/ttyS0 + mavlink start usleep 5000 - - set EXIT_ON_END yes fi fi From 5976815f2baff6b17ca41dbb28e3e5698c12a180 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 15:07:05 +0100 Subject: [PATCH 147/154] Fixed startup parameters. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 29 ++++++++++++------------- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index d6b2319f94..d38796cca0 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -11,24 +11,23 @@ sh /etc/init.d/rc.mc_defaults # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig - param set MC_ATTRATE_D 0 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0 - param set MC_ATT_I 0.3 - param set MC_ATT_P 5 - param set MC_YAWPOS_D 0.1 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 1 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 + param set MC_ROLL_P 5.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.0 + param set MC_PITCH_P 5.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.0 + param set MC_YAW_P 1.0 + param set MC_YAW_D 0.1 param set MC_YAWRATE_P 0.15 - - param set SYS_AUTOCONFIG 0 - param save + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.15 fi set FMU_MODE gpio_serial diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1baac0eb3a..9983aa0e77 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes -o $ARDRONE == yes] + if [ $TTYS1_BUSY == yes -o $ARDRONE == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 From 30665816f0005cfb32b048a9ad4db84f78b3eff3 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 15:11:41 +0100 Subject: [PATCH 148/154] Added a newline at the end of file. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index d38796cca0..cb8260cccb 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -32,4 +32,4 @@ fi set FMU_MODE gpio_serial set OUTPUT_MODE fmu -set ARDRONE yes \ No newline at end of file +set ARDRONE yes From bcdc8c9e1dcefc2176fd1f853d4bae90e7d196d6 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 16:16:10 +0100 Subject: [PATCH 149/154] Get rid of one set of beeps. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index cb8260cccb..512c4cb736 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -33,3 +33,4 @@ fi set FMU_MODE gpio_serial set OUTPUT_MODE fmu set ARDRONE yes +set USE_IO no From 36ba60d63c024c23efdb4bba780ac98b5b257ee6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 20:03:21 +0400 Subject: [PATCH 150/154] mc_att_control: hotfix, memset all structs to zeroes on start, fixes garbage in actuator controls 4..8 --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 24226880ed..9cb8e83445 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -262,8 +262,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : { 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(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); _params.att_p.zero(); From a897c97d955de4873aea1f01f65cab11faab2d3a Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 6 Mar 2014 12:45:10 +0100 Subject: [PATCH 151/154] Changed ARDRONE to an OUTPUT_MODE setting and added a skip option to mixer. Fewer beeps than before. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 5 ++--- ROMFS/px4fmu_common/init.d/rc.interface | 9 ++++++--- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++---- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 512c4cb736..39fe66234b 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -30,7 +30,6 @@ then param set MC_YAW_FF 0.15 fi -set FMU_MODE gpio_serial -set OUTPUT_MODE fmu -set ARDRONE yes +set OUTPUT_MODE ardrone set USE_IO no +set MIXER skip \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d25f01dde0..afe71460de 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -3,7 +3,7 @@ # Script to configure control interface # -if [ $MIXER != none ] +if [ $MIXER != none -a $MIXER != skip] then # # Load mixer @@ -33,8 +33,11 @@ then tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] Mixer not defined" - tone_alarm $TUNE_OUT_ERROR + if [ $MIXER != skip ] + then + echo "[init] Mixer not defined" + tone_alarm $TUNE_OUT_ERROR + fi fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9983aa0e77..7b9ae09955 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -240,6 +240,11 @@ then fi fi + if [ $OUTPUT_MODE == ardrone ] + then + set FMU_MODE gpio_serial + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -277,7 +282,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - if [ $OUTPUT_MODE == fmu ] + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ] then echo "[init] Use FMU PWM as primary output" if fmu mode_$FMU_MODE @@ -351,7 +356,7 @@ then fi fi else - if [ $OUTPUT_MODE != fmu ] + if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ] then if fmu mode_$FMU_MODE then @@ -387,7 +392,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes -o $ARDRONE == yes ] + if [ $TTYS1_BUSY == yes -o $OUTPUT_MODE == ardrone ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 @@ -430,7 +435,7 @@ then # # Start up ARDrone Motor interface # - if [ $ARDRONE == yes ] + if [ $OUTPUT_MODE == ardrone ] then ardrone_interface start -d /dev/ttyS1 fi From 25faf1b8f8716323233d6f966aac65e0c9e8d61a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Mar 2014 17:44:10 +0400 Subject: [PATCH 152/154] ardrone: minor fixes --- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 +- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 39fe66234b..0f98f7b6c2 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -32,4 +32,4 @@ fi set OUTPUT_MODE ardrone set USE_IO no -set MIXER skip \ No newline at end of file +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7b9ae09955..b1cd919f72 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -284,7 +284,7 @@ then fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ] then - echo "[init] Use FMU PWM as primary output" + echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -299,7 +299,7 @@ then then set TTYS1_BUSY yes fi - if [ $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ] then set TTYS1_BUSY yes fi @@ -372,7 +372,7 @@ then then set TTYS1_BUSY yes fi - if [ $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ] then set TTYS1_BUSY yes fi @@ -392,7 +392,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes -o $OUTPUT_MODE == ardrone ] + if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 From 2fbcc32870f59aa1f08337ca99129b35902971be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 6 Mar 2014 23:00:00 +0100 Subject: [PATCH 153/154] fix comparison in rcS --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b1cd919f72..57299d7724 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -282,7 +282,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ] + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE From 602e9c87062409955c122631d273c6aae50331ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 6 Mar 2014 23:11:13 +0100 Subject: [PATCH 154/154] fix if in rc.interface --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index afe71460de..7f793b2198 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -3,7 +3,7 @@ # Script to configure control interface # -if [ $MIXER != none -a $MIXER != skip] +if [ $MIXER != none -a $MIXER != skip ] then # # Load mixer