From cfaf0ada45a9b99a83221c6c51a50090c55a1483 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 23:14:05 +0400 Subject: [PATCH 001/222] Heading in VFR message fixed --- src/modules/mavlink/orb_listener.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34fa..2804a8191f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -248,7 +249,7 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t heading = _wrap_2pi(att.yaw) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } From e7f4d91022af363905b758ab39bde1addcb69517 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 23:15:02 +0400 Subject: [PATCH 002/222] geo: cleanup of wrap_XXX functions --- src/lib/geo/geo.c | 54 +++++++++++++++++++++++++++++++---------------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba7..9e533b92d8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -390,22 +390,22 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { + if (!isfinite(bearing)) { return bearing; } int c = 0; - - while (bearing > M_PI_F && c < 30) { + while (bearing > M_PI_F) { bearing -= M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } c = 0; - - while (bearing <= -M_PI_F && c < 30) { + while (bearing <= -M_PI_F) { bearing += M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } return bearing; @@ -418,12 +418,18 @@ __EXPORT float _wrap_2pi(float bearing) return bearing; } - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; + int c = 0; + while (bearing > M_TWOPI_F) { + bearing -= M_TWOPI_F; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; + c = 0; + while (bearing <= 0.0f) { + bearing += M_TWOPI_F; + if (c++ > 3) + return NAN; } return bearing; @@ -436,12 +442,18 @@ __EXPORT float _wrap_180(float bearing) return bearing; } + int c = 0; while (bearing > 180.0f) { - bearing = bearing - 360.0f; + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= -180.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; @@ -454,12 +466,18 @@ __EXPORT float _wrap_360(float bearing) return bearing; } - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; + int c = 0; + while (bearing > 360.0f) { + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= 0.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; From c31d065340a44ef121069b5b82395526b910d3e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 14 Nov 2013 11:59:18 +0400 Subject: [PATCH 003/222] Revert "Heading in VFR message fixed" This reverts commit cfaf0ada45a9b99a83221c6c51a50090c55a1483. --- src/modules/mavlink/orb_listener.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2804a8191f..abc91d34fa 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,7 +54,6 @@ #include #include #include -#include #include @@ -249,7 +248,7 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = _wrap_2pi(att.yaw) / M_PI_F * 180.0f; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } From b23af6108772f8049ca94dfd8c648e1014917062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:47:35 +0100 Subject: [PATCH 004/222] 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 005/222] 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 006/222] 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 007/222] 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 008/222] 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 009/222] 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 010/222] 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 011/222] 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 012/222] 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 013/222] 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 014/222] 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 015/222] 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 016/222] 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 017/222] 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 018/222] 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 019/222] 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 020/222] 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 8ffb9e29c62e645b48573439d4ebc70acfa7db54 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:56:15 +0100 Subject: [PATCH 021/222] Teach IO firmware that arming and lockdown are two different things, clean up arming check --- src/modules/px4iofirmware/mixer.cpp | 15 +++++++++++++-- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 8 ++------ 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a9..ea9e71c530 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -217,14 +217,25 @@ mixer_tick(void) } } - if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { + /* set arming */ + bool needs_to_arm = (should_arm || should_always_enable_pwm); + + /* check any conditions that prevent arming */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { + needs_to_arm = false; + } + if (!should_arm && !should_always_enable_pwm) { + needs_to_arm = false; + } + + if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); - } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { + } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3a..ce8d26cc8b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -166,6 +166,7 @@ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ad44730736..42938b4468 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,7 +169,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -453,11 +454,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * lockup of the IO arming state. */ - // XXX do not reset IO's safety state by FMU for now - // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - // } - if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } From ded8cc6e14c7ec42d2a0e08b83c1510f213bf55d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:56:39 +0100 Subject: [PATCH 022/222] Add IOCTLs to disable lockdown of an output port --- src/drivers/drv_pwm_output.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37b..53065f8eb7 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,20 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the lockdown override flag to enable outputs in HIL */ +#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21) + +/** get the lockdown override flag to enable outputs in HIL */ +#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) + +/* + * + * + * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE! + * + * + */ + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) From 978cfdccce9148ebb2282646b609d5eb7c59f487 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:57:09 +0100 Subject: [PATCH 023/222] Teach IO driver how to disable lockdown mode --- src/drivers/px4io/px4io.cpp | 99 +++++++++++++++++++++++++++++++++---- 1 file changed, 89 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f29..658bcd8d89 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,6 +273,7 @@ private: actuator_outputs_s _outputs; ///ioctl(1, mode); /* XXX magic numbers */ - delete interface; + if (interface) { + result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + } else { + errx(1, "interface not loaded, exiting"); + } errx(0, "test returned %d", result); } +void +lockdown(int argc, char *argv[]) +{ + if (g_dev != nullptr) { + + if (argc > 2 && !strcmp(argv[2], "disable")) { + + warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + warnx("Press 'y' to enable, any other key to abort."); + + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; + + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } + } + + usleep(10000); + } + + if (hrt_elapsed_time(&start) > timeout) + errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + + warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + } else { + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + warnx("ACTUATORS ARE NOW SAFE IN HIL."); + } + + } else { + errx(1, "driver not loaded, exiting"); + } + exit(0); +} + } /* namespace */ int @@ -2883,6 +2959,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "lockdown")) + lockdown(argc, argv); + out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } From 62076c0e81d505544ecddb05b039a4f010200480 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:58:05 +0100 Subject: [PATCH 024/222] Teach the commander to arm on the commandline --- src/modules/commander/commander.cpp | 57 ++++++++++++++++++++++++----- 1 file changed, 47 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index add7312de7..2a2bcca727 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -154,6 +154,16 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + /* tasks waiting for low prio thread */ typedef enum { LOW_PRIO_TASK_NONE = 0, @@ -210,6 +220,9 @@ void print_reject_arm(const char *msg); void print_status(); +int arm(); +int disarm(); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** @@ -277,6 +290,16 @@ int commander_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "arm")) { + arm(); + exit(0); + } + + if (!strcmp(argv[1], "disarm")) { + disarm(); + exit(0); + } + usage("unrecognized command"); exit(1); } @@ -344,6 +367,30 @@ void print_status() static orb_advert_t control_mode_pub; static orb_advert_t status_pub; +int arm() +{ + int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + } else { + return 1; + } +} + +int disarm() +{ + int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + } else { + return 1; + } +} + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -537,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } -static struct vehicle_status_s status; - -/* armed topic */ -static struct actuator_armed_s armed; - -static struct safety_s safety; - -/* flags for control apps */ -struct vehicle_control_mode_s control_mode; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ From d174998b45349348ffe41150aa1d22d7d943b790 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 12:00:17 +0100 Subject: [PATCH 025/222] RSSI and concurrent S.Bus output handling --- src/drivers/px4io/px4io.cpp | 6 ++++-- src/modules/px4iofirmware/controls.c | 9 +++++++++ src/modules/px4iofirmware/px4io.c | 5 +++++ src/modules/px4iofirmware/px4io.h | 1 + 4 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dede5976da..8e080c043b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1453,8 +1453,10 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } /* lazily advertise on first publication */ if (_to_input_rc == 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1c..5859f768bf 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,18 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 0b8c4a6a8b..150af35df1 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -196,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a0daa97ea0..18c7468f69 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) From 5bc61c3c57164a7d2d118d3f7399a2a24e7199cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 08:30:48 +0100 Subject: [PATCH 026/222] S.BUS output disable cleanup --- src/drivers/boards/px4io-v2/board_config.h | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 -- src/modules/px4iofirmware/px4io.h | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8152ea4d4b..ef9bb5cadd 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf56..9f8c0eeb27 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 18c7468f69..d7f3e9adb5 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,7 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) -# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) From 1c40ce968a668a8d80535ab6799518da68fe0ac2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:01:55 +0100 Subject: [PATCH 027/222] RC config params set to more useful default values - needs more testing --- src/modules/sensors/sensor_params.c | 63 ++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef938..30659fd3a0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); From ac37172b5280e661395035c17d2dfde91672b4b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:17:04 +0100 Subject: [PATCH 028/222] Hotfix: Prevent failures in boot handling due to missing microSD card logfile - we are not depending on the microSD any more completely --- ROMFS/px4fmu_common/init.d/rcS | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a9c5c59ea9..a2e562d6e4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,6 @@ # set MODE autostart -set LOG_FILE /fs/microsd/bootlog.txt set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt @@ -21,10 +20,12 @@ set TUNE_OUT_ERROR ML< Date: Thu, 23 Jan 2014 08:25:37 +0100 Subject: [PATCH 029/222] Enable the PX4IO self check and debug interfaces. No reason to disable them, since they are runtime-configured (and needed, for the case of memory) --- src/modules/px4iofirmware/px4io.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 150af35df1..e9838d38ca 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.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 @@ -228,7 +228,6 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); @@ -245,7 +244,6 @@ user_start(int argc, char *argv[]) phase = !phase; usleep(300000); } -#endif /* * Run everything in a tight loop. @@ -275,7 +273,6 @@ user_start(int argc, char *argv[]) check_reboot(); -#if 0 /* check for debug activity */ show_debug_messages(); @@ -292,7 +289,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } From f504863feef167c1ad493bf58c44420265892193 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:25:56 +0100 Subject: [PATCH 030/222] Make startup less chatty --- 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 a2e562d6e4..cbbdeeee8f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -181,7 +181,7 @@ then set IO_PRESENT yes else - echo "[init] PX4IO CRC failure, trying to update" + echo "[init] Trying to update" echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MLL32CP8MB From c3e4e4ee68f1f31d3ae281b0afb281fc7c58bc27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:26:53 +0100 Subject: [PATCH 031/222] Build fix, replaced usleep with up_udelay in memory lockdown state --- src/modules/px4iofirmware/px4io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e9838d38ca..d8ebb43e91 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -242,7 +242,7 @@ user_start(int argc, char *argv[]) } phase = !phase; - usleep(300000); + up_udelay(300000); } /* From a0db455334c0928b953ab088e868e72fa3fc08f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:43:55 +0100 Subject: [PATCH 032/222] Cleanup wording 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 cbbdeeee8f..edf2c77fba 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,7 +182,7 @@ then set IO_PRESENT yes else echo "[init] Trying to update" - echo "PX4IO CRC failure" >> $LOG_FILE + echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB From 8833f81b48aa738125b42a08aca05e3131cb8f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:45:29 +0100 Subject: [PATCH 033/222] Do not make PX4IO start mandatory for forceupdate --- src/drivers/px4io/px4io.cpp | 37 ++++++++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 27ee1fb420..4844372b84 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2367,8 +2367,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2769,18 +2771,35 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); } - // tear down the px4io instance - delete g_dev; + if (OK != g_dev->init()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver init failed"); + } } + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; From 1ac8501d95f9d01bd2efc5b75373260dcc8d4530 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Jan 2014 23:51:22 -0800 Subject: [PATCH 034/222] Clear the screen more properly. --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4844372b84..b85638788a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2646,17 +2646,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); From 0994412ccae65349b144e0f29781889b18cd23ca Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Jan 2014 23:51:53 -0800 Subject: [PATCH 035/222] Fix the initialisation and operation of the PX4IO ADC - now RSSI and VSERVO voltages should be read correctly. --- src/modules/px4iofirmware/adc.c | 53 ++++++++++++++----------------- src/modules/px4iofirmware/px4io.c | 3 ++ 2 files changed, 26 insertions(+), 30 deletions(-) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a2..2f5908ac5e 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d8ebb43e91..d03b648094 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -210,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); From d77a15e94fd024633661eb92f72455d737a0aa84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:16:40 +0100 Subject: [PATCH 036/222] Last small fixes to IO driver to support updates with and without switch pressed and with and without px4io start call before the forceupdate call --- src/drivers/px4io/px4io.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4844372b84..879e19973a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2784,9 +2784,7 @@ px4io_main(int argc, char *argv[]) } if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); + warnx("driver init failed, still trying.."); } } From 2aa76f1a3c4eb99074b38d287e0f18a98973671d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:17:46 +0100 Subject: [PATCH 037/222] Fixes to memory check handling, split out switch handling to allow separate initialization --- src/modules/px4iofirmware/px4io.c | 41 ++++++++++++++++++++++-------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/safety.c | 8 ++++-- 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d8ebb43e91..dba35752e2 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -228,23 +228,42 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - up_udelay(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } + /* Start the failsafe led init */ + failsafe_led_init(); + /* * Run everything in a tight loop. */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index d7f3e9adb5..bffbc0ce2a 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,7 @@ extern void mixer_handle_text(const void *buffer, size_t length); * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index cdb54a80ad..2ce479ffd3 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -84,7 +84,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -165,8 +169,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; From 9e72e726442198feb6b4d3a725c8195ef55ffe55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:29:59 +0100 Subject: [PATCH 038/222] Make SBUS switching conditional to be friendly to IO v1 --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5859f768bf..5e2c92bf46 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -123,7 +123,9 @@ controls_tick() { /* switch S.Bus output pin as needed */ if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif } perf_end(c_gather_sbus); From 6a40acdbdc6abe57fb202a02e6ab6fa8c90698a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:58:22 +0100 Subject: [PATCH 039/222] Fixed PPM warning to be only printed with PPM inputs enabled --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08ff3792fe..a54bb39646 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1810,7 +1810,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); From 6acb8fa66f38d20af57b8c45cc7878257abb24d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 12:16:02 +0100 Subject: [PATCH 040/222] Replace mission_item_triplet with position_setpoint_triplet, WIP --- src/modules/controllib/uorb/blocks.cpp | 6 +- src/modules/controllib/uorb/blocks.hpp | 8 +- src/modules/fixedwing_backside/fixedwing.hpp | 2 +- .../fw_pos_control_l1_main.cpp | 104 ++-- src/modules/mavlink/orb_listener.c | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 31 +- src/modules/navigator/navigator_main.cpp | 451 ++++++++---------- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/objects_common.cpp | 4 +- ..._triplet.h => position_setpoint_triplet.h} | 38 +- 10 files changed, 304 insertions(+), 344 deletions(-) rename src/modules/uORB/topics/{mission_item_triplet.h => position_setpoint_triplet.h} (77%) diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e213ac17fc..e8fecef0d9 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 8cc0d77d47..7c80c4b2ba 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd); + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription _attCmd; UOrbSubscription _ratesCmd; UOrbSubscription _pos; - UOrbSubscription _missionCmd; + UOrbSubscription _missionCmd; UOrbSubscription _manual; UOrbSubscription _status; UOrbSubscription _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index b4dbc36b09..e1c85c2618 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - mission_item_triplet_s _lastMissionCmd; + position_setpoint_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: 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 d8dbf9085a..3889012c9d 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 @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _mission_item_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -145,7 +145,7 @@ private: 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 mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + 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 */ @@ -332,10 +332,10 @@ private: * Control position. */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, - const struct mission_item_triplet_s &_mission_item_triplet); + const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -367,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _mission_item_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -406,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - mission_item_triplet_s _mission_item_triplet = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; sensor_combined_s _sensor_combined = {0}; @@ -653,11 +653,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool mission_item_triplet_updated; - orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (mission_item_triplet_updated) { - orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); _setpoint_valid = true; } } @@ -700,7 +700,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +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) { @@ -713,12 +713,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; - if (mission_item_triplet.previous_valid) { - distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { - distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); @@ -751,11 +751,11 @@ void FixedwingPositionControl::navigation_capabilities_publish() bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, - const struct mission_item_triplet_s &mission_item_triplet) + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); + calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -767,7 +767,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -785,58 +785,56 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp(0) = mission_item_triplet.previous.lat; - prev_wp(1) = mission_item_triplet.previous.lon; + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = mission_item_triplet.current.lat; - prev_wp(1) = mission_item_triplet.current.lon; + prev_wp(0) = pos_sp_triplet.current.lat; + prev_wp(1) = pos_sp_triplet.current.lon; } - if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ @@ -847,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (mission_item_triplet.previous_valid) { + if (pos_sp_triplet.previous_valid) { target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; @@ -884,18 +882,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; float 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_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -914,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = mission_item_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.altitude; land_stayonground = true; } @@ -977,7 +975,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); @@ -1011,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -1022,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1037,14 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), - // (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset land state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1053,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; } @@ -1176,7 +1174,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1271,7 +1269,7 @@ FixedwingPositionControl::task_main() * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _mission_item_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1285,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 6e177bc4d9..e491911d3e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -410,7 +410,7 @@ l_local_position(const struct listener *l) void l_global_position_setpoint(const struct listener *l) { - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; 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 5ce4500cd0..bc20a4f470 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -117,7 +117,7 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _mission_items_sub; /**< mission item triplet */ + int _pos_sp_triplet_sub; /**< mission item triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -130,7 +130,7 @@ private: struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ - struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -236,7 +236,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _mission_items_sub(-1), + _pos_sp_triplet_sub(-1), /* publications */ _local_pos_sp_pub(-1), @@ -250,7 +250,7 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_arming, 0, sizeof(_arming)); memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); - memset(&_mission_items, 0, sizeof(_mission_items)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); _params.pos_p.zero(); @@ -405,10 +405,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_mission_items_sub, &updated); + orb_check(_pos_sp_triplet_sub, &updated); if (updated) - orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } float @@ -450,7 +450,7 @@ MulticopterPositionControl::task_main() _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -626,16 +626,15 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - if (_mission_items.current_valid) { - struct mission_item_s item = _mission_items.current; + if (_pos_sp_triplet.current.valid) { + struct position_setpoint_s current_sp = _pos_sp_triplet.current; - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + _pos_sp(2) = -(current_sp.alt - ref_alt); - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); + map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); - if (isfinite(_mission_items.current.yaw)) { - _att_sp.yaw_body = _mission_items.current.yaw; + if (isfinite(current_sp.yaw)) { + _att_sp.yaw_body = current_sp.yaw; } /* in case of interrupted mission don't go to waypoint but stay at current position */ @@ -688,7 +687,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { _vel_sp(2) = _params.land_speed; } @@ -790,7 +789,7 @@ MulticopterPositionControl::task_main() float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); float tilt_max = _params.tilt_max; if (!_control_mode.flag_control_manual_enabled) { - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; if (thr_min < 0.0f) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca5735509a..dfd07d3158 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,7 +64,7 @@ #include #include #include -#include +#include #include #include #include @@ -152,7 +152,7 @@ private: int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _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 */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -160,8 +160,10 @@ private: 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 mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + 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 */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -323,35 +325,24 @@ private: void set_rtl_item(); /** - * Helper function to get a loiter item + * Set position setpoint for mission item */ - void get_loiter_item(mission_item_s *item); + void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); /** * Helper function to get a takeoff item */ - void get_takeoff_item(mission_item_s *item); + void get_takeoff_setpoint(position_setpoint_s *pos_sp); /** * Publish a new mission item triplet for position controller */ - void publish_mission_item_triplet(); + void publish_position_setpoint_triplet(); /** * Publish vehicle_control_mode topic for controllers */ void publish_control_mode(); - - - /** - * Compare two mission items if they are equivalent - * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. - * - * @return true if equivalent, false otherwise - */ - bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); - - void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -385,7 +376,7 @@ Navigator::Navigator() : _capabilities_sub(-1), /* publications */ - _triplet_pub(-1), + _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -402,6 +393,7 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), + _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), _geofence_violation_warning_sent(false) @@ -414,14 +406,9 @@ Navigator::Navigator() : _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); - + memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_mission_item, 0, sizeof(struct mission_item_s)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; @@ -482,7 +469,6 @@ Navigator::parameters_update() void Navigator::global_position_update() { - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } @@ -938,23 +924,25 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { void Navigator::start_none() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_ready() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; @@ -964,7 +952,7 @@ Navigator::start_ready() _rtl_state = RTL_STATE_NONE; } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -973,38 +961,38 @@ Navigator::start_loiter() _do_takeoff = false; /* set loiter position if needed */ - if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _mission_item_triplet.current.altitude_is_relative = false; float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { - _mission_item_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + if (_rtl_state == RTL_STATE_LAND) { /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ _rtl_state = RTL_STATE_DESCEND; } } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; - get_loiter_item(&_mission_item_triplet.current); - - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -1020,8 +1008,7 @@ void Navigator::set_mission_item() { /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _reset_loiter_pos = true; _do_takeoff = false; @@ -1030,36 +1017,34 @@ Navigator::set_mission_item() bool onboard; unsigned index; - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - takeoff_alt_amsl += _home_pos.altitude; + float takeoff_alt_amsl = _pos_sp_triplet.current.alt; if (_vstatus.condition_landed) { /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ @@ -1067,30 +1052,29 @@ Navigator::set_mission_item() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + /* move current position setpoint to next */ + memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - /* set current item to TAKEOFF */ - get_takeoff_item(&_mission_item_triplet.current); + /* set current setpoint to takeoff */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = takeoff_alt_amsl; - _mission_item_triplet.current.altitude_is_relative = false; + _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.alt = takeoff_alt_amsl; + _pos_sp_triplet.current.yaw = NAN; + _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude); } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); @@ -1100,23 +1084,24 @@ Navigator::set_mission_item() } } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_triplet.current_valid = false; + _mission_item_valid = false; + _pos_sp_triplet.current.valid = false; warnx("ERROR: current WP can't be set"); } if (!_do_takeoff) { - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + mission_item_s item_next; + ret = _mission.get_next_mission_item(&item_next); if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); } else { /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.next.valid = false; } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -1129,8 +1114,8 @@ Navigator::start_rtl() } else { _rtl_state = RTL_STATE_RETURN; if (_reset_loiter_pos) { - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.altitude = _global_pos.alt; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; } } } @@ -1144,99 +1129,110 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) + if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = (double)_global_pos.lat / 1e7d; + _mission_item.lon = (double)_global_pos.lon / 1e7d; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); break; } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _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 + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); break; } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.altitude + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); break; } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.altitude; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); break; } @@ -1247,18 +1243,46 @@ Navigator::set_rtl_item() } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); +} + +void +Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +{ + sp->valid = true; + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* set home position for RTL item */ + sp->lat = _home_pos.lat; + sp->lon = _home_pos.lon; + sp->alt = _home_pos.altitude + _parameters.rtl_alt; + } else { + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt; + } + sp->yaw = item->yaw; + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + } else { + sp->type = SETPOINT_TYPE_NORMAL; + } } bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ - if (!_mission_item_triplet.current_valid) { + if (!_mission_item_valid) { return false; } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; } else { @@ -1269,10 +1293,10 @@ Navigator::check_mission_item_reached() } /* XXX TODO count turns */ - if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { return false; } @@ -1282,8 +1306,8 @@ Navigator::check_mission_item_reached() if (!_waypoint_position_reached) { float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; @@ -1294,11 +1318,11 @@ Navigator::check_mission_item_reached() float dist_z = -1.0f; /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; + float wp_alt_amsl = _mission_item.altitude; + if (_mission_item.altitude_is_relative) + _mission_itemt.altitude += _home_pos.altitude; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); @@ -1315,9 +1339,9 @@ Navigator::check_mission_item_reached() } if (!_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + 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_triplet.current.yaw - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } @@ -1330,14 +1354,14 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - if (_mission_item_triplet.current.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + if (_mission_item.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + 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; @@ -1389,67 +1413,16 @@ Navigator::on_mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - //item->yaw - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; - -} - -void -Navigator::get_takeoff_item(mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_TAKEOFF; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* append the home position to RTL item */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; - new_mission_item->altitude_is_relative = false; - new_mission_item->loiter_radius = _parameters.loiter_radius; - new_mission_item->acceptance_radius = _parameters.acceptance_radius; - } -} - -void -Navigator::publish_mission_item_triplet() +Navigator::publish_position_setpoint_triplet() { /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { + if (_pos_sp_triplet_pub > 0) { /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } @@ -1534,24 +1507,6 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (a.altitude_is_relative == b.altitude_is_relative && - fabs(a.lat - b.lat) < FLT_EPSILON && - fabs(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - a.autocontinue == b.autocontinue) { - return true; - } else { - return false; - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e0bb81e0eb..de510e0223 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -750,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 79a820c066..4c84c1f25e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/mission_item_triplet.h" -ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); +#include "topics/position_setpoint_triplet.h" +ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h similarity index 77% rename from src/modules/uORB/topics/mission_item_triplet.h rename to src/modules/uORB/topics/position_setpoint_triplet.h index b35eae6070..a8bd6b8e38 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -46,31 +46,39 @@ #include #include "../uORB.h" -#include "mission.h" - /** * @addtogroup topics * @{ */ +enum SETPOINT_TYPE +{ + SETPOINT_TYPE_NORMAL = 0, + SETPOINT_TYPE_LOITER, + SETPOINT_TYPE_TAKEOFF, + SETPOINT_TYPE_LAND, +}; + +struct position_setpoint_s +{ + bool valid; /**< true if point is valid */ + double lat; /**< latitude, in deg */ + double lon; /**< longitude, in deg */ + float alt; /**< altitude AMSL, in m */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ +}; + /** * Global position setpoint triplet in WGS84 coordinates. * * This are the three next waypoints (or just the next two or one). */ -struct mission_item_triplet_s +struct position_setpoint_triplet_s { - bool previous_valid; - bool current_valid; /**< flag indicating previous mission item is valid */ - bool next_valid; /**< flag indicating next mission item is valid */ - - struct mission_item_s previous; - struct mission_item_s current; - struct mission_item_s next; - - int previous_index; - int current_index; - int next_index; + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; }; /** @@ -78,6 +86,6 @@ struct mission_item_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); +ORB_DECLARE(position_setpoint_triplet); #endif From 1afe7f2c50016fbb39cb532252be171ba03ad357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 18:39:32 +0100 Subject: [PATCH 041/222] Added tune on IO upgrade error --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index edf2c77fba..6f4e1f3b56 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -199,10 +199,12 @@ then else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi fi From dda50c62bfd26463718f50d2f9c1cdbecc7de4ac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 22 Jan 2014 16:33:35 +1100 Subject: [PATCH 042/222] hmc5883: much faster calibration code with bug fixes this fixes two bugs in "hmc5883 calibrate" and also makes it much faster, so it can be run on every boot. It now uses the correct 2.5Ga range when calibrating, and fixes the expected values for X/Y/Z axes The basic calibration approach is similar to the APM2 driver, waiting for 10 good samples after discarding some initial samples. That allows the calibration to run fast enough that it can be done on every boot without causing too much boot delay. --- src/drivers/hmc5883/hmc5883.cpp | 160 +++++++++++++++++--------------- 1 file changed, 87 insertions(+), 73 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66e..9b9c11af29 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,32 +915,17 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; + + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; warnx("starting mag scale calibration"); - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { warn("failed to set 2Hz poll rate"); @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* Set to 2.5 Gauss. We ask for 3 to get the right part of + * the chained if statement above. */ + if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("failed to set 2.5 Ga range"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; } //warnx("periodic read %u", i); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#if 0 + warnx("measurement avg: %.6f %.6f %.6f", + (double)sum_excited[0]/good_count, + (double)sum_excited[1]/good_count, + (double)sum_excited[2]/good_count); +#endif float scaling[3]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } From 4524fe3e4888d569f855d1e7a82c8d5116636a0a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Jan 2014 16:20:43 +1100 Subject: [PATCH 043/222] px4fmu: added PWM_SERVO_SET_COUNT API this allows the balance between PWM channels and GPIOs to be changed after the main flight code has started, which makes it possible to change the balance with a parameter in APM --- src/drivers/drv_pwm_output.h | 4 ++++ src/drivers/px4fmu/fmu.cpp | 35 ++++++++++++++++++++++++++++++++++- 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37b..88da94b1ed 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4b1b0ed0bb..c067d363b7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1443,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); From 0c116e8de5c5c958b9463f147576f3e0377c4c00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 22:10:03 +0100 Subject: [PATCH 044/222] Implemented S.Bus 2 decoding support --- src/modules/px4iofirmware/sbus.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356a..4efa72dc1b 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.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 @@ -218,7 +218,30 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { + sbus_frame_drops++; + return false; + } + + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of these, else abort */ sbus_frame_drops++; return false; } From 8bdbce5fe2893353bf9582294c28ab2831f96a9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 22:27:04 +0100 Subject: [PATCH 045/222] We do not know all secret S.BUS2 codes yet --- src/modules/px4iofirmware/sbus.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 4efa72dc1b..4954477402 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -241,9 +241,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint case 0xE3: break; default: - /* we expect one of these, else abort */ - sbus_frame_drops++; - return false; + /* we expect one of the bits above, but there are some we don't know yet */ + break; } /* we have received something we think is a frame */ From 1cffa9d2f77f788f8446e0aceec60b7676a0a65f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 22:41:26 +0100 Subject: [PATCH 046/222] position_setpoint_triplet refactoring finished --- .../fw_pos_control_l1_main.cpp | 26 +++++++++---------- src/modules/mavlink/orb_listener.c | 15 ++++------- src/modules/mavlink/orb_topics.h | 3 +-- src/modules/mavlink_onboard/orb_topics.h | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 8 +++--- src/modules/navigator/navigator_main.cpp | 21 +++++++++------ src/modules/sdlog2/sdlog2.c | 13 ++++------ src/modules/sdlog2/sdlog2_messages.h | 9 +++---- .../uORB/topics/position_setpoint_triplet.h | 9 ++++--- 9 files changed, 51 insertions(+), 55 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 3889012c9d..f3d688646d 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 @@ -828,7 +828,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -845,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (pos_sp_triplet.previous_valid) { + if (pos_sp_triplet.previous.valid) { target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; @@ -877,7 +877,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // /* do not go down too early */ // if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; // } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param @@ -888,12 +888,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -912,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; } @@ -1009,7 +1009,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1020,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1042,7 +1042,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _loiter_hold = false; /* reset land state */ - if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1051,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; } @@ -1283,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index e491911d3e..0b8ac6d3db 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -411,22 +411,17 @@ void l_global_position_setpoint(const struct listener *l) { struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (!triplet.current_valid) + if (!triplet.current.valid) return; - if (triplet.current.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - coordinate_frame, + MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), + (int32_t)(triplet.current.alt * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } @@ -804,7 +799,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 4d428406ac..30ba348cf6 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -51,9 +51,8 @@ #include #include #include -#include +#include #include -#include #include #include #include diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 86bfa26f25..bbc9f6e66a 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #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 bc20a4f470..d3e39e3a02 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -117,7 +117,7 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< mission item triplet */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -687,7 +687,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -789,7 +789,7 @@ MulticopterPositionControl::task_main() float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); float tilt_max = _params.tilt_max; if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; if (thr_min < 0.0f) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dfd07d3158..ba51b024f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -972,10 +972,10 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _pos_sp_triplet.current.altitude = 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)); } else { - _pos_sp_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } @@ -987,6 +987,8 @@ Navigator::start_loiter() } } + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; @@ -1052,7 +1054,7 @@ Navigator::set_mission_item() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; @@ -1067,14 +1069,14 @@ Navigator::set_mission_item() _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (item.nav_cmd == NAV_CMD_LAND) { + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); @@ -1207,7 +1209,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); break; } case RTL_STATE_LAND: { @@ -1258,9 +1260,12 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ } else { sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude; } 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; } else if (item->nav_cmd == NAV_CMD_LAND) { @@ -1320,7 +1325,7 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) - _mission_itemt.altitude += _home_pos.altitude; + wp_alt_amsl += _home_pos.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index de510e0223..46f8ed8278 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -908,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -1263,18 +1263,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); + orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; - log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 503dc0afca..98736dd211 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -210,16 +210,13 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t altitude_is_relative; int32_t lat; int32_t lon; - float altitude; + float alt; float yaw; - uint8_t nav_cmd; + uint8_t type; float loiter_radius; int8_t loiter_direction; - float acceptance_radius; - float time_inside; float pitch_min; }; @@ -307,7 +304,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index a8bd6b8e38..4b57833b63 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -61,12 +61,15 @@ enum SETPOINT_TYPE struct position_setpoint_s { - bool valid; /**< true if point is valid */ + bool valid; /**< true if setpoint is valid */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ - float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */ - enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float loiter_radius; /**< loiter radius (only for fixed wing), in m */ + int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; /** From 55f845888bece8aaa49ebc2b5e25663b6c8f3f3c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 22:41:56 +0100 Subject: [PATCH 047/222] px4fmu-v2_test makefile fixed, CMSIS added --- makefiles/config_px4fmu-v2_test.mk | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index d5a50cfae8..8623c0584b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,11 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS + # # Transitional support - add commands from the NuttX export archive. # From 58792c5ca6e42bc251dd3c92b0e79217ff5d5403 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 24 Jan 2014 00:06:10 +0100 Subject: [PATCH 048/222] Use double for lat/lon in vehicle_global_position topic, use filed names lat, lon, alt, vel_n, vel_e, vel_d for global positions --- src/drivers/frsky_telemetry/frsky_data.c | 6 +-- src/examples/fixedwing_control/main.c | 6 +-- .../att_pos_estimator_ekf/KalmanNav.cpp | 7 ++-- .../attitude_estimator_ekf_main.cpp | 6 +-- src/modules/commander/commander.cpp | 10 ++--- src/modules/fixedwing_backside/fixedwing.cpp | 8 ++-- .../fixedwing_pos_control_main.c | 2 +- .../fw_att_control/fw_att_control_main.cpp | 6 +-- .../fw_pos_control_l1_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 6 +-- src/modules/mavlink/orb_listener.c | 21 +++++----- src/modules/navigator/navigator_main.cpp | 40 +++++++++---------- .../position_estimator_inav_main.c | 14 +++---- src/modules/sdlog2/sdlog2.c | 10 ++--- src/modules/uORB/topics/home_position.h | 2 +- .../uORB/topics/vehicle_global_position.h | 17 ++++---- 16 files changed, 76 insertions(+), 87 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 63b2d2d29f..e201ecbb3d 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -230,11 +230,11 @@ void frsky_send_frame2(int uart) struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat = frsky_format_gps(abs(global_pos.lat)); lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; - lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon = frsky_format_gps(abs(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e00077..067d773649 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index aca3fe7b67..8e88130e15 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -318,10 +318,9 @@ void KalmanNav::updatePublications() _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6a1bec1531..66ec20b954 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -414,9 +414,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; - vel(0) = global_pos.vx; - vel(1) = global_pos.vy; - vel(2) = global_pos.vz; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838cf..d51bb63ffb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1009,12 +1009,12 @@ int commander_thread_main(int argc, char *argv[]) /* copy position data to uORB home message, store it locally as well */ - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d6..f7c0b61488 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e0..888dd09423 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e49b3c1405..dc2196de6d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } 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 f3d688646d..a62b53221c 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 @@ -1262,7 +1262,7 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f9c718d2c..7c23488d7e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -638,9 +638,9 @@ handle_message(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 0b8ac6d3db..7f6237535e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; @@ -247,10 +248,10 @@ l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -380,13 +381,13 @@ l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(MAVLINK_COMM_0, global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.0f, + (global_pos.alt - home.alt) * 1000.0f, + global_pos.vel_n * 100.0f, + global_pos.vel_e * 100.0f, + global_pos.vel_d * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } @@ -657,11 +658,9 @@ l_optical_flow(const struct listener *l) void l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ba51b024f6..cfcc886b68 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -833,11 +833,11 @@ Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } if (_fence_valid) { @@ -964,11 +964,11 @@ Navigator::start_loiter() if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + 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) { @@ -1063,8 +1063,8 @@ Navigator::set_mission_item() /* set current setpoint to takeoff */ - _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.alt = takeoff_alt_amsl; _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; @@ -1111,7 +1111,7 @@ Navigator::start_rtl() { _do_takeoff = false; if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; @@ -1133,15 +1133,15 @@ Navigator::set_rtl_item() case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + float climb_alt = _home_pos.alt + _parameters.rtl_alt; if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); } _mission_item_valid = true; - _mission_item.lat = (double)_global_pos.lat / 1e7d; - _mission_item.lon = (double)_global_pos.lon / 1e7d; + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; @@ -1158,7 +1158,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); break; } case RTL_STATE_RETURN: { @@ -1194,7 +1194,7 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.altitude + _parameters.land_alt; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; @@ -1220,7 +1220,7 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.altitude; + _mission_item.altitude = _home_pos.alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; @@ -1256,11 +1256,11 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* set home position for RTL item */ sp->lat = _home_pos.lat; sp->lon = _home_pos.lon; - sp->alt = _home_pos.altitude + _parameters.rtl_alt; + sp->alt = _home_pos.alt + _parameters.rtl_alt; } else { sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; } sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; @@ -1325,10 +1325,10 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.altitude; + wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); if (_do_takeoff) { 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 02fa6a8f28..af04bb0bc4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -840,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; } /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { @@ -860,7 +856,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (local_pos.v_z_valid) { - global_pos.vz = local_pos.vz; + global_pos.vel_d = local_pos.vz; } global_pos.yaw = local_pos.yaw; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 46f8ed8278..9bac2958e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1252,12 +1252,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e9..08d11abaea 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e372..ae771ca008 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,7 +54,7 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. @@ -65,14 +65,13 @@ struct vehicle_global_position_s uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ - float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ }; /** From d8c1131f1e8e61bcb15b0faa36de1bba00e9716d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Jan 2014 11:06:33 +1100 Subject: [PATCH 049/222] px4io: improved reliability of forceupdate re-starting px4io this adds a 0.1s delay after update to give px4io time to boot. It removes the need for the user to reboot after an IO update --- src/drivers/px4io/px4io_uploader.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee1..d9f991210d 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -226,6 +227,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } From 65118f0c2ef6e4305259a35751c8cb92d751b671 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:26:13 +0100 Subject: [PATCH 050/222] Disable debug in the airspeed sensor driver - prevents console spam if it fails (and on probing during startup) --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936c..f73a3ef01e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); From 4f78c3e60596d1b596e5ebcf4bb4e101a5b356e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:27:28 +0100 Subject: [PATCH 051/222] Disable PX4IO debug - spams console on comms failure. Each command does report the failure separately, so we get a better feedback level without the spam. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a54bb39646..519aa96eb7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -505,7 +505,7 @@ PX4IO::PX4IO(device::Device *interface) : /* open MAVLink text channel */ _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = true; + _debug_enabled = false; _servorail_status.rssi_v = 0; } From 2f968357a368ee59f53d75119b893487abd3883b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:28:04 +0100 Subject: [PATCH 052/222] Make the protocol version more descriptive - helps to understand when / how px4io detect fails. --- src/drivers/px4io/px4io.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 519aa96eb7..6f1323fced 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -580,6 +580,12 @@ PX4IO::init() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol == _io_reg_get_error) { + log("failed to communicate with IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort."); + return -1; + } + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); From 1960f7d6c5c502860ad4f2520eae364a4abfe9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:29:27 +0100 Subject: [PATCH 053/222] Initialize null pointers correctly, always set the pointer to null after deletes. Remove some verbosity from startup and do not try to initialise IO when we just want to reboot it into the bootloader. --- src/drivers/px4io/px4io.cpp | 39 ++++++++++++++++++++++++++----------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f1323fced..df847a64da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -451,7 +451,7 @@ private: namespace { -PX4IO *g_dev; +PX4IO *g_dev = nullptr; } @@ -780,8 +780,6 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - log("starting"); - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* @@ -815,8 +813,6 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; - log("ready"); - /* lock against the ioctl handler */ lock(); @@ -1679,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1703,7 +1710,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } if (ret) return ret; @@ -2705,6 +2723,7 @@ px4io_main(int argc, char *argv[]) printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2788,10 +2807,6 @@ px4io_main(int argc, char *argv[]) delete interface; errx(1, "driver alloc failed"); } - - if (OK != g_dev->init()) { - warnx("driver init failed, still trying.."); - } } uint16_t arg = atol(argv[2]); @@ -2803,6 +2818,7 @@ px4io_main(int argc, char *argv[]) // tear down the px4io instance delete g_dev; + g_dev = nullptr; // upload the specified firmware const char *fn[2]; @@ -2861,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } From 73a483c2657d97619021b85759bc742f637cfff4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:30:40 +0100 Subject: [PATCH 054/222] Finally fix the timing race between the IO driver, IO uploader and the on-IO firmware by making the uploader tolerant of timing offsets. --- src/drivers/px4io/px4io_uploader.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d9f991210d..dd8abbac5a 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -121,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ From 92a6c7d7344ae0a463e0c04c3b5bc6cf8f4ddc53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:33:04 +0100 Subject: [PATCH 055/222] Set timeouts back to short, now that we have multiple tries in the uploader. This ensures we try often enough in the 200 ms IO bootloader wait phase to hit it. --- src/drivers/px4io/uploader.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e23..55f63eef96 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); From c5cb3cfd2187c82b11bb1f12d644e77ecd807efc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:34:42 +0100 Subject: [PATCH 056/222] Make the IO mixer upload report not only a global success / fail flag, but on transfer basis. Also use a crude lock to avoid updating the mixer while it runs (we have no proper mutexes on IO, and this is a pure read/write locking case with two locks, which should make the execution even with this crude approach thread-safe). --- src/modules/px4iofirmware/mixer.cpp | 30 ++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a9..2e79f0ac65 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.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 @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void From 15f8e5acf12125eb4fb7b3d5d530b3e27c25f34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:35:29 +0100 Subject: [PATCH 057/222] =?UTF-8?q?Make=20in=20the=20comments=20explicit?= =?UTF-8?q?=20that=20we=20don=E2=80=99t=20do=20anything=20here=20under=20n?= =?UTF-8?q?ormal=20circumstances=20to=20make=20it=20less=20tempting=20to?= =?UTF-8?q?=20comment=20out=20the=20helpful=20debug=20tools=20in=20this=20?= =?UTF-8?q?section.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/px4io.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e2da359394..d4c25911e3 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -295,10 +295,12 @@ user_start(int argc, char *argv[]) check_reboot(); - /* check for debug activity */ + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); From 33688fec9c66692e88a1b328fd022adc6e906853 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:36:13 +0100 Subject: [PATCH 058/222] Make the sensors app less verbose --- src/modules/sensors/sensors.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9903a90a0e..23f20b0cb2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -797,7 +797,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -837,7 +836,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -1507,9 +1505,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); From bd15653b173029dfc12c3d4a73b897570e0867c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:37:01 +0100 Subject: [PATCH 059/222] Use the proper status registers for locking out from mixer updates and return the value of the mixer change. --- src/modules/px4iofirmware/registers.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ad44730736..2c437d2c0c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -382,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -540,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } From 880342b9c1e90e0c22180d7fe1411d0988d97a49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:37:34 +0100 Subject: [PATCH 060/222] Missing header for mixer status change. --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bffbc0ce2a..393e0560e8 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -178,7 +178,7 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. From 06227331ea053d8cc297ecd1bf7afd1da1677c39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 18:21:28 +0100 Subject: [PATCH 061/222] Checking out registers page state from master, as this is clearly a symptom of a bad merge --- src/modules/px4iofirmware/registers.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 19fc61f78b..2c437d2c0c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -89,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -600,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } From e07d91613b0a90fd4a9ce8c2e10d3bff8a1ebc44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 18:24:54 +0100 Subject: [PATCH 062/222] Remove unused field --- src/modules/px4iofirmware/safety.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 2ce479ffd3..ff2e4af6ea 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -45,7 +45,6 @@ #include "px4io.h" static struct hrt_call arming_call; -static struct hrt_call heartbeat_call; static struct hrt_call failsafe_call; /* From f2f94f0f176b9a2d818a0849eae18f018d7fb5a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:03:21 +0100 Subject: [PATCH 063/222] IO driver: Variable name and comment cleanup, no binary / functionality changes. --- src/drivers/px4io/px4io.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64da..e83fa68d5e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -444,7 +444,7 @@ private: * @param vservo vservo register * @param vrssi vrssi register */ - void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + void io_handle_vservo(uint16_t vservo, uint16_t vrssi); }; @@ -1357,7 +1357,10 @@ PX4IO::io_get_status() uint16_t regs[6]; int ret; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + /* get + * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, + * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI + * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) From bafcbd99a695a3c4d478fb58e1d53940f331392f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:04:16 +0100 Subject: [PATCH 064/222] Stop setting RSSI by cross-reading servo status. --- src/drivers/px4io/px4io.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e83fa68d5e..4bdf530a79 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1457,13 +1457,6 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } - /* set RSSI */ - - if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; - } - /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); From aff11d6d8611d5e5b72742d2bee4132168fefa72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:06:53 +0100 Subject: [PATCH 065/222] =?UTF-8?q?IO=20firmware:=20Use=20right=20base=20r?= =?UTF-8?q?eg=20value=20-=20since=20the=20wrong=20one=20had=20the=20same?= =?UTF-8?q?=20value=20this=20hasn=E2=80=99t=20been=20an=20issue,=20but=20i?= =?UTF-8?q?t=20would=20have=20become=20one=20once=20one=20of=20them=20chan?= =?UTF-8?q?ged.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 393e0560e8..ee9d5b0c41 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -97,7 +97,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] From b06d199129d57eabe2b73da713c9ac4ce98a68bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:10:48 +0100 Subject: [PATCH 066/222] Fixed year in controls.c comment. --- src/modules/px4iofirmware/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5e2c92bf46..ed37cc43d4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.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 From ebc7cb03b726ebfb864e770a82b92bb67b6bfd4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:24:12 +0100 Subject: [PATCH 067/222] =?UTF-8?q?=C2=ABflighttermination=20state=C2=BB?= =?UTF-8?q?=20replaced=20by=20more=20general=20=C2=ABfailsafe=20state?= =?UTF-8?q?=C2=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 39 ++- .../commander/state_machine_helper.cpp | 60 ++--- src/modules/commander/state_machine_helper.h | 4 +- .../fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 252 ++++++++++-------- .../uORB/topics/vehicle_control_mode.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 11 +- 7 files changed, 206 insertions(+), 164 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d51bb63ffb..7715f73e07 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe 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 - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); @@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); - } + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); } + // TODO add other failsafe modes if position estimate not available } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // 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 (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1176,12 +1175,12 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + if (arming_state_changed || main_state_changed || failsafe_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ffc..90ca2a6d23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,7 +63,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,39 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* only check transition if the new state is actually different from the current one */ + if (new_failsafe_state == status->failsafe_state) { + ret = TRANSITION_NOT_CHANGED; - } else { + } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_RTL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_TERMINATION: + ret = TRANSITION_CHANGED; + break; - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + default: + break; } - return ret; + if (ret == TRANSITION_CHANGED) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + } + } + + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f37..f04879ff9e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index dc2196de6d..17b1028f9a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cfcc886b68..89a62e1661 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -643,87 +643,101 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + stick_mode = true; + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + 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); + } + 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); + stick_mode = true; + } + } + } + + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } + + } 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); + } + } + } } - stick_mode = true; } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - 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); - } - 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); - stick_mode = true; - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + dispatch(EVENT_RTL_REQUESTED); - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } 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); - } - } - } + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -1442,40 +1456,74 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + default: break; + } - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1493,10 +1541,6 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985c..f9f8414df8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -92,7 +92,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f51..a3a862d858 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -82,9 +82,11 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, + FAILSAFE_STATE_RTL, + FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +175,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +226,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** From 92ddf7903b4a540215905f01acd1819eac1f176d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:37:26 +0100 Subject: [PATCH 068/222] commander: more user-friendly states indication --- src/modules/commander/commander.cpp | 43 ++++++++++++++++++++---- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 ++- 3 files changed, 40 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7715f73e07..722230effa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -575,6 +575,26 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1108,8 +1128,8 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { @@ -1127,13 +1147,11 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } } else { @@ -1179,9 +1197,20 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed || failsafe_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); + /* print new state */ + if (arming_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 89a62e1661..8ecc28b11d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -802,7 +802,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a3a862d858..73102090f0 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { From 062b64a1e21406cf787d93aa53921ce0ef6627fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:49:33 +0100 Subject: [PATCH 069/222] navigator: RTL on failsafe bug fixed --- src/modules/navigator/navigator_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8ecc28b11d..d72ed70585 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -729,7 +729,9 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - dispatch(EVENT_RTL_REQUESTED); + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } } else { /* shouldn't act */ From eee2508644ea1ca3b267ed89db6d0deb8fe0d3e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:14:19 +0100 Subject: [PATCH 070/222] Add additional flags to RC topic, not used yet. --- src/drivers/drv_rc_input.h | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 6b87141e9d..0afe2f16f0 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -92,6 +92,34 @@ struct rc_input_values { /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; + /** + * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. + * Only the true state is reliable, as there are some (PPM) receivers on the market going + * into failsafe without telling us explicitly. + * */ + bool rc_failsafe; + + /** + * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. + * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. + * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. + * */ + bool rc_lost; + + /** + * Number of lost RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_lost_frame_count; + + /** + * Number of total RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_total_frame_count; + /** Input source */ enum RC_INPUT_SOURCE input_source; From 57d38bc8cec1362308f632e74e99485f82a35501 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:17:26 +0100 Subject: [PATCH 071/222] Clean up RC related metadata, put everything into the RC data page. This ensures atomic reads, makes the reads more efficient and allows for some headroom for more RC flags. The IO driver side is updated as well, however, these flags are not published yet. --- src/drivers/px4io/px4io.cpp | 10 ++++-- src/modules/px4iofirmware/controls.c | 49 +++++++++++++++++++++------ src/modules/px4iofirmware/protocol.h | 14 +++++--- src/modules/px4iofirmware/px4io.h | 3 +- src/modules/px4iofirmware/registers.c | 8 +++-- src/modules/px4iofirmware/sbus.c | 21 +++++++----- 6 files changed, 76 insertions(+), 29 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4bdf530a79..382acb6d08 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1763,6 +1763,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + uint16_t io_status_flags = flags; printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), @@ -1770,8 +1771,6 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -1824,6 +1823,11 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); + printf("status 0x%04x%s", flags, + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + ); for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); @@ -1831,7 +1835,7 @@ PX4IO::print_status() printf("\n"); if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { - int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); if ((frame_len - raw_inputs * 2000 - 3000) < 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ed37cc43d4..1cac977d7a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -97,28 +97,57 @@ controls_tick() { /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; +#ifdef ADC_RSSI + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } +#endif + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); if (dsm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; else - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - rssi = 255; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + bool sbus_failsafe, sbus_frame_drop; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + + rssi = 255; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + rssi = 100; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + rssi = 0; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } /* switch S.Bus output pin as needed */ @@ -136,12 +165,9 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { - /* XXX sample RSSI properly here */ - rssi = 255; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } perf_end(c_gather_ppm); @@ -150,6 +176,9 @@ controls_tick() { if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -247,7 +276,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0 || rssi == 0) { + if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3a..738dc7d6ed 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,7 +111,6 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ @@ -128,8 +127,6 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ -#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -140,7 +137,16 @@ /* array of raw RC input values, microseconds */ #define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ -#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ #define PX4IO_PAGE_RC_INPUT 5 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ee9d5b0c41..a2e1ed6cde 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -96,6 +96,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] #define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) @@ -215,7 +216,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 2c437d2c0c..477efa0826 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,8 +90,6 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, - [PX4IO_P_STATUS_NRSSI] = 0, - [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 4954477402..32ab2a6988 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_ * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, rssi, max_channels); + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -289,20 +289,23 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ /* report that we failed to read anything valid off the receiver */ - *rssi = 0; + *sbus_failsafe = true; + *sbus_frame_drop = true; return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + /* set a special warning flag * * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; } - *rssi = 255; - return true; } From c7f05539382a48d7ecaad3bfdf71261cde2ee8c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:50:34 +0100 Subject: [PATCH 072/222] cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND --- src/modules/commander/commander.cpp | 35 ++++++- .../commander/state_machine_helper.cpp | 99 ++++++++++--------- src/modules/navigator/navigator_main.cpp | 58 ++++++++++- .../uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 7 +- 5 files changed, 148 insertions(+), 52 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 722230effa..bca0569d5f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ + /* evaluate the main state machine according to mode switches */ res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { @@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } + + } else if (armed.armed) { + /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } } - // TODO add other failsafe modes if position estimate not available } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 90ca2a6d23..77edea546d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_EASY: - - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_AUTO: - - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; } - if (ret == TRANSITION_CHANGED) { + break; + + case MAIN_STATE_EASY: + + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_AUTO: + + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + } + + if (ret == TRANSITION_CHANGED) { + if (current_state->main_state != new_main_state) { current_state->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_failsafe_state == status->failsafe_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { + ret = TRANSITION_NOT_CHANGED; + } - } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - ret = TRANSITION_CHANGED; + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } break; case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; @@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } if (ret == TRANSITION_CHANGED) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d72ed70585..388fefd024 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -216,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -292,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -733,6 +734,12 @@ Navigator::task_main() dispatch(EVENT_RTL_REQUESTED); } + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + } else { /* shouldn't act */ dispatch(EVENT_NONE_REQUESTED); @@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, @@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* 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_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, @@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, @@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, @@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, + { + /* STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + }, }; void @@ -1142,6 +1165,27 @@ Navigator::start_rtl() set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; + + mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); +} + void Navigator::set_rtl_item() { @@ -1508,9 +1552,21 @@ Navigator::publish_control_mode() navigator_enabled = true; break; + case FAILSAFE_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + case FAILSAFE_STATE_TERMINATION: navigator_enabled = true; /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index f9f8414df8..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 73102090f0..a5988d3ba4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -84,9 +84,10 @@ typedef enum { } hil_state_t; typedef enum { - FAILSAFE_STATE_NORMAL = 0, - FAILSAFE_STATE_RTL, - FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX } failsafe_state_t; From 7d2efe9367787cdfc4590f335f600f3b79b2cbc7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:52:33 +0100 Subject: [PATCH 073/222] commander, navigator: minor cleanup (refactoring), code style fixed --- src/modules/commander/commander.cpp | 19 +- .../commander/state_machine_helper.cpp | 24 +- src/modules/navigator/navigator_main.cpp | 358 +++++++++++------- 3 files changed, 243 insertions(+), 158 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bca0569d5f..15f229bf00 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -405,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -447,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) ret = true; @@ -486,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -508,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* 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 @@ -900,6 +904,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { 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) { status.battery_voltage = battery.voltage_filtered_v; @@ -1135,6 +1140,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); } @@ -1164,14 +1170,18 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state == MAIN_STATE_AUTO) { /* check if AUTO mode still allowed */ transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { /* LAND mode denied, switch to failsafe state TERMINATION */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } @@ -1181,15 +1191,20 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.armed) { /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate), try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 77edea546d..1be0a16b88 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -219,7 +220,7 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -232,9 +233,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -243,8 +244,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + if (status->condition_local_position_valid || + status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -253,7 +254,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need global position estimate */ - if (current_state->condition_global_position_valid) { + if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -261,8 +262,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m } if (ret == TRANSITION_CHANGED) { - if (current_state->main_state != new_main_state) { - current_state->main_state = new_main_state; + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; } else { @@ -378,11 +379,14 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; + case FAILSAFE_STATE_RTL: if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; break; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 388fefd024..9b5cfa6b4b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -166,12 +166,12 @@ private: bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -358,7 +358,7 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -490,16 +490,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -516,6 +520,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -558,11 +563,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -578,7 +585,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -649,6 +656,7 @@ Navigator::task_main() if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { if (_vstatus.main_state == MAIN_STATE_AUTO) { bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ @@ -656,21 +664,27 @@ Navigator::task_main() if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + stick_mode = true; + } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { dispatch(EVENT_LOITER_REQUESTED); 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); } + 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); @@ -696,15 +710,18 @@ Navigator::task_main() case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + break; case NAV_STATE_RTL: if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + break; default: @@ -717,12 +734,14 @@ Navigator::task_main() if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } } } + } else { /* not in AUTO mode */ dispatch(EVENT_NONE_REQUESTED); @@ -765,7 +784,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -786,6 +805,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -794,15 +814,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -852,48 +872,55 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -903,8 +930,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ + { + /* STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, @@ -915,7 +942,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, @@ -925,8 +952,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -936,8 +963,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -948,7 +975,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, { - /* STATE_LAND */ + /* STATE_LAND */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -1013,6 +1040,7 @@ Navigator::start_loiter() 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)); + } else { _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); @@ -1065,22 +1093,22 @@ Navigator::set_mission_item() position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; @@ -1108,6 +1136,7 @@ Navigator::set_mission_item() _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; @@ -1116,13 +1145,16 @@ Navigator::set_mission_item() if (_do_takeoff) { mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_valid = false; @@ -1136,6 +1168,7 @@ Navigator::set_mission_item() if (ret == OK) { position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + } else { /* this will fail for the last WP */ _pos_sp_triplet.next.valid = false; @@ -1149,17 +1182,21 @@ void Navigator::start_rtl() { _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; + } else { _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { _mission_item.altitude_is_relative = false; _mission_item.altitude = _global_pos.alt; } } } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); @@ -1191,118 +1228,123 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + float climb_alt = _home_pos.alt + _parameters.rtl_alt; + + if (_vstatus.condition_landed) { + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); + break; } - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); - break; - } case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; + _mission_item_valid = true; - _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 - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _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 + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _pos_sp_triplet.next.valid = false; + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; + _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _pos_sp_triplet.next.valid = false; + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; + _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _pos_sp_triplet.next.valid = false; + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } publish_position_setpoint_triplet(); @@ -1312,28 +1354,35 @@ void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { sp->valid = true; + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* set home position for RTL item */ sp->lat = _home_pos.lat; sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + } 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; + if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; + } else if (item->nav_cmd == NAV_CMD_LAND) { sp->type = SETPOINT_TYPE_LAND; + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { sp->type = SETPOINT_TYPE_LOITER; + } else { sp->type = SETPOINT_TYPE_NORMAL; } @@ -1350,6 +1399,7 @@ Navigator::check_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; + } else { /* For fw there is currently no landing detector: * make sure control is not stopped when overshooting the landing waypoint */ @@ -1364,7 +1414,7 @@ Navigator::check_mission_item_reached() _mission_item.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); @@ -1384,18 +1434,20 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; + if (_mission_item.altitude_is_relative) wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1407,9 +1459,11 @@ Navigator::check_mission_item_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); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1419,20 +1473,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _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); } } - + /* 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) { + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1445,6 +1501,7 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); @@ -1452,23 +1509,28 @@ Navigator::on_mission_item_reached() if (_mission.current_mission_available()) { set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* 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); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1546,6 +1608,7 @@ Navigator::publish_control_mode() default: break; } + break; case FAILSAFE_STATE_RTL: @@ -1583,6 +1646,7 @@ Navigator::publish_control_mode() /* navigator has control, set control mode flags according to nav state*/ if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; + if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; @@ -1591,6 +1655,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + } else { _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1669,8 +1734,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); From b7c69262a7e4d51fb7806ab40a4dbb2b0ea4f75b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:58:50 +0100 Subject: [PATCH 074/222] state_machine_helper: added missed transition to FAILSAFE_STATE_LAND, transition conditions fixed --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1be0a16b88..c7256583a3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -377,17 +377,28 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - if (status->condition_global_position_valid) { + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; case FAILSAFE_STATE_TERMINATION: + /* always allowed */ ret = TRANSITION_CHANGED; break; From a737a2a4061babb3de524ac2001a659786081e4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 13:22:26 +0100 Subject: [PATCH 075/222] RSSI and SBUS out config now handled as setup feature flags. --- src/modules/px4iofirmware/protocol.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 738dc7d6ed..69c12d877d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -163,6 +163,10 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ From c841929e3f617e67adec6606b3ec6517aa455834 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 14:12:27 +0100 Subject: [PATCH 076/222] =?UTF-8?q?commander:=20=C2=ABhome=20position=20se?= =?UTF-8?q?t=C2=BB=20condition=20fixed,=20failsafe=20fixes,=20navigator:?= =?UTF-8?q?=20state=20indication=20bugfix,=20control=5Fmode=20fixes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 264 +++++++++++------------ src/modules/navigator/navigator_main.cpp | 34 +-- 2 files changed, 152 insertions(+), 146 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 15f229bf00..0e91728004 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -565,7 +565,6 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -597,7 +596,8 @@ int commander_thread_main(int argc, char *argv[]) char *failsafe_states_str[FAILSAFE_STATE_MAX]; failsafe_states_str[0] = "NORMAL"; failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "TERMINATION"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -628,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -1027,13 +1028,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed && global_position.valid) { + /* copy position data to uORB home message, store it locally as well */ - - home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; @@ -1050,164 +1050,160 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + /* start RC input check */ + if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; + + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } } + } - status.rc_signal_lost = false; + status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t res; // store all transitions results here - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * do it only for rotary wings */ - if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - } else { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; - } - - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - 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) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } - - stick_on_counter = 0; - - } else { - stick_on_counter++; - } } else { - stick_on_counter = 0; - } - - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } - - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); - } - - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); - } - } - - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); - - /* evaluate the main state machine according to mode switches */ - res = check_main_state_machine(&status); - - if (res == TRANSITION_CHANGED) { - tune_positive(); - - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + stick_off_counter++; } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; - } + stick_off_counter = 0; + } - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + 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) { + print_reject_arm("NOT ARMING: Press safety switch first."); - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); - - } else if (res == TRANSITION_DENIED) { - /* LAND mode denied, switch to failsafe state TERMINATION */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } - } + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else if (armed.armed) { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + stick_on_counter = 0; + + } else { + stick_on_counter++; + } + + } else { + stick_on_counter = 0; + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } + + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine according to mode switches */ + res = check_main_state_machine(&status); + + if (res == TRANSITION_CHANGED) { + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); } else if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } - } else if (res == TRANSITION_DENIED) { - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (armed.armed) { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9b5cfa6b4b..7c37237ff2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -411,11 +411,13 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_item, 0, sizeof(struct mission_item_s)); + memset(&nav_states_str, 0, sizeof(nav_states_str)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; nav_states_str[2] = "LOITER"; nav_states_str[3] = "MISSION"; nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -750,6 +752,7 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); } @@ -981,7 +984,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, @@ -1220,7 +1223,7 @@ Navigator::start_land() _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.yaw = NAN; - mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); + mavlink_log_info(_mavlink_fd, "[navigator] land started"); } void @@ -1616,14 +1619,7 @@ Navigator::publish_control_mode() break; case FAILSAFE_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + navigator_enabled = true; break; case FAILSAFE_STATE_TERMINATION: @@ -1647,7 +1643,8 @@ Navigator::publish_control_mode() if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; - if (myState == NAV_STATE_READY) { + switch (myState) { + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1655,14 +1652,27 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + break; - } else { + case NAV_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; + break; } } From 16eb68f2e9b1de47b4f77d2ccaf72f102eb67fdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:13:57 +0100 Subject: [PATCH 077/222] Allow the setup flags to control the S.BUS / RSSI port instead of doing some wild, likely incorrect, guesses. --- src/modules/px4iofirmware/controls.c | 21 +++++-------- src/modules/px4iofirmware/registers.c | 44 +++++++++++++++++++++++++-- 2 files changed, 49 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 1cac977d7a..e70eaed093 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -98,13 +98,15 @@ controls_tick() { uint16_t rssi = 0; #ifdef ADC_RSSI - unsigned counts = adc_measure(ADC_RSSI); - if (counts != 0xffff) { - /* use 1:1 scaling on 3.3V ADC input */ - unsigned mV = counts * 3300 / 4096; + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 */ - rssi = mV / 13; + /* scale to 0..253 */ + rssi = mV / 13; + } } #endif @@ -150,13 +152,6 @@ controls_tick() { } - /* switch S.Bus output pin as needed */ - if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { - #ifdef ENABLE_SBUS_OUT - ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); - #endif - } - perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 477efa0826..2d7a1fe0d1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -148,7 +148,12 @@ uint16_t r_page_scratch[32]; */ volatile uint16_t r_page_setup[] = { +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + /* default to RSSI ADC functionality */ + [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, +#else [PX4IO_P_SETUP_FEATURES] = 0, +#endif [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, @@ -166,7 +171,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; -#define PX4IO_P_SETUP_FEATURES_VALID (0) +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) +#else +#define PX4IO_P_SETUP_FEATURES_VALID 0 +#endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ @@ -442,9 +454,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; - r_setup_features = value; - /* no implemented feature selection at this point */ + /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ + + /* switch S.Bus output pin as needed */ + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); + + /* disable the conflicting options */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } + #endif + + /* disable the conflicting options with ADC RSSI */ + if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ + if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* apply changes */ + r_setup_features = value; break; From 731ab465b3d7d40ffb5ce3ca3d14660c6fee1ae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:22:54 +0100 Subject: [PATCH 078/222] Add support in the IO driver to control the S.Bus / RSSI port. --- src/drivers/drv_rc_input.h | 8 ++- src/drivers/drv_sbus.h | 58 ++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 103 ++++++++++++++++++++++++++++++++++-- 3 files changed, 163 insertions(+), 6 deletions(-) create mode 100644 src/drivers/drv_sbus.h diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 0afe2f16f0..715df7e013 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -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 @@ -135,8 +135,12 @@ ORB_DECLARE(input_rc); #define _RC_INPUT_BASE 0x2b00 /** Fetch R/C input values into (rc_input_values *)arg */ - #define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0) +/** Enable RSSI input via ADC */ +#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) + +/** Enable RSSI input via PWM signal */ +#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) #endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h new file mode 100644 index 0000000000..927c904ec2 --- /dev/null +++ b/src/drivers/drv_sbus.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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_sbus.h + * + * Futaba S.BUS / S.BUS 2 compatible interface. + */ + +#ifndef _DRV_SBUS_H +#define _DRV_SBUS_H + +#include +#include + +#include "drv_orb_dev.h" + +/** + * Path for the default S.BUS device + */ +#define SBUS_DEVICE_PATH "/dev/sbus" + +#define _SBUS_BASE 0x2c00 + +/** Enable S.BUS version 1 / 2 output (0 to disable) */ +#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0) + +#endif /* _DRV_SBUS_H */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 382acb6d08..0fb7b7d24f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -1824,9 +1825,11 @@ PX4IO::print_status() uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s", flags, + printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); for (unsigned i = 0; i < raw_inputs; i++) @@ -1861,7 +1864,13 @@ PX4IO::print_status() printf("\n"); /* setup and state */ - printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); + printf("features 0x%04x%s%s\n", features, + ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, @@ -2283,6 +2292,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; + case RC_INPUT_ENABLE_RSSI_ANALOG: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); + } + + break; + + case RC_INPUT_ENABLE_RSSI_PWM: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); + } + + break; + + case SBUS_SET_PROTO_VERSION: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -2932,6 +2973,60 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "sbus1_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + + if (ret != 0) { + errx(ret, "S.BUS v1 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "sbus2_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + + if (ret != 0) { + errx(ret, "S.BUS v2 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_analog")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); + + if (ret != 0) { + errx(ret, "RSSI analog failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_pwm")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); + + if (ret != 0) { + errx(ret, "RSSI PWM failed"); + } + + exit(0); + } + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc',\n" + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } From 9cdbbab855d463bffb39d8dd55888fc1e0423818 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:52:46 +0100 Subject: [PATCH 079/222] Differentiate between publication and signal receive timestamp, correctly set the rc_lost flag in the frame. Ready for prime-time testing. --- src/drivers/drv_rc_input.h | 18 +++++++++++++++-- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++-- src/drivers/px4io/px4io.cpp | 31 ++++++++++++++++++++++++------ src/modules/mavlink/orb_listener.c | 2 +- src/modules/sensors/sensors.cpp | 9 ++++++--- src/systemcmds/tests/test_rc.c | 2 +- 6 files changed, 59 insertions(+), 15 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 715df7e013..20763e2657 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -62,6 +62,11 @@ */ #define RC_INPUT_MAX_CHANNELS 18 +/** + * Maximum RSSI value + */ +#define RC_INPUT_RSSI_MAX 255 + /** * Input signal type, value is a control position from zero to 100 * percent. @@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE { * on the board involved. */ struct rc_input_values { - /** decoding time */ - uint64_t timestamp; + /** publication time */ + uint64_t timestamp_publication; + + /** last valid reception time */ + uint64_t timestamp_last_signal; /** number of channels actually being seen */ uint32_t channel_count; @@ -120,6 +128,12 @@ struct rc_input_values { * */ uint16_t rc_total_frame_count; + /** + * Length of a single PPM frame. + * Zero for non-PPM systems + */ + uint16_t rc_ppm_frame_length; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c067d363b7..0fbd849243 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -626,7 +626,7 @@ PX4FMU::task_main() #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { + if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; @@ -638,7 +638,15 @@ PX4FMU::task_main() rc_in.values[i] = ppm_buffer[i]; } - rc_in.timestamp = ppm_last_valid_decode; + rc_in.timestamp_publication = ppm_last_valid_decode; + rc_in.timestamp_last_signal = ppm_last_valid_decode; + + rc_in.rc_ppm_frame_length = ppm_frame_length; + rc_in.rssi = RC_INPUT_RSSI_MAX; + rc_in.rc_failsafe = false; + rc_in.rc_lost = false; + rc_in.rc_lost_frame_count = 0; + rc_in.rc_total_frame_count = 0; /* lazily advertise on first publication */ if (to_input_rc == 0) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fb7b7d24f..5601230a42 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -239,6 +239,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels + uint64_t _rc_last_valid; ///< last valid timestamp volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag @@ -468,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) : _update_interval(0), _rc_handling_disabled(false), _rc_chan_count(0), + _rc_last_valid(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), @@ -1398,7 +1400,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp = hrt_absolute_time(); + input_rc.timestamp_publication = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1408,13 +1411,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[0]; + channel_count = regs[PX4IO_P_RAW_RC_COUNT]; if (channel_count != _rc_chan_count) perf_count(_perf_chan_count); _rc_chan_count = channel_count; + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; + input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; + input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + + /* rc_lost has to be set before the call to this function */ + if (!input_rc.rc_lost && !input_rc.rc_failsafe) + _rc_last_valid = input_rc.timestamp_publication; + + input_rc.timestamp_last_signal = _rc_last_valid; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1431,13 +1446,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int PX4IO::io_publish_raw_rc() { - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; /* fetch values from IO */ rc_input_values rc_val; - rc_val.timestamp = hrt_absolute_time(); + + /* set the RC status flag ORDER MATTERS! */ + rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); int ret = io_get_raw_rc_input(rc_val); @@ -1456,6 +1470,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* we do not know the RC input, only publish if RC OK flag is set */ + /* if no raw RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; } /* lazily advertise on first publication */ diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..41c7544058 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -355,7 +355,7 @@ l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + rc_raw.timestamp_publication / 1000, i, (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 23f20b0cb2..df6cbb7b2f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,9 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + if (rc_input.rc_lost) + return; + struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1320,7 +1323,7 @@ Sensors::rc_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp_last_signal; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1369,9 +1372,9 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp; + _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp; + manual_control.timestamp = rc_input.timestamp_last_signal; /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 6a602ecfc9..57c0e7f4c7 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[]) return ERROR; } - if (hrt_absolute_time() - rc_input.timestamp > 100000) { + if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) { warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; From 2dc3cf5e43154a516505d768885e734a5ab25e14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:58:21 +0100 Subject: [PATCH 080/222] Remove unneeded header and commented out dead code from MEAS airspeed driver --- src/drivers/meas_airspeed/meas_airspeed.cpp | 22 +++++++-------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a95c4576b0..9251cff7b2 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include @@ -178,24 +177,17 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; @@ -204,7 +196,7 @@ MEASAirspeed::collect() struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } @@ -392,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) From b67f7b2c7cb4d176ff8b3c5b7ee5e839845eef94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:12:12 +0100 Subject: [PATCH 081/222] Fix printing of IO status --- src/drivers/px4io/px4io.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5601230a42..e24236de73 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1784,7 +1784,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t io_status_flags = flags; - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), @@ -1843,6 +1843,12 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + + printf("\n"); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), @@ -1851,11 +1857,6 @@ PX4IO::print_status() ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); - for (unsigned i = 0; i < raw_inputs; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); - - printf("\n"); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -1884,7 +1885,7 @@ PX4IO::print_status() /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04x%s%s\n", features, + printf("features 0x%04x%s%s%s%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), From ac32116f00a07d61b2873dd98a2a4ed28515d522 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:17:01 +0100 Subject: [PATCH 082/222] Fix docs header --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a2e1ed6cde..39272104d6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.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 From cc68d11353e7fd7f0e775b8c11d99bd21982d2a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:26:19 +0100 Subject: [PATCH 083/222] Add flag to indicate proper RC mapping --- src/modules/px4iofirmware/protocol.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 69c12d877d..e3f499435a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -141,6 +141,7 @@ #define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ From d1c934233f42e32a59117cbec317c1116d39be82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:30:16 +0100 Subject: [PATCH 084/222] Fix S.BUS decoder to return value even if its just failsafe --- src/modules/px4iofirmware/sbus.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 32ab2a6988..f6ec542eb0 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -291,7 +291,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag From 9e9105048ac70b7abaa40ef8ce3f6f75910ada0a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 15:46:14 +0100 Subject: [PATCH 085/222] commander, navigator: failsafe fixes, mavlink messages cleanup --- src/modules/commander/commander.cpp | 71 ++++++++++-------------- src/modules/navigator/navigator_main.cpp | 8 +-- 2 files changed, 32 insertions(+), 47 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0e91728004..4c29d774af 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1080,9 +1080,9 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1100,7 +1100,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + 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) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1138,10 +1138,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); - } } /* fill current_status according to mode switches */ @@ -1165,47 +1161,40 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } - } else if (res == TRANSITION_DENIED) { - /* LAND mode denied, switch to failsafe state TERMINATION */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } - } else if (armed.armed) { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); - - } else if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); - - } else if (res == TRANSITION_DENIED) { - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } - } + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7c37237ff2..4be9f055b3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1072,7 +1072,6 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); set_mission_item(); } @@ -1201,7 +1200,6 @@ Navigator::start_rtl() } _reset_loiter_pos = true; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); } @@ -1222,8 +1220,6 @@ Navigator::start_land() _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.yaw = NAN; - - mavlink_log_info(_mavlink_fd, "[navigator] land started"); } void @@ -1259,7 +1255,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1312,7 +1308,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } From 2a30c574ce569c876dd2b95919a0d86c7c1b1023 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:50:19 +0100 Subject: [PATCH 086/222] IO status printing improvements / fixes --- src/drivers/px4io/px4io.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e24236de73..9595518ec9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1850,14 +1850,15 @@ PX4IO::print_status() printf("\n"); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s%s%s%s", flags, + printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") ); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { + if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2717,7 +2718,7 @@ monitor(void) /* clear screen */ printf("\033[2J"); - unsigned cancels = 3; + unsigned cancels = 2; for (;;) { pollfd fds[1]; From 00a3270dc696e09ad1e8f7b0eec579b92b6c0e2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:52:51 +0100 Subject: [PATCH 087/222] Differentiate between failsafe having kicked in (which stops the normal output mixing based on RC outputs and prevents unwanted control commands due to failsafe) and a true loss of the receiver, where we stop outputting RC channel readings downstream on FMU. --- src/modules/px4iofirmware/controls.c | 167 +++++++++++++++------------ src/modules/px4iofirmware/px4io.h | 3 +- 2 files changed, 96 insertions(+), 74 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e70eaed093..b3a999f223 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { + /* no channels */ + r_raw_rc_count = 0; + rc_channels_timestamp_received = 0; + rc_channels_timestamp_valid = 0; + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); @@ -121,6 +126,9 @@ controls_tick() { else r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } perf_end(c_gather_dsm); @@ -164,6 +172,8 @@ controls_tick() { if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_ppm); @@ -185,97 +195,100 @@ controls_tick() { */ if (dsm_updated || sbus_updated || ppm_updated) { - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); - /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + /* do not command anything in failsafe, kick in the RC loss counter */ + if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - uint16_t raw = r_raw_rc_values[i]; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - int16_t scaled; + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + uint16_t raw = r_raw_rc_values[i]; - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + int16_t scaled; - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + + } } } - } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* - * If we got an update with zero channels, treat it as - * a loss of input. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { - rc_input_lost = true; - } else { - /* set RC OK flag */ + /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); + } } /* @@ -288,7 +301,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -296,24 +309,32 @@ controls_tick() { PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); + } /* * Handle losing RC input */ - if (rc_input_lost) { + /* this kicks in if the receiver is gone or the system went to failsafe */ + if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ + r_rc_valid = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + } - /* Mark the arrays as empty */ + /* this kicks in if the receiver is completely gone */ + if (rc_input_lost) { + + /* Set channel count to zero */ r_raw_rc_count = 0; - r_rc_valid = 0; } /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 39272104d6..bb224f3880 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -116,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ */ struct sys_state_s { - volatile uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp_received; + volatile uint64_t rc_channels_timestamp_valid; /** * Last FMU receive time, in microseconds since system boot From 0393b2aa129050307896b87c657ee0ed8e449891 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 16:01:39 +0100 Subject: [PATCH 088/222] Build fix for IO control input parsing. --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b3a999f223..941500f0d9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -61,8 +61,8 @@ controls_init(void) { /* no channels */ r_raw_rc_count = 0; - rc_channels_timestamp_received = 0; - rc_channels_timestamp_valid = 0; + system_state.rc_channels_timestamp_received = 0; + system_state.rc_channels_timestamp_valid = 0; /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); From 1e63e8d9321d685e3af09aa16b58c5985e878435 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 12:07:27 +0100 Subject: [PATCH 089/222] navigator: wait before landing in RTL --- src/modules/navigator/navigator_main.cpp | 8 ++++++-- src/modules/navigator/navigator_params.c | 1 + 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4be9f055b3..e311c469f5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -198,6 +198,7 @@ private: float takeoff_alt; float land_alt; float rtl_alt; + float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { @@ -208,6 +209,7 @@ private: param_t takeoff_alt; param_t land_alt; param_t rtl_alt; + param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -406,6 +408,7 @@ Navigator::Navigator() : _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); + _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); @@ -463,6 +466,7 @@ Navigator::parameters_update() param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); + param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); @@ -1299,9 +1303,9 @@ 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 = 0.0f; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; + _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ff819fca40..af1d9d7d58 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -60,3 +60,4 @@ 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 PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing From ad51e0a08abcd7c9cd1d018f2bcaae3055b7c18e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 12:09:23 +0100 Subject: [PATCH 090/222] navigator: minor mavlink messages and comments fixes --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e311c469f5..dd471928e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1150,7 +1150,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { @@ -1435,7 +1435,7 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ + /* calculate AMSL altitude for this waypoint */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) From 20108ed95d5bbae64bfcb95de5404fa97d9d0ac1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 13:18:54 +0100 Subject: [PATCH 091/222] commander: minor refactoring current_status -> status --- src/modules/commander/commander.cpp | 58 ++++++++++++++--------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4c29d774af..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void print_reject_mode(const char *msg); @@ -1140,11 +1140,11 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill current_status according to mode switches */ + /* fill status according to mode switches */ check_mode_switches(&sp_man, &status); /* evaluate the main state machine according to mode switches */ - res = check_main_state_machine(&status); + res = set_main_state_rc(&status); if (res == TRANSITION_CHANGED) { tune_positive(); @@ -1418,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { + switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state @@ -1492,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status) print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages print_reject_mode("SEATBELT"); // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; From ba90dc87f6fcb6198e36265239774d35a8efe9cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 16:43:44 +0100 Subject: [PATCH 092/222] HOTFIX: Re-enable legacy config support, uncomment commented out configs. Needs more work. --- ROMFS/px4fmu_common/init.d/rc.autostart | 40 ++++++++++++------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 325520dd07..75cac3e50c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -33,7 +33,7 @@ fi if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/1002_rc_fw_state.hil + sh /etc/init.d/1002_rc_fw_state.hil fi if param compare SYS_AUTOSTART 1003 @@ -52,47 +52,47 @@ fi if param compare SYS_AUTOSTART 2100 100 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + sh /etc/init.d/2100_mpx_easystar + set MODE custom fi if param compare SYS_AUTOSTART 2101 101 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/2101_hk_bixler + set MODE custom fi if param compare SYS_AUTOSTART 2102 102 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/2102_3dr_skywalker + set MODE custom fi # # Flying wing # -if param compare SYS_AUTOSTART 3030 +if param compare SYS_AUTOSTART 3030 30 then - #sh /etc/init.d/3030_io_camflyer + sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 +if param compare SYS_AUTOSTART 3031 31 then sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 +if param compare SYS_AUTOSTART 3032 32 then sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 +if param compare SYS_AUTOSTART 3033 33 then sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 +if param compare SYS_AUTOSTART 3034 34 then sh /etc/init.d/3034_fx79 fi @@ -101,27 +101,27 @@ fi # Quad X # -if param compare SYS_AUTOSTART 4008 +if param compare SYS_AUTOSTART 4008 8 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 +if param compare SYS_AUTOSTART 4009 9 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 +if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 +if param compare SYS_AUTOSTART 4011 11 then sh /etc/init.d/4011_dji_f450 fi -if param compare SYS_AUTOSTART 4012 +if param compare SYS_AUTOSTART 4012 12 then sh /etc/init.d/4012_hk_x550 fi @@ -130,12 +130,12 @@ fi # Wide arm / H frame # -if param compare SYS_AUTOSTART 10015 +if param compare SYS_AUTOSTART 10015 15 then sh /etc/init.d/10015_tbs_discovery fi -if param compare SYS_AUTOSTART 10016 +if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi From 068cc190e20b8901b8e4128f3c21200724f0cd4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:18 +0100 Subject: [PATCH 093/222] Updated / added all system types that have been available before --- .../init.d/1000_rc_fw_easystar.hil | 58 ++----------- .../px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +++---------- .../init.d/1004_rc_fw_Rascal110.hil | 1 - ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/2101_hk_bixler | 48 +---------- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 48 +---------- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 84 ++++++++----------- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 33 +++++++- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 ++++++++ 13 files changed, 312 insertions(+), 238 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm 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 40a13b5d17..ebe8a1a1ea 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6d..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then 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 -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +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 75a00a675c..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -43,4 +43,3 @@ set HIL yes set VEHICLE_TYPE fw set MIXER FMU_AERT - diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 0000000000..5f3cec4e0a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: 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 FMU_octo_cox + +set PWM_OUTPUTS 1234 +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/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba074..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # 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 FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +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 a6d2ace962..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # 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 FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +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 65f01c9743..cbcc6189b6 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # 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 -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +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 03f2822375..143310af95 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,8 +7,37 @@ if [ $DO_AUTOCONFIG == yes ] then - # TODO + # + # 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 set VEHICLE_TYPE fw -set MIXER FMU_Q +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 0000000000..2e5f6ca4f5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: 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 FMU_quad_+ + +set PWM_OUTPUTS 1234 +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/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 0000000000..ddec8f36ef --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: 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 FMU_hexa_x + +set PWM_OUTPUTS 1234 +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/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 0000000000..106e0fb54c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: 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 FMU_hexa_+ + +set PWM_OUTPUTS 1234 +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/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 0000000000..f0eea339b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: 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 FMU_octo_x + +set PWM_OUTPUTS 1234 +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/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 0000000000..992a7aeba6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: 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 FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 From 4c4cd41b72471ce28ccebc4b24b592d6159f8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:31 +0100 Subject: [PATCH 094/222] Registered all new system types --- ROMFS/px4fmu_common/init.d/rc.autostart | 54 +++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 75cac3e50c..34da2dfef2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -126,6 +126,51 @@ then sh /etc/init.d/4012_hk_x550 fi +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + # # Wide arm / H frame # @@ -139,3 +184,12 @@ if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi From 7e855b5a23c9053dc8a59fcf33670e1cb93dd34b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:20:24 +0100 Subject: [PATCH 095/222] Deleted test - should not be in mainline --- ROMFS/px4fmu_common/init.d/cmp_test | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/cmp_test diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85bb..0000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi \ No newline at end of file From d1508a7813ad09a173fe314608c25dc8c3cd7a1f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 20:49:17 +0100 Subject: [PATCH 096/222] vehicle_control_mode publication moved to commander, WIP --- src/modules/commander/commander.cpp | 136 ++++++- .../commander/state_machine_helper.cpp | 4 + src/modules/mavlink/mavlink.c | 4 + src/modules/navigator/navigator_main.cpp | 362 ++++++------------ src/modules/navigator/navigator_state.h | 21 + src/modules/sdlog2/sdlog2.c | 4 +- .../uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 265 insertions(+), 287 deletions(-) create mode 100644 src/modules/navigator/navigator_state.h diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52ad..60fb4f486a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); +void set_control_mode(); + void print_reject_mode(const char *msg); void print_reject_arm(const char *msg); @@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } static struct vehicle_status_s status; - -/* armed topic */ +static struct vehicle_control_mode_s control_mode; static struct actuator_armed_s armed; - static struct safety_s safety; int commander_thread_main(int argc, char *argv[]) @@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1530,6 +1534,102 @@ set_main_state_rc(struct vehicle_status_s *status) return res; } +void +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; + } + + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + void print_reject_mode(const char *msg) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a3..43d0e023e3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_RTL: /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } @@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_LAND: /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f80917162..5406c35c21 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,6 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO get nav state + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + /* if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_READY) { @@ -231,6 +234,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e3..2117755eed 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -84,6 +84,7 @@ #include #include +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -151,10 +152,10 @@ private: int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ 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 _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -274,6 +275,10 @@ private: */ void vehicle_status_update(); + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -341,11 +346,6 @@ private: * Publish a new mission item triplet for position controller */ void publish_position_setpoint_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); }; namespace navigator @@ -373,6 +373,7 @@ 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), @@ -381,7 +382,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -541,9 +541,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -589,11 +599,13 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); @@ -605,12 +617,11 @@ Navigator::task_main() orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -627,6 +638,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _control_mode_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -652,127 +665,113 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; /* evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { - if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { - if (_vstatus.main_state == MAIN_STATE_AUTO) { - bool stick_mode = false; + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - - stick_mode = true; - - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - 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); - } - - 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); - stick_mode = true; - } - } + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + stick_mode = true; - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); } 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); - } - } + dispatch(EVENT_LOITER_REQUESTED); } + + 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); + stick_mode = true; + } + } + } + + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + break; + + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; } } else { - /* not in AUTO mode */ - dispatch(EVENT_NONE_REQUESTED); + /* 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); + } + } } - - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { - /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - - dispatch(EVENT_RTL_REQUESTED); - } - - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { - /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); - } - - } else { - /* shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); } } else { - /* not armed */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -840,12 +839,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); } perf_end(_loop_perf); @@ -1556,139 +1549,6 @@ Navigator::publish_position_setpoint_triplet() } } -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator has control */ - bool navigator_enabled = false; - - switch (_vstatus.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - - default: - break; - } - - break; - - case FAILSAFE_STATE_RTL: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_LAND: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_TERMINATION: - navigator_enabled = true; - /* disable all controllers on termination */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_termination_enabled = true; - break; - - default: - break; - } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - _control_mode.flag_control_manual_enabled = false; - - switch (myState) { - case NAV_STATE_READY: - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - break; - - case NAV_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - } - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); - - } else { - /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 0000000000..6a1475c9b0 --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * navigator_state.h + * + * Created on: 27.01.2014 + * Author: ton + */ + +#ifndef NAVIGATOR_STATE_H_ +#define NAVIGATOR_STATE_H_ + +typedef enum { + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + +#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bac2958e9..1e032d1c81 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1067,8 +1067,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac8987..7cbb37cd8e 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,23 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -86,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a5988d3ba4..1b3639e309 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include #include "../uORB.h" +#include + /** * @addtogroup topics @{ */ From 7d334ed54f41bd89fcaaddff4091e3eb8901b6b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 21:52:23 +0100 Subject: [PATCH 097/222] navigator: RTL fix, more informative log message --- src/modules/navigator/navigator_main.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2117755eed..8985b28df5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1182,20 +1182,22 @@ Navigator::start_rtl() { _do_takeoff = false; + /* decide if we need climb */ if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; - - if (_reset_loiter_pos) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - } } } + /* if switching directly to return state, reset altitude setpoint */ + if (_rtl_state == RTL_STATE_RETURN) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + _reset_loiter_pos = true; set_rtl_item(); } @@ -1278,7 +1280,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt); break; } From 3f79057dd1a6e336c6138553268576345c3bf6e1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 27 Jan 2014 23:46:02 +0100 Subject: [PATCH 098/222] fw att pos estimator: fix output lat/lon conversion (introduced by changes in 58792c5ca6e42bc251dd3c92b0e79217ff5d5403) --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 8e88130e15..83145ac72d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -315,8 +315,8 @@ void KalmanNav::updatePublications() _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; - _pos.lat = getLatDegE7(); - _pos.lon = getLonDegE7(); + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); _pos.vel_n = vN; _pos.vel_e = vE; From 3fdb082cb89dc82538637b3f060787d929105567 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 00:03:29 +0100 Subject: [PATCH 099/222] mavlink: AUTO states indication fixed --- src/modules/mavlink/mavlink.c | 47 ++++++++++--------- src/modules/mavlink/orb_listener.c | 31 ++---------- src/modules/mavlink/orb_topics.h | 6 +-- src/modules/navigator/navigator_main.cpp | 27 +++++++---- .../uORB/topics/position_setpoint_triplet.h | 3 ++ 5 files changed, 55 insertions(+), 59 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5406c35c21..4d975066f7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + if (armed.armed) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -208,33 +207,37 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; union px4_custom_mode custom_mode; custom_mode.data = 0; - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { + if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO get nav state - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - /* - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + if (pos_sp_triplet.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 7f6237535e..69cd820a02 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; -struct vehicle_control_mode_s control_mode; +struct position_setpoint_triplet_s pos_sp_triplet; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -127,7 +127,6 @@ static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); -static void l_control_mode(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -154,7 +153,6 @@ static const struct listener listeners[] = { {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -315,6 +313,7 @@ l_vehicle_status(const struct listener *l) /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet); /* enable or disable HIL */ if (v_status.hil_state == HIL_STATE_ON) @@ -682,26 +681,6 @@ l_nav_cap(const struct listener *l) } -void -l_control_mode(const struct listener *l) -{ - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - static void * uorb_receive_thread(void *arg) { @@ -777,9 +756,9 @@ uorb_receive_start(void) status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - /* --- CONTROL MODE --- */ - mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- POSITION SETPOINT TRIPLET --- */ + mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */ /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 30ba348cf6..89f647bdc8 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -94,7 +94,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; - int control_mode_sub; + int position_setpoint_triplet_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -111,8 +111,8 @@ extern struct navigation_capabilities_s nav_cap; /** Vehicle status */ extern struct vehicle_status_s v_status; -/** Vehicle control mode */ -extern struct vehicle_control_mode_s control_mode; +/** Position setpoint triplet */ +extern struct position_setpoint_triplet_s pos_sp_triplet; /** RC channels */ extern struct rc_channels_s rc; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8985b28df5..5a02bf522f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -189,6 +189,8 @@ private: 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]; struct { @@ -399,6 +401,7 @@ Navigator::Navigator() : _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _pos_sp_triplet_updated(false), _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); @@ -835,6 +838,11 @@ Navigator::task_main() } } + /* publish position setpoint triplet if updated */ + if (_pos_sp_triplet_updated) { + publish_position_setpoint_triplet(); + } + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); @@ -999,7 +1007,7 @@ Navigator::start_none() _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1018,7 +1026,7 @@ Navigator::start_ready() _rtl_state = RTL_STATE_NONE; } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1061,7 +1069,7 @@ Navigator::start_loiter() _pos_sp_triplet.next.valid = false; _mission_item_valid = false; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1174,7 +1182,7 @@ Navigator::set_mission_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1280,7 +1288,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1345,7 +1353,7 @@ Navigator::set_rtl_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1540,9 +1548,12 @@ Navigator::on_mission_item_reached() void Navigator::publish_position_setpoint_triplet() { - /* lazily publish the mission triplet only once available */ + /* update navigation state */ + _pos_sp_triplet.nav_state = static_cast(myState); + + /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the mission triplet */ + /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4b57833b63..e8d68281a8 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,6 +45,7 @@ #include #include #include "../uORB.h" +#include /** * @addtogroup topics @@ -82,6 +83,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + nav_state_t nav_state; /**< navigation state */ }; /** From f9b5709e9bb96afc134b32b6eb17c7d308afd82b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 00:23:23 +0100 Subject: [PATCH 100/222] sdlog2: NavState moved to GPSP message --- src/modules/sdlog2/sdlog2.c | 19 +++---------------- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 6 insertions(+), 19 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1e032d1c81..c3ea30cbfb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -62,7 +62,6 @@ #include #include -#include #include #include #include @@ -740,7 +739,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -767,7 +765,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -847,12 +844,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- VEHICLE CONTROL MODE --- */ - subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - fds[fdsc_count].fd = subs.control_mode_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -1002,7 +993,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; - /* poll all topics if logging enabled or only management (first 2) if not */ + /* poll all topics if logging enabled or only management (first 3) if not */ int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ @@ -1064,11 +1055,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { /* don't orb_copy, it's already done few lines above */ - /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ - orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; @@ -1078,7 +1066,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + /* don't orb_copy, it's already done few lines above */ log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; @@ -1094,8 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } - ifds++; // skip CONTROL MODE, already handled - /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1265,6 +1251,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 98736dd211..1e2441c331 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,7 +149,6 @@ struct log_ATTC_s { #define LOG_STAT_MSG 10 struct log_STAT_s { uint8_t main_state; - uint8_t navigation_state; uint8_t arming_state; float battery_remaining; uint8_t battery_warning; @@ -210,6 +209,7 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { + uint8_t nav_state; int32_t lat; int32_t lon; float alt; @@ -297,14 +297,14 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), From dfaa5a0c7c81f432d64cfffdaaf3d683c01f10b4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 09:57:47 +0800 Subject: [PATCH 101/222] Reduced stack from 2048 to 1024. Top reports stack usage at 812 under flight conditions. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64da..dbe97a1780 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -756,7 +756,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); From d1fb7651876236432dc66c5331c60258ff962352 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 10:30:16 +0800 Subject: [PATCH 102/222] Reduced low priority thread stack size to 1728. Top indicates the high-water-mark is at 1380 during accelerometer calibration. Safety margin ~25% --- 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 add7312de7..6f15e07027 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -648,7 +648,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 1728); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); From 0488d5b41c5c91658494242f778c7b9de99bcca4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 10:43:33 +0800 Subject: [PATCH 103/222] Reduced commander main task stack size to 2088. The high-water-mark measured at 1668 after calibration and flight. 25% safety margin, but commander is fairly complex. There are surely untested code paths here, but each is relatively shallow. --- 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 6f15e07027..33589940c3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -237,7 +237,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2088, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); From 984a815b94709916174f6a0beeb8ae8217a9aed1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:08:23 +0100 Subject: [PATCH 104/222] Navigator: make state names generic so that they can be used by the FSM visualisation tool later --- src/modules/commander/commander.cpp | 6 +- src/modules/mavlink/mavlink.c | 10 +- src/modules/navigator/navigator_main.cpp | 144 +++++++++--------- .../uORB/topics/vehicle_control_mode.h | 14 +- 4 files changed, 87 insertions(+), 87 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52ad..ad142d09ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -493,14 +493,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; + status->set_nav_state = STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; + status->set_nav_state = STATE_MISSION; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -624,7 +624,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; + status.set_nav_state = STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f80917162..a13d0c537f 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,15 +220,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + if (control_mode.nav_state == STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { + } else if (control_mode.nav_state == STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (control_mode.nav_state == STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (control_mode.nav_state == STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (control_mode.nav_state == STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e3..8cc2a1489e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -188,7 +188,7 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - char *nav_states_str[NAV_STATE_MAX]; + char *nav_states_str[MAX_STATE]; struct { float min_altitude; @@ -227,7 +227,7 @@ private: /** * State machine transition table */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; + static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; enum RTLState { RTL_STATE_NONE = 0, @@ -363,7 +363,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), + StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -423,7 +423,7 @@ Navigator::Navigator() : nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; + myState = STATE_NONE; start_none(); } @@ -604,7 +604,7 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = STATE_NONE; bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -667,7 +667,7 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -691,7 +691,7 @@ Navigator::task_main() stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; @@ -705,15 +705,15 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: + case STATE_NONE: /* nothing to do */ break; - case NAV_STATE_LOITER: + case STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - case NAV_STATE_MISSION: + case STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -723,8 +723,8 @@ Navigator::task_main() break; - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + case STATE_RTL: + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -737,7 +737,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 (myState == STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -755,14 +755,14 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { + if (myState != STATE_READY) { dispatch(EVENT_LAND_REQUESTED); } @@ -814,7 +814,7 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { + if (myState == STATE_MISSION || myState == STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); } @@ -903,19 +903,19 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: + case STATE_NONE: warnx("State: None"); break; - case NAV_STATE_LOITER: + case STATE_LOITER: warnx("State: Loiter"); break; - case NAV_STATE_MISSION: + case STATE_MISSION: warnx("State: Mission"); break; - case NAV_STATE_RTL: + case STATE_RTL: warnx("State: RTL"); break; @@ -925,72 +925,72 @@ Navigator::status() } } -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { { /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, }, { /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* 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_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_READY}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_READY}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_READY}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_READY}, }, { /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, }, { /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, }, { /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, // TODO need to reset rtl_state }, { /* STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, }, }; @@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached() void Navigator::on_mission_item_reached() { - if (myState == NAV_STATE_MISSION) { + if (myState == STATE_MISSION) { if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1644,7 +1644,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_manual_enabled = false; switch (myState) { - case NAV_STATE_READY: + case STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1654,7 +1654,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_climb_rate_enabled = false; break; - case NAV_STATE_LAND: + case STATE_LAND: /* land with or without position control */ _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac8987..7869a69585 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,13 +62,13 @@ */ typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + STATE_NONE = 0, + STATE_READY, + STATE_LOITER, + STATE_MISSION, + STATE_RTL, + STATE_LAND, + MAX_STATE } nav_state_t; struct vehicle_control_mode_s From cd9ec04904fa684f0ecddb3f2a4511346458fe8b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:15:28 +0100 Subject: [PATCH 105/222] Added FSM visualisation script --- Tools/draw_fsm_diagram.py | 202 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100755 Tools/draw_fsm_diagram.py diff --git a/Tools/draw_fsm_diagram.py b/Tools/draw_fsm_diagram.py new file mode 100755 index 0000000000..a36d39b41e --- /dev/null +++ b/Tools/draw_fsm_diagram.py @@ -0,0 +1,202 @@ +#!/usr/bin/python3 + +"""draw_fsm_diagram.py: Create dot code from a state transition table + +convert dot code to png using graphviz: + +dot fsm.dot -Tpng -o fsm.png +""" + +from optparse import OptionParser +import re +import subprocess + +__author__ = "Julian Oes" + +def get_dot_header(): + + return """digraph finite_state_machine { + graph [ dpi = 300 ]; + ratio = 1.5 + node [shape = circle];""" + +def get_dot_footer(): + + return """}\n""" + +def main(): + + # parse input arguments + parser = OptionParser() + parser.add_option("-i", "--input-file", default=None, help="choose file to parse") + parser.add_option("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_option("-t", "--table-file", default=None, help="choose file for output of table") + (options, args) = parser.parse_args() + + # open source file + if options.input_file == None: + exit('please specify file') + f = open(options.input_file,'r') + source = f.read() + + # search for state transition table and extract the table itself + # first look for StateTable::Tran + # then accept anything including newline until { + # but don't accept the definition (without ;) + # then extract anything inside the brackets until }; + match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source) + + if not match: + exit('no state transition table found') + + table_source = match.group(1) + + # bookkeeping for error checking + num_errors_found = 0 + + states = [] + events = [] + + # first get all states and events + for table_line in table_source.split('\n'): + + match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + if match: + states.append(str(match.group(1))) + # go to next line + continue + + if len(states) == 1: + match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line) + if match: + events.append(str(match.group(1))) + + print('Found %d states and %d events' % (len(states), len(events))) + + + # keep track of origin state + state = None + + # fill dot code in here + dot_code = '' + + # create table len(states)xlen(events) + transition_table = [[[] for x in range(len(states))] for y in range(len(events))] + + # now fill the transition table and write the dot code + for table_line in table_source.split('\n'): + + # get states + # from: /* STATE_NONE */ + # extract only "NONE" + match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + if match: + state = match.group(1) + state_index = states.index(state) + # go to next line + continue + + # can't advance without proper state + if state == None: + continue + + # get event and next state + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY} + # extract "READY_REQUESTED" and "READY" if there is ACTION + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*STATE_(\w+)', table_line) + + # get event and next state + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + # extract "NONE_REQUESTED" and "STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*STATE_(\w+)', table_line) + + # ignore lines with brackets only + if match_action or match_no_action: + + # only write arrows for actions + if match_action: + event = match_action.group(1) + new_state = match_action.group(2) + dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n' + + elif match_no_action: + event = match_no_action.group(1) + new_state = match_no_action.group(2) + + # check for state changes without action + if state != new_state: + print('Error: no action but state change:') + print('State: ' + state + ' changed to: ' + new_state) + print(table_line) + num_errors_found += 1 + + # check for wrong events + if event not in events: + print('Error: unknown event: ' + event) + print(table_line) + num_errors_found += 1 + + # check for wrong new states + if new_state not in states: + print('Error: unknown new state: ' + new_state) + print(table_line) + num_errors_found += 1 + + # save new state in transition table + event_index = events.index(event) + + # bold for action + if match_action: + transition_table[event_index][state_index] = '**' + new_state + '**' + else: + transition_table[event_index][state_index] = new_state + + + + # assemble dot code + dot_code = get_dot_header() + dot_code + get_dot_footer() + + # write or print dot file + if options.dot_file: + f = open(options.dot_file,'w') + f.write(dot_code) + print('Wrote dot file') + else: + print('##########Dot-start##########') + print(dot_code) + print('##########Dot-end############') + + + # assemble doku wiki table + table_code = '| ^ ' + # start with header of all states + for state in states: + table_code += state + ' ^ ' + + table_code += '\n' + + # add events and new states + for event, row in zip(events, transition_table): + table_code += '^ ' + event + ' | ' + for new_state in row: + table_code += new_state + ' | ' + table_code += '\n' + + # write or print wiki table + if options.table_file: + f = open(options.table_file,'w') + f.write(table_code) + print('Wrote table file') + else: + print('##########Table-start########') + print(table_code) + print('##########Table-end##########') + + # report obvous errors + if num_errors_found: + print('Obvious errors found: %d' % num_errors_found) + else: + print('No obvious errors found') + +if __name__ == '__main__': + main() From 6002819f8fccb491dcfbe23de892a827cd9f4618 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:21:48 +0100 Subject: [PATCH 106/222] Navigator: FSM bugfix and missing break --- src/modules/navigator/navigator_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8cc2a1489e..2a782ea032 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -989,8 +989,8 @@ StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LAND}, }, }; @@ -1607,6 +1607,7 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break; From c5a3dd916899ddd8409380c27c05a8b2d903c8f4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:46:44 +0100 Subject: [PATCH 107/222] FSM visualisation script: use argparse instead of optionparse and some minor cleanup --- Tools/draw_fsm_diagram.py | 29 ++++++++++++++--------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/Tools/draw_fsm_diagram.py b/Tools/draw_fsm_diagram.py index a36d39b41e..fe37b6ba3f 100755 --- a/Tools/draw_fsm_diagram.py +++ b/Tools/draw_fsm_diagram.py @@ -1,15 +1,14 @@ -#!/usr/bin/python3 +#!/usr/bin/env python3 -"""draw_fsm_diagram.py: Create dot code from a state transition table +"""draw_fsm_diagram.py: Create dot code and dokuwiki table from a state transition table convert dot code to png using graphviz: dot fsm.dot -Tpng -o fsm.png """ -from optparse import OptionParser +import argparse import re -import subprocess __author__ = "Julian Oes" @@ -27,16 +26,16 @@ def get_dot_footer(): def main(): # parse input arguments - parser = OptionParser() - parser.add_option("-i", "--input-file", default=None, help="choose file to parse") - parser.add_option("-d", "--dot-file", default=None, help="choose file for output dot file") - parser.add_option("-t", "--table-file", default=None, help="choose file for output of table") - (options, args) = parser.parse_args() + parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.') + parser.add_argument("-i", "--input-file", default=None, help="choose file to parse") + parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table") + args = parser.parse_args() # open source file - if options.input_file == None: + if args.input_file == None: exit('please specify file') - f = open(options.input_file,'r') + f = open(args.input_file,'r') source = f.read() # search for state transition table and extract the table itself @@ -157,8 +156,8 @@ def main(): dot_code = get_dot_header() + dot_code + get_dot_footer() # write or print dot file - if options.dot_file: - f = open(options.dot_file,'w') + if args.dot_file: + f = open(args.dot_file,'w') f.write(dot_code) print('Wrote dot file') else: @@ -183,8 +182,8 @@ def main(): table_code += '\n' # write or print wiki table - if options.table_file: - f = open(options.table_file,'w') + if args.table_file: + f = open(args.table_file,'w') f.write(table_code) print('Wrote table file') else: From 1488d32d0f25469da9119f36429c8783256048b1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:49:14 +0100 Subject: [PATCH 108/222] FSM visualisation script: renamed the file --- Tools/{draw_fsm_diagram.py => fsm_visualisation.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename Tools/{draw_fsm_diagram.py => fsm_visualisation.py} (98%) diff --git a/Tools/draw_fsm_diagram.py b/Tools/fsm_visualisation.py similarity index 98% rename from Tools/draw_fsm_diagram.py rename to Tools/fsm_visualisation.py index fe37b6ba3f..5532b99bc8 100755 --- a/Tools/draw_fsm_diagram.py +++ b/Tools/fsm_visualisation.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -"""draw_fsm_diagram.py: Create dot code and dokuwiki table from a state transition table +"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table convert dot code to png using graphviz: From 19c3525f58b4123c38592644252226219ddef07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 11:11:34 +0100 Subject: [PATCH 109/222] Hotfix: Fixed telemetry transmission of RC channels - we always sent one set too much - by lieron --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..192c8fdbab 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -352,7 +352,7 @@ l_input_rc(const struct listener *l) const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, rc_raw.timestamp / 1000, From 547080f1882918a65db4e7cba396ae2c72d84544 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 12:29:30 +0100 Subject: [PATCH 110/222] Revert "Navigator: make state names generic so that they can be used by the FSM visualisation tool later" This reverts commit 984a815b94709916174f6a0beeb8ae8217a9aed1. --- src/modules/commander/commander.cpp | 6 +- src/modules/mavlink/mavlink.c | 10 +- src/modules/navigator/navigator_main.cpp | 156 +++++++++--------- .../uORB/topics/vehicle_control_mode.h | 14 +- 4 files changed, 93 insertions(+), 93 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ad142d09ec..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -493,14 +493,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = STATE_LOITER; + status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = STATE_MISSION; + status->set_nav_state = NAV_STATE_MISSION; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -624,7 +624,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = STATE_NONE; + status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a13d0c537f..4f80917162 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,15 +220,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == STATE_NONE) { // failsafe, shouldn't happen + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == STATE_READY) { + } else if (control_mode.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == STATE_LOITER) { + } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == STATE_MISSION) { + } else if (control_mode.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == STATE_RTL) { + } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2a782ea032..e6b7ef93d8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -188,7 +188,7 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - char *nav_states_str[MAX_STATE]; + char *nav_states_str[NAV_STATE_MAX]; struct { float min_altitude; @@ -227,7 +227,7 @@ private: /** * State machine transition table */ - static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; + static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; enum RTLState { RTL_STATE_NONE = 0, @@ -363,7 +363,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), + StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -423,7 +423,7 @@ Navigator::Navigator() : nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = STATE_NONE; + myState = NAV_STATE_NONE; start_none(); } @@ -604,7 +604,7 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = STATE_NONE; + unsigned prevState = NAV_STATE_NONE; bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -667,7 +667,7 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -691,7 +691,7 @@ Navigator::task_main() stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == STATE_RTL) { + 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); stick_mode = true; @@ -705,15 +705,15 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case STATE_NONE: + case NAV_STATE_NONE: /* nothing to do */ break; - case STATE_LOITER: + case NAV_STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - case STATE_MISSION: + case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -723,8 +723,8 @@ Navigator::task_main() break; - case STATE_RTL: - if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -737,7 +737,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == STATE_NONE) { + if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -755,14 +755,14 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { /* LAND on failsafe */ - if (myState != STATE_READY) { + if (myState != NAV_STATE_READY) { dispatch(EVENT_LAND_REQUESTED); } @@ -814,7 +814,7 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == STATE_MISSION || myState == STATE_RTL) { + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); } @@ -903,19 +903,19 @@ Navigator::status() } switch (myState) { - case STATE_NONE: + case NAV_STATE_NONE: warnx("State: None"); break; - case STATE_LOITER: + case NAV_STATE_LOITER: warnx("State: Loiter"); break; - case STATE_MISSION: + case NAV_STATE_MISSION: warnx("State: Mission"); break; - case STATE_RTL: + case NAV_STATE_RTL: warnx("State: RTL"); break; @@ -925,72 +925,72 @@ Navigator::status() } } -StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* NAV_STATE_NONE */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { - /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_READY}, + /* NAV_STATE_READY */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* 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_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* NAV_STATE_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { - /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + /* NAV_STATE_MISSION */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { - /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, // TODO need to reset rtl_state + /* NAV_STATE_RTL */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, { - /* STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LAND}, + /* NAV_STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, }, }; @@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached() void Navigator::on_mission_item_reached() { - if (myState == STATE_MISSION) { + if (myState == NAV_STATE_MISSION) { if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1645,7 +1645,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_manual_enabled = false; switch (myState) { - case STATE_READY: + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1655,7 +1655,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_climb_rate_enabled = false; break; - case STATE_LAND: + case NAV_STATE_LAND: /* land with or without position control */ _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7869a69585..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,13 +62,13 @@ */ typedef enum { - STATE_NONE = 0, - STATE_READY, - STATE_LOITER, - STATE_MISSION, - STATE_RTL, - STATE_LAND, - MAX_STATE + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX } nav_state_t; struct vehicle_control_mode_s From 99875bbd28ee813950efb9402b3acce55c34469b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 12:31:23 +0100 Subject: [PATCH 111/222] FSM visualisation script: cope with prefixed state names --- Tools/fsm_visualisation.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py index 5532b99bc8..c678ef0f4b 100755 --- a/Tools/fsm_visualisation.py +++ b/Tools/fsm_visualisation.py @@ -59,7 +59,7 @@ def main(): # first get all states and events for table_line in table_source.split('\n'): - match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) if match: states.append(str(match.group(1))) # go to next line @@ -86,9 +86,9 @@ def main(): for table_line in table_source.split('\n'): # get states - # from: /* STATE_NONE */ + # from: /* NAV_STATE_NONE */ # extract only "NONE" - match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) if match: state = match.group(1) state_index = states.index(state) @@ -100,14 +100,14 @@ def main(): continue # get event and next state - # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY} + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY} # extract "READY_REQUESTED" and "READY" if there is ACTION - match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*STATE_(\w+)', table_line) + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line) # get event and next state - # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - # extract "NONE_REQUESTED" and "STATE_NONE" if there is NO_ACTION - match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*STATE_(\w+)', table_line) + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + # extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line) # ignore lines with brackets only if match_action or match_no_action: From 33daf84c00004bc1f56c507abb13a0dd9075c649 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 16:56:53 +0100 Subject: [PATCH 112/222] lib/geo: bugs fixed, added function add_vector_to_global_position() --- src/lib/geo/geo.c | 27 +++++++++++++++++---------- src/lib/geo/geo.h | 6 ++++-- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 08fe2b696e..446cd5bee3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; double lat1 = phi_1; double lon1 = lambda_0; @@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are double lon2 = lambda_0 + 0.5 / 180 * M_PI; double sin_lat_2 = sin(lat2); double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH; /* 2) calculate distance rho on plane */ double k_bar = 0; @@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - const double radius_earth = 6371000.0d; - return radius_earth * c; + return CONSTANTS_RADIUS_OF_EARTH * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) @@ -210,7 +208,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -221,11 +219,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -236,8 +234,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); + *v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat; + *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); +} + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + + *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 5f4bce6986..94afb4df0a 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -115,9 +115,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); From 3601b807479ba8ea029b92872fcdd2ad2d051c54 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 16:57:31 +0100 Subject: [PATCH 113/222] mc_pos_control: avoid global to local projections, work in global frame --- .../mc_pos_control/mc_pos_control_main.cpp | 255 +++++++++--------- 1 file changed, 131 insertions(+), 124 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 d3e39e3a02..af2674192a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -63,8 +63,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -109,6 +108,7 @@ private: bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ @@ -116,11 +116,11 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _local_pos_sub; /**< vehicle local position */ + int _global_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -128,8 +128,7 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ - struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct vehicle_global_position_s _global_pos; /**< vehicle global position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ @@ -177,9 +176,14 @@ private: math::Vector<3> sp_offs_max; } _params; - math::Vector<3> _pos; + double _lat_sp; + double _lon_sp; + float _alt_sp; + + bool _reset_lat_lon_sp; + bool _reset_alt_sp; + math::Vector<3> _vel; - math::Vector<3> _pos_sp; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ @@ -200,6 +204,16 @@ private: static float scale_control(float ctl, float end, float dz); + /** + * Reset lat/lon to current position + */ + void reset_lat_lon_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + /** * Shim for calling task_main from task_create. */ @@ -227,6 +241,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _task_should_exit(false), _control_task(-1), + _mavlink_fd(-1), /* subscriptions */ _att_sub(-1), @@ -235,21 +250,27 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _local_pos_sub(-1), + _global_pos_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ - _local_pos_sp_pub(-1), _att_sp_pub(-1), - _global_vel_sp_pub(-1) + _pos_sp_triplet_pub(-1), + _global_vel_sp_pub(-1), + + _lat_sp(0.0), + _lon_sp(0.0), + _alt_sp(0.0f), + + _reset_lat_lon_sp(true), + _reset_alt_sp(true) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_global_pos, 0, sizeof(_global_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); @@ -261,9 +282,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); - _pos.zero(); _vel.zero(); - _pos_sp.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -405,10 +424,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_pos_sp_triplet_sub, &updated); + orb_check(_global_pos_sub, &updated); if (updated) - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } float @@ -431,14 +450,34 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) pos_control::g_control->task_main(); } +void +MulticopterPositionControl::reset_lat_lon_sp() +{ + if (_reset_lat_lon_sp) { + _reset_lat_lon_sp = false; + _lat_sp = _global_pos.lat; + _lon_sp = _global_pos.lon; + mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _alt_sp = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", (double)_alt_sp); + } +} + void MulticopterPositionControl::task_main() { warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions @@ -449,7 +488,7 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -460,8 +499,6 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_sp_z = true; - bool reset_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; @@ -472,11 +509,6 @@ MulticopterPositionControl::task_main() const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - hrt_abstime ref_timestamp = 0; - int32_t ref_lat = 0.0f; - int32_t ref_lon = 0.0f; - float ref_alt = 0.0f; - math::Vector<3> sp_move_rate; sp_move_rate.zero(); math::Vector<3> thrust_int; @@ -488,7 +520,7 @@ MulticopterPositionControl::task_main() struct pollfd fds[1]; /* Setup of loop */ - fds[0].fd = _local_pos_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -505,8 +537,6 @@ MulticopterPositionControl::task_main() continue; } - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - poll_subscriptions(); parameters_update(false); @@ -516,8 +546,8 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -529,60 +559,26 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - _pos(2) = _local_pos.z; - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - _vel(2) = _local_pos.vz; + _vel(0) = _global_pos.vel_n; + _vel(1) = _global_pos.vel_e; + _vel(2) = _global_pos.vel_d; sp_move_rate.zero(); - if (_local_pos.ref_timestamp != ref_timestamp) { - /* initialize local projection with new reference */ - double lat_home = _local_pos.ref_lat * 1e-7; - double lon_home = _local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - - if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { - /* correct setpoint in manual mode to stay in the same point */ - float ref_change_x = 0.0f; - float ref_change_y = 0.0f; - map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); - _pos_sp(0) += ref_change_x; - _pos_sp(1) += ref_change_y; - _pos_sp(2) += _local_pos.ref_alt - ref_alt; - } - - ref_timestamp = _local_pos.ref_timestamp; - ref_lat = _local_pos.ref_lat; - ref_lon = _local_pos.ref_lon; - ref_alt = _local_pos.ref_alt; - } - + /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ if (_control_mode.flag_control_altitude_enabled) { - /* reset setpoint Z to current altitude if needed */ - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); - } + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); /* move altitude setpoint with throttle stick */ sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { - /* reset setpoint XY to current position if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); - } + /* reset lat/lon setpoint to current position if needed */ + reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); @@ -602,85 +598,96 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - _pos_sp += sp_move_rate * dt; + add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); + _alt_sp -= sp_move_rate(2) * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); + pos_sp_offs(0) /= _params.sp_offs_max(0); + pos_sp_offs(1) /= _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + pos_sp_offs(2) = -(_alt_sp - _global_pos.alt) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); + _alt_sp = _global_pos.alt - pos_sp_offs(2) * _params.sp_offs_max(2); } - } else { + /* fill position setpoint triplet */ + _pos_sp_triplet.previous.valid = true; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = true; + + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + _pos_sp_triplet.current.lat = _lat_sp; + _pos_sp_triplet.current.lon = _lon_sp; + _pos_sp_triplet.current.alt = _alt_sp; + _pos_sp_triplet.current.yaw = _att_sp.yaw_body; + _pos_sp_triplet.current.loiter_radius = 0.0f; + _pos_sp_triplet.current.loiter_direction = 1.0f; + _pos_sp_triplet.current.pitch_min = 0.0f; + + /* publish position setpoint triplet */ + if (_pos_sp_triplet_pub > 0) { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); + + } else { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); + } + + } else if (_control_mode.flag_control_auto_enabled) { /* AUTO */ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + if (_pos_sp_triplet.current.valid) { - struct position_setpoint_s current_sp = _pos_sp_triplet.current; - - _pos_sp(2) = -(current_sp.alt - ref_alt); - - map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); - - if (isfinite(current_sp.yaw)) { - _att_sp.yaw_body = current_sp.yaw; - } - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; + + _lat_sp = _pos_sp_triplet.current.lat; + _lon_sp = _pos_sp_triplet.current.lon; + _alt_sp = _pos_sp_triplet.current.alt; } else { /* no waypoint, loiter, reset position setpoint if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } - - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - } + reset_lat_lon_sp(); + reset_alt_sp(); } - } - - /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ - _local_pos_sp.yaw = _att_sp.yaw_body; - _local_pos_sp.x = _pos_sp(0); - _local_pos_sp.y = _pos_sp(1); - _local_pos_sp.z = _pos_sp(2); - - /* publish local position setpoint */ - if (_local_pos_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + /* no control, reset setpoint */ + reset_lat_lon_sp(); + reset_alt_sp(); } /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; + 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 - _global_pos.alt); + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); if (!_control_mode.flag_control_altitude_enabled) { - reset_sp_z = true; + _reset_alt_sp = true; _vel_sp(2) = 0.0f; } if (!_control_mode.flag_control_position_enabled) { - reset_sp_xy = true; + _reset_lat_lon_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -730,7 +737,7 @@ MulticopterPositionControl::task_main() } thrust_int(2) = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } } else { @@ -742,7 +749,7 @@ 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"); + mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); } } else { @@ -946,8 +953,8 @@ MulticopterPositionControl::task_main() } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_alt_sp = true; + _reset_lat_lon_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -957,7 +964,7 @@ MulticopterPositionControl::task_main() } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); + mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); From 5ffa28b0e950ac3633df61bcfbdb7e2cefec3be4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 18:21:37 +0100 Subject: [PATCH 114/222] dlog2: GPSP message fixed --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1e2441c331..baac2ee3ec 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -304,7 +304,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "LLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), From 48f1b7e1c77a66973c6bb847290018531a99503c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 18:39:55 +0100 Subject: [PATCH 115/222] mc_pos_control: fill nav_state = NONE when publishing position setpoint triplet --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + 1 file changed, 1 insertion(+) 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 af2674192a..4ff13d4df2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -628,6 +628,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = true; + _pos_sp_triplet.nav_state = NAV_STATE_NONE; _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; _pos_sp_triplet.current.lat = _lat_sp; _pos_sp_triplet.current.lon = _lon_sp; From 6a1a29f77ecc9ded341bfbca037c9a6768ed3fb4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 20:40:05 +0100 Subject: [PATCH 116/222] global_position topic: added baro_alt, mc_pos_control: SEATBELT mode fixed, use baro/AMSL alt --- src/drivers/frsky_telemetry/frsky_data.c | 2 +- .../att_pos_estimator_ekf/KalmanNav.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.cpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 45 ++++++++++++++++--- src/modules/navigator/navigator_main.cpp | 5 ++- .../position_estimator_inav_main.c | 23 ++++------ src/modules/sdlog2/sdlog2.c | 2 + src/modules/sdlog2/sdlog2_messages.h | 4 +- .../uORB/topics/vehicle_global_position.h | 12 +++-- 11 files changed, 69 insertions(+), 34 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index e201ecbb3d..cfcf91e3fb 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.valid) { + if (global_pos.global_valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 8e88130e15..7f0dd92192 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -314,7 +314,7 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.valid = true; + _pos.global_valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 66ec20b954..620185fb77 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 60fb4f486a..901f919115 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1030,7 +1030,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.valid) { + && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c23488d7e..a371a499ef 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,7 +634,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet hil_global_pos.timestamp = timestamp; - hil_global_pos.valid = true; + hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; 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 4ff13d4df2..923a9dab05 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -182,6 +182,7 @@ private: bool _reset_lat_lon_sp; bool _reset_alt_sp; + bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ math::Vector<3> _vel; math::Vector<3> _vel_sp; @@ -214,6 +215,11 @@ private: */ void reset_alt_sp(); + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); + /** * Shim for calling task_main from task_create. */ @@ -263,7 +269,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _alt_sp(0.0f), _reset_lat_lon_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _use_global_alt(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -466,8 +473,23 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _alt_sp = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", (double)_alt_sp); + _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + } +} + +void +MulticopterPositionControl::select_alt(bool global) +{ + if (global != _use_global_alt) { + _use_global_alt = global; + if (global) { + /* switch from barometric to global altitude */ + _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { + /* switch from global to barometric altitude */ + _alt_sp += _global_pos.baro_alt - _global_pos.alt; + } } } @@ -565,8 +587,16 @@ MulticopterPositionControl::task_main() sp_move_rate.zero(); + float alt = _global_pos.alt; + /* select control source */ if (_control_mode.flag_control_manual_enabled) { + /* select altitude source and update setpoint */ + select_alt(_global_pos.global_valid); + if (!_use_global_alt) { + alt = _global_pos.baro_alt; + } + /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ @@ -612,7 +642,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = -(_alt_sp - _global_pos.alt) / _params.sp_offs_max(2); + pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); @@ -620,7 +650,7 @@ MulticopterPositionControl::task_main() if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); - _alt_sp = _global_pos.alt - pos_sp_offs(2) * _params.sp_offs_max(2); + _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); } /* fill position setpoint triplet */ @@ -647,6 +677,9 @@ MulticopterPositionControl::task_main() } } else if (_control_mode.flag_control_auto_enabled) { + /* always use AMSL altitude for AUTO */ + select_alt(true); + /* AUTO */ bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -678,7 +711,7 @@ MulticopterPositionControl::task_main() 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 - _global_pos.alt); + pos_err(2) = -(_alt_sp - alt); _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a02bf522f..170e5df059 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -840,6 +840,7 @@ Navigator::task_main() /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { + _pos_sp_triplet_updated = false; publish_position_setpoint_triplet(); } @@ -882,9 +883,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - if (_global_pos.valid) { + if (_global_pos.global_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); 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 af04bb0bc4..e045ce4cca 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - bool flag_armed = false; - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; + global_pos.baro_valid = true; } } } @@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - flag_armed = armed.armed; - baro_offset -= z_est[0]; - corr_baro = 0.0f; - local_pos.ref_alt -= z_est[0]; - local_pos.ref_timestamp = t; - z_est[0] = 0.0f; - alt_avg = 0.0f; - } } /* sensor combined */ @@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + dt = fmaxf(fminf(0.02, dt), 0.005); t_prev = t; /* use GPS if it's valid and reference position initialized */ @@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; - baro_counter += offs_corr; + corr_baro += offs_corr; } /* accelerometer bias correction */ @@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.xy_global; + global_pos.global_valid = local_pos.xy_global; if (local_pos.xy_global) { double est_lat, est_lon; @@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.z_valid) { + global_pos.baro_alt = baro_offset - local_pos.z; + } + if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c3ea30cbfb..3c218e21fb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1244,6 +1244,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index baac2ee3ec..db87e3a6a2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -204,6 +204,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; + float baro_alt; + uint8_t flags; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -303,7 +305,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ae771ca008..ff9e98e1c4 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -61,17 +61,21 @@ */ struct vehicle_global_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool global_valid; /**< true if position satisfies validity criteria of estimator */ + bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ + + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude AMSL in meters */ float vel_n; /**< Ground north velocity, m/s */ float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ + + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** From 23a87f5a5204580b4fad435a75b38efc9d15c05c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 21:44:46 +0100 Subject: [PATCH 117/222] navigator: add home position check for RTL state --- src/modules/navigator/navigator_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 170e5df059..73b7c13d0f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -685,7 +685,8 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + /* switch to RTL if not already landed after RTL and home position set */ + if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -742,7 +743,7 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -815,6 +816,11 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); + /* publish position setpoint triplet on each position update if navigator active */ + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + _pos_sp_triplet_updated = true; + } + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { From 338b753a3c56813c5820e54209b4949e04a12ad9 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:00:07 +0800 Subject: [PATCH 118/222] Reduced stack size to 1200. Max stack space used was 956. --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 20853379de..ebf01a2f47 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, mavlink_thread_main, (const char **)argv); From 58a1f19d79da8a621454f11055534610921accd6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:09:05 +0800 Subject: [PATCH 119/222] Stack size reduced to 1816. Max stack reported by top was 1448 under HIL. --- 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 7b6fad6589..9fc7b748ad 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -871,7 +871,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 3000); + pthread_attr_setstacksize(&receiveloop_attr, 1816); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); From b0f65bb708e2728d562dbef8db04a846170d0ca6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:12:16 +0800 Subject: [PATCH 120/222] Stack size reduced to 1648. Max stack usage reported top at 1316. --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..ed04882b90 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -838,7 +838,7 @@ uorb_receive_start(void) pthread_attr_init(&uorb_attr); /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); + pthread_attr_setstacksize(&uorb_attr, 1648); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); From 70b1037c2ebd38e06a3d12dca7bad295da02e16c Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:35:34 +0800 Subject: [PATCH 121/222] Stack size reduced to 2408. Max stack usage reported by top in EASY mode at 1924. --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c096..a89c7eace0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, - 4096, + 2408, multirotor_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); From 0cc311b872c688f2242a4745cd3a6de4933c9e62 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:39:35 +0800 Subject: [PATCH 122/222] Reduced stack size to 2568. Max stack usage reported as 2052 with MARG + GPS with 3d lock - no px4flow, but should be more than enough buffer to accomodate its addition. --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3084b6d928..eb5a23b697 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From 1d70a65f40895b0c9e1ee5c65970efa1eae841e8 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:52:22 +0800 Subject: [PATCH 123/222] Stack size reduced to 1280. Max stack size reported as 1020 with UBX GPS with 3d lock. MTK not tested. --- src/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560fa..f2faf711ba 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -209,7 +209,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); From 48cec50dd30cd2b3163aedbeb11ae52866e2601b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 13:12:57 +0100 Subject: [PATCH 124/222] navigator: handle regaining global position lock while LANDing --- src/modules/navigator/navigator_main.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 73b7c13d0f..80bb1b7526 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -178,6 +178,7 @@ 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 _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -393,6 +394,7 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), @@ -819,12 +821,19 @@ Navigator::task_main() /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; - } - /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (check_mission_item_reached()) { - on_mission_item_reached(); + if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + /* got global position when landing, update setpoint */ + start_land(); + } + + _global_pos_valid = _global_pos.global_valid; + + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); + } } } From 6b709e21d3aa5e88458b4f3319a39b4abbadc085 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2014 21:26:28 +1100 Subject: [PATCH 125/222] FMUv2: set SPI GPIO pins on bus 1 to 50MHz this solves the ms5611 issue related to temperature. We now need to test if it is OK with all sensors --- 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 507df70a23..c6c771491d 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -260,9 +260,9 @@ * * There are sensors on SPI1, and SPI2 is connected to the FRAM. */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 From e7e3e132adcbc755cd6cc2b97f6c8c827a9140bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2014 21:30:15 +1100 Subject: [PATCH 126/222] FMUv2: push SPI2 GPIO speed up to 50MHz as well --- 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 c6c771491d..e56b14ba41 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -264,9 +264,9 @@ #define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 -#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) /************************************************************************************ * Public Data From 08a6057ef8c4aa796751c5ac07ab8efa7529b150 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 13:23:06 +0100 Subject: [PATCH 127/222] Increase SPI GPIO speed for FMUv1 analog to v2 --- nuttx-configs/px4fmu-v1/include/board.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index 27ace4b7db..ff1c634244 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -246,14 +246,14 @@ * * There are sensors on SPI1, and SPI3 is connected to the microSD slot. */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) -#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 -#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 -#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 -#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 +#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_NSS (GPIO_SPI3_NSS_2|GPIO_SPEED_50MHz) /* SPI DMA configuration for SPI3 (microSD) */ #define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 From 591b355981c781f6d30a6697b690225031792cfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 14:39:36 +0100 Subject: [PATCH 128/222] setpoint type IDLE added (for AUTO_READY state), LAND mode fixed --- .../mc_pos_control/mc_pos_control_main.cpp | 549 +++++++++--------- src/modules/navigator/navigator_main.cpp | 46 +- .../uORB/topics/position_setpoint_triplet.h | 9 +- 3 files changed, 324 insertions(+), 280 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 923a9dab05..fe8377a40d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -707,269 +707,16 @@ MulticopterPositionControl::task_main() reset_alt_sp(); } - /* 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); + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + R.identity(); + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); - - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - _vel_sp(2) = 0.0f; - } - - if (!_control_mode.flag_control_position_enabled) { - _reset_lat_lon_sp = true; - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - } - - if (!_control_mode.flag_control_manual_enabled) { - /* use constant descend rate when landing, ignore altitude setpoint */ - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - _vel_sp(2) = _params.land_speed; - } - - /* limit 3D speed only in AUTO mode */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); - - if (vel_sp_norm > 1.0f) { - _vel_sp /= vel_sp_norm; - } - } - - _global_vel_sp.vx = _vel_sp(0); - _global_vel_sp.vy = _vel_sp(1); - _global_vel_sp.vz = _vel_sp(2); - - /* publish velocity setpoint */ - if (_global_vel_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - - } else { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); - } - - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - /* reset integrals if needed */ - if (_control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = _params.thr_min; - - if (reset_int_z_manual) { - i = _manual.throttle; - - if (i < _params.thr_min) { - i = _params.thr_min; - - } else if (i > _params.thr_max) { - i = _params.thr_max; - } - } - - thrust_int(2) = -i; - mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - } else { - reset_int_z = true; - } - - if (_control_mode.flag_control_velocity_enabled) { - if (reset_int_xy) { - 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 { - reset_int_xy = true; - } - - /* velocity error */ - math::Vector<3> vel_err = _vel_sp - _vel; - - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; - _vel_prev = _vel; - - /* thrust vector in NED frame */ - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; - - if (!_control_mode.flag_control_velocity_enabled) { - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - } - - if (!_control_mode.flag_control_climb_rate_enabled) { - thrust_sp(2) = 0.0f; - } - - /* limit thrust vector and check for saturation */ - bool saturation_xy = false; - bool saturation_z = false; - - /* limit min lift */ - float thr_min = _params.thr_min; - - if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { - /* don't allow downside thrust direction in manual attitude mode */ - thr_min = 0.0f; - } - - if (-thrust_sp(2) < thr_min) { - thrust_sp(2) = -thr_min; - saturation_z = true; - } - - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) - thr_min = 0.0f; - } - } - - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; - } - } else { - /* thrust compensation for altitude only control mode */ - float att_comp; - - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; - saturation_z = true; - } else { - att_comp = 1.0f; - saturation_z = true; - } - - thrust_sp(2) *= att_comp; - } - - /* limit max thrust */ - float thrust_abs = thrust_sp.length(); - - if (thrust_abs > _params.thr_max) { - if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.thr_max) { - /* thrust Z component is too large, limit it */ - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - thrust_sp(2) = -_params.thr_max; - saturation_xy = true; - saturation_z = true; - - } else { - /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); - float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); - float k = thrust_xy_max / thrust_xy_abs; - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; - } - - } else { - /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; - thrust_sp *= k; - saturation_xy = true; - saturation_z = true; - } - - thrust_abs = _params.thr_max; - } - - /* update integrals */ - if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { - thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; - thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; - } - - if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { - thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; - - /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) - thrust_int(2) = 0.0f; - } - - /* calculate attitude setpoint from thrust vector */ - if (_control_mode.flag_control_velocity_enabled) { - /* desired body_z axis = -normalize(thrust_vector) */ - math::Vector<3> body_x; - math::Vector<3> body_y; - math::Vector<3> body_z; - - if (thrust_abs > SIGMA) { - body_z = -thrust_sp / thrust_abs; - - } else { - /* no thrust, set Z axis to safe value */ - body_z.zero(); - body_z(2) = 1.0f; - } - - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); - - if (fabsf(body_z(2)) > SIGMA) { - /* desired body_x axis, orthogonal to body_z */ - body_x = y_C % body_z; - - /* keep nose to front while inverted upside down */ - if (body_z(2) < 0.0f) { - body_x = -body_x; - } - - body_x.normalize(); - - } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x.zero(); - body_x(2) = 1.0f; - } - - /* desired body_y axis */ - body_y = body_z % body_x; - - /* fill rotation matrix */ - for (int i = 0; i < 3; i++) { - R(i, 0) = body_x(i); - R(i, 1) = body_y(i); - R(i, 2) = body_z(i); - } - - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - - /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> euler = R.to_euler(); - _att_sp.roll_body = euler(0); - _att_sp.pitch_body = euler(1); - /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - } - - _att_sp.thrust = thrust_abs; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = 0.0f; + _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); @@ -982,7 +729,283 @@ MulticopterPositionControl::task_main() } } else { - reset_int_z = true; + /* 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); + + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode.flag_control_position_enabled) { + _reset_lat_lon_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + /* use constant descend rate when landing, ignore altitude setpoint */ + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + if (!_control_mode.flag_control_manual_enabled) { + /* limit 3D speed only in non-manual modes */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } + } + + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } + + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual.throttle; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + thrust_int(2) = -i; + mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + } else { + reset_int_z = true; + } + + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + 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 { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } + + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + } + + _att_sp.thrust = thrust_abs; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } else { + reset_int_z = true; + } } } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 80bb1b7526..73514185b1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1030,8 +1030,11 @@ void Navigator::start_ready() { _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; + _mission_item_valid = false; _reset_loiter_pos = true; @@ -1229,20 +1232,32 @@ Navigator::start_rtl() void Navigator::start_land() { + /* 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; _reset_loiter_pos = true; - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.next.valid = false; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = _global_pos.alt; - _pos_sp_triplet.current.loiter_direction = 1; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _pos_sp_triplet.current.yaw = NAN; + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; } void @@ -1546,8 +1561,8 @@ Navigator::on_mission_item_reached() } } - } else { - /* RTL finished */ + } else if (myState == NAV_STATE_RTL) { + /* RTL completed */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); @@ -1558,6 +1573,11 @@ Navigator::on_mission_item_reached() _rtl_state = (RTLState)(_rtl_state + 1); set_rtl_item(); } + + } else if (myState == NAV_STATE_LAND) { + /* landing completed */ + mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); + dispatch(EVENT_READY_REQUESTED); } } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index e8d68281a8..cf1ddfc38f 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -54,10 +54,11 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, - SETPOINT_TYPE_LOITER, - SETPOINT_TYPE_TAKEOFF, - SETPOINT_TYPE_LAND, + SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s From d40382425f3b79bd662949e7259479e568e8698e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 14:51:57 +0100 Subject: [PATCH 129/222] Triggering param documentation generation --- Tools/px4params/.gitignore | 3 ++- Tools/px4params/dokuwikiout.py | 29 +++++++++++++++++++++++++++-- Tools/px4params/xmlrpc.sh | 5 +++++ 3 files changed, 34 insertions(+), 3 deletions(-) create mode 100644 Tools/px4params/xmlrpc.sh diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore index 73cf39575d..5d0378b4a2 100644 --- a/Tools/px4params/.gitignore +++ b/Tools/px4params/.gitignore @@ -1,2 +1,3 @@ parameters.wiki -parameters.xml \ No newline at end of file +parameters.xml +cookies.txt \ No newline at end of file diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 4d40a6201d..c4fadbd354 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -1,10 +1,24 @@ import output +from xml.sax.saxutils import escape class DokuWikiOutput(output.Output): def Generate(self, groups): - result = "" + pre_text = """ + + wiki.putPage + + + + :dev:parameters:reference + + + + + """ + result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values." 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" for param in group.GetParams(): code = param.GetFieldValue("code") @@ -34,4 +48,15 @@ class DokuWikiOutput(output.Output): result += "|" result += "|\n" result += "\n" - return result + post_text = """ + + + + + sum + Updated parameters automagically from code. + + + + """ + return pre_text + escape(result) + post_text diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh new file mode 100644 index 0000000000..36c52ff714 --- /dev/null +++ b/Tools/px4params/xmlrpc.sh @@ -0,0 +1,5 @@ +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" From 60859607ff240f1aaad05b306ba86b84d15be2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 15:11:50 +0100 Subject: [PATCH 130/222] Better output --- Tools/px4params/dokuwikiout.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index c4fadbd354..42da19e8bb 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -27,26 +27,26 @@ class DokuWikiOutput(output.Output): result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") if min_val is not None: - result += "| %s " % min_val + result += " | %s " % min_val else: - result += "|" + result += " | " max_val = param.GetFieldValue("max") if max_val is not None: - result += "| %s " % max_val + result += " | %s " % max_val else: - result += "|" + result += " | " def_val = param.GetFieldValue("default") if def_val is not None: result += "| %s " % def_val else: - result += "|" + result += " | " long_desc = param.GetFieldValue("long_desc") if long_desc is not None: long_desc = long_desc.replace("\n", "") result += "| %s " % long_desc else: - result += "|" - result += "|\n" + result += " | " + result += " |\n" result += "\n" post_text = """ From b81520cf30b329ad4d52f2197f25bfc5c8f5269f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 16:05:09 +0100 Subject: [PATCH 131/222] Use NAV_STATE_LAND instead RTL_STATE_LAND --- src/modules/navigator/navigator_main.cpp | 91 +++++++++++++----------- 1 file changed, 48 insertions(+), 43 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 73514185b1..46e7c2e188 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -237,8 +237,7 @@ private: RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LAND + RTL_STATE_DESCEND }; enum RTLState _rtl_state; @@ -304,6 +303,7 @@ private: void start_mission(); void start_rtl(); void start_land(); + void start_land_home(); /** * Guards offboard mission @@ -688,7 +688,8 @@ Navigator::task_main() /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ - if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -745,7 +746,8 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -994,7 +996,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, @@ -1040,8 +1042,8 @@ Navigator::start_ready() _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state != RTL_STATE_LAND) { - /* allow RTL if landed not at home */ + if (_rtl_state != RTL_STATE_DESCEND) { + /* reset RTL state if landed not at home */ _rtl_state = RTL_STATE_NONE; } @@ -1074,11 +1076,6 @@ Navigator::start_loiter() } _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - - if (_rtl_state == RTL_STATE_LAND) { - /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ - _rtl_state = RTL_STATE_DESCEND; - } } _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1260,6 +1257,36 @@ Navigator::start_land() _pos_sp_triplet.next.valid = false; } +void +Navigator::start_land_home() +{ + /* land to home position, should be called when hovering above home, from RTL state */ + _do_takeoff = false; + _reset_loiter_pos = true; + + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; +} + void Navigator::set_rtl_item() { @@ -1350,33 +1377,6 @@ Navigator::set_rtl_item() break; } - case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } - default: { mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); @@ -1563,10 +1563,15 @@ Navigator::on_mission_item_reached() } else if (myState == NAV_STATE_RTL) { /* RTL completed */ - if (_rtl_state == RTL_STATE_LAND) { - /* landed at home position */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); - dispatch(EVENT_READY_REQUESTED); + if (_rtl_state == RTL_STATE_DESCEND) { + /* hovering above home position, land if needed or loiter */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { + dispatch(EVENT_LAND_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } } else { /* next RTL step */ From 6f559b279e3d03dbf28eff436b41f3b022c5fa82 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 17:59:02 +0100 Subject: [PATCH 132/222] mc_att_control: yaw deadzone increased --- 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 245ac024b8..44baccefc0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -85,7 +85,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f -#define YAW_DEADZONE 0.01f +#define YAW_DEADZONE 0.05f #define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl From 01c9092213449c761759bcda11ef9613226be713 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 18:06:03 +0100 Subject: [PATCH 133/222] Revert "Added check for offboard_control_signal_lost when switching to offboard mode with rc switch" This reverts commit 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3687f55fbb..bec6884e2d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; From 2b17909f0c4131f6a728697f5de1eb13a1337234 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 19:18:44 +0100 Subject: [PATCH 134/222] Changed param location --- Tools/px4params/dokuwikiout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 42da19e8bb..c5cf65ea60 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -9,7 +9,7 @@ class DokuWikiOutput(output.Output): - :dev:parameters:reference + :firmware:parameters From 7d2f2523f86b4622815a323969a77b245a4ceaa3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 22:11:38 +0100 Subject: [PATCH 135/222] navigator: reset mission item timer on state and mission item changes --- src/modules/navigator/navigator_main.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 061baf0c5f..e961a8f942 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,6 +865,9 @@ 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); @@ -1112,6 +1115,9 @@ 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); @@ -1290,6 +1296,9 @@ Navigator::start_land_home() void Navigator::set_rtl_item() { + /*reset time counter for new RTL item */ + _time_first_inside_orbit = 0; + switch (_rtl_state) { case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); From 1458bdfbcb34d251da2476386864ee680407b90f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 13:07:17 +0100 Subject: [PATCH 136/222] Pure code style fix of cpuload, no funcationality changes --- src/modules/systemlib/cpuload.c | 3 +-- src/modules/systemlib/cpuload.h | 12 ++++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index afc5b072c3..ccc516f396 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -93,8 +93,7 @@ void cpuload_initialize_once() #endif /* CONFIG_SCHED_WORKQUEUE */ // perform static initialization of "system" threads - for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) - { + for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) { system_load.tasks[system_load.total_count].start_time = now; system_load.tasks[system_load.total_count].total_runtime = 0; system_load.tasks[system_load.total_count].curr_start_time = 0; diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index c7aa18d3c4..16d132fdb8 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -40,15 +40,15 @@ __BEGIN_DECLS #include struct system_load_taskinfo_s { - uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load - uint64_t curr_start_time; ///< Start time of the current scheduling slot - uint64_t start_time; ///< FIRST start time of task - FAR struct tcb_s *tcb; ///< - bool valid; ///< Task is currently active / valid + uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time; ///< Start time of the current scheduling slot + uint64_t start_time; ///< FIRST start time of task + FAR struct tcb_s *tcb; ///< + bool valid; ///< Task is currently active / valid }; struct system_load_s { - uint64_t start_time; ///< Global start time of measurements + uint64_t start_time; ///< Global start time of measurements struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; uint8_t initialized; int total_count; From 13a5b5b4a3d8e487fb836e14185d61c03493ef0a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 Jan 2014 16:02:17 +0100 Subject: [PATCH 137/222] mc_att_control: major cleanup and code reorganization --- .../mc_att_control/mc_att_control_main.cpp | 716 ++++++++++-------- 1 file changed, 383 insertions(+), 333 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 44baccefc0..e37fe18b8f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -113,28 +113,39 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _arming_sub; /**< arming status of outputs */ + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ + 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 */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ struct { param_t att_p; @@ -160,9 +171,9 @@ private: int parameters_update(); /** - * Update control outputs + * Check for parameter update and handle it. */ - void control_update(); + void parameter_update_poll(); /** * Check for changes in vehicle control mode. @@ -175,15 +186,30 @@ private: void vehicle_manual_poll(); /** - * Check for set triplet updates. + * Check for attitude setpoint updates. */ - void vehicle_setpoint_poll(); + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); /** * Check for arming status updates. */ void arming_status_poll(); + /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + /** * Shim for calling task_main from task_create. */ @@ -195,7 +221,7 @@ private: void task_main() __attribute__((noreturn)); }; -namespace att_control +namespace mc_att_control { /* oddly, ERROR is not defined for c++ */ @@ -213,34 +239,42 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), - _att_sp_sub(-1), - _control_mode_sub(-1), + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), _params_sub(-1), - _manual_sub(-1), - _arming_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), /* publications */ _att_sp_pub(-1), - _rates_sp_pub(-1), + _v_rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) { - memset(&_att, 0, sizeof(_att)); - memset(&_att_sp, 0, sizeof(_att_sp)); - memset(&_manual, 0, sizeof(_manual)); - memset(&_control_mode, 0, sizeof(_control_mode)); - memset(&_arming, 0, sizeof(_arming)); + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_armed, 0, sizeof(_armed)); _params.p.zero(); _params.rate_p.zero(); _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(); _params_handles.att_p = param_find("MC_ATT_P"); _params_handles.yaw_p = param_find("MC_YAW_P"); @@ -276,7 +310,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - att_control::g_control = nullptr; + mc_att_control::g_control = nullptr; } int @@ -312,42 +346,68 @@ MulticopterAttitudeControl::parameters_update() } void -MulticopterAttitudeControl::vehicle_control_mode_poll() +MulticopterAttitudeControl::parameter_update_poll() { - bool control_mode_updated; + bool updated; /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &control_mode_updated); + orb_check(_params_sub, &updated); - if (control_mode_updated) { + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); } } void MulticopterAttitudeControl::vehicle_manual_poll() { - bool manual_updated; + bool updated; /* get pilots inputs */ - orb_check(_manual_sub, &manual_updated); + orb_check(_manual_control_sp_sub, &updated); - if (manual_updated) { + if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } void -MulticopterAttitudeControl::vehicle_setpoint_poll() +MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); + bool updated; + orb_check(_v_att_sp_sub, &updated); - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +void +MulticopterAttitudeControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); } } @@ -355,351 +415,341 @@ void MulticopterAttitudeControl::arming_status_poll() { /* check if there is a new setpoint */ - bool arming_updated; - orb_check(_arming_sub, &arming_updated); + bool updated; + orb_check(_armed_sub, &updated); - if (arming_updated) { - orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + */ +void +MulticopterAttitudeControl::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.throttle; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual_control_sp.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual_control_sp.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.yaw; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.roll; + _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + 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]); + + } 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); + + /* 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)); + _v_att_sp.R_valid = true; + } + + /* publish the attitude setpoint if needed */ + if (publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } + } + + /* rotation matrix for current state */ + _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)); + + /* axis and sin(angle) of desired rotation */ + 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); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + 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)); + + } else { + /* zero roll/pitch rotation */ + 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)); + 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; + + if (e_R_z_cos < 0.0f) { + /* 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); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.p.emult(e_R); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w; +} + +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +MulticopterAttitudeControl::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector<3> rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > 0.1f && _att_control.length() < _thrust_sp) { + for (int i = 0; i < 3; i++) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } } } void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { - att_control::g_control->task_main(); + mc_att_control::g_control->task_main(); } void MulticopterAttitudeControl::task_main() { - /* inform about start */ warnx("started"); fflush(stdout); /* * do subscriptions */ - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit attitude updates to 100Hz */ - orb_set_interval(_att_sub, 10); + /* 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(); - /* initialize values of critical structs until first regular update */ - _arming.armed = false; + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; - /* get an initial update for all sensor and status data */ - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - arming_status_poll(); - - /* setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - R_sp.identity(); - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.identity(); - - /* current angular rates */ - math::Vector<3> rates; - rates.zero(); - - /* angular rates integral error */ - math::Vector<3> rates_int; - rates_int.zero(); - - /* identity matrix */ - math::Matrix<3, 3> I; - I.identity(); - - math::Quaternion q; - - bool reset_yaw_sp = true; - - /* wakeup source(s) */ - struct pollfd fds[2]; - - /* Setup of loop */ - fds[0].fd = _params_sub; + fds[0].fd = _v_att_sub; fds[0].events = POLLIN; - fds[1].fd = _att_sub; - fds[1].events = POLLIN; while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for _task_should_exit */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); continue; } perf_begin(_loop_perf); - /* only update parameters if they changed */ + /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { - /* copy the topic to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - parameters_update(); - } - - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); - /* guard against too large dt's */ - if (dt > 0.02f) + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { dt = 0.02f; + } /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - vehicle_setpoint_poll(); + /* check for updates in other topics */ + parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } - - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - _att_sp.R_valid = false; - publish_att_sp = true; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; - } + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; } - if (_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_att_sp.R_body[0][0]); + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); - /* copy rotation matrix back to setpoint struct */ - memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - } - - if (publish_att_sp) { - /* publish the attitude setpoint */ - _att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } } - - /* rotation matrix for current state */ - R.set(_att.R); - - /* current body angular rates */ - rates(0) = _att.rollspeed; - rates(1) = _att.pitchspeed; - rates(2) = _att.yawspeed; - - /* 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)); - - /* axis and sin(angle) of desired rotation */ - 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); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - 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)); - - } else { - /* zero roll/pitch rotation */ - 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)); - 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; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - 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)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* angular rates setpoint*/ - math::Vector<3> rates_sp = _params.p.emult(e_R); - - /* feed forward yaw setpoint rate */ - rates_sp(2) += yaw_sp_move_rate * yaw_w; - - /* reset integral if disarmed */ - // TODO add LANDED flag here - if (!_arming.armed) { - rates_int.zero(); - } - - /* rate controller */ - math::Vector<3> rates_err = rates_sp - rates; - math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; - _rates_prev = rates; - - /* update integral */ - for (int i = 0; i < 3; i++) { - float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i)) { - if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { - rates_int(i) = rate_i; - } - } - } - - /* publish the attitude rates setpoint */ - _rates_sp.roll = rates_sp(0); - _rates_sp.pitch = rates_sp(1); - _rates_sp.yaw = rates_sp(2); - _rates_sp.thrust = _att_sp.thrust; - _rates_sp.timestamp = hrt_absolute_time(); - - if (_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); - - } else { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } - - /* publish the attitude controls */ - if (_control_mode.flag_control_rates_enabled) { - _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; - _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; - _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; - _actuators.timestamp = hrt_absolute_time(); - - } else { - /* controller disabled, publish zero attitude controls */ - _actuators.control[0] = 0.0f; - _actuators.control[1] = 0.0f; - _actuators.control[2] = 0.0f; - _actuators.control[3] = 0.0f; - _actuators.timestamp = hrt_absolute_time(); - } - - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } } perf_end(_loop_perf); @@ -739,17 +789,17 @@ int mc_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) errx(1, "already running"); - att_control::g_control = new MulticopterAttitudeControl; + mc_att_control::g_control = new MulticopterAttitudeControl; - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "alloc failed"); - if (OK != att_control::g_control->start()) { - delete att_control::g_control; - att_control::g_control = nullptr; + if (OK != mc_att_control::g_control->start()) { + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; err(1, "start failed"); } @@ -757,16 +807,16 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "not running"); - delete att_control::g_control; - att_control::g_control = nullptr; + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; exit(0); } if (!strcmp(argv[1], "status")) { - if (att_control::g_control) { + if (mc_att_control::g_control) { errx(0, "running"); } else { From 9cbc31b588f3bac6d3b03870801c97068a4b4ecc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 Jan 2014 19:44:06 +0100 Subject: [PATCH 138/222] mavlink: revert stack size 2048 to fix suspending in HIL mode --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9bb92cbf5a..4d975066f7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -794,7 +794,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 2048, mavlink_thread_main, (const char **)argv); From eb177def141d321b43a4c20819179423e128a92f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 Jan 2014 19:44:06 +0100 Subject: [PATCH 139/222] mavlink: revert stack size 2048 to fix suspending in HIL mode --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ebf01a2f47..20853379de 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 2048, mavlink_thread_main, (const char **)argv); From 5316741ed40965b837fab77074ff4fbd4fe6f858 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 21:53:27 +0100 Subject: [PATCH 140/222] Revert "mavlink: revert stack size 2048 to fix suspending in HIL mode" This reverts commit eb177def141d321b43a4c20819179423e128a92f. --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 20853379de..ebf01a2f47 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, mavlink_thread_main, (const char **)argv); From 8d79d919504b4b92ad05a7ebc12334083ae0f4b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 21:54:29 +0100 Subject: [PATCH 141/222] Revert "Merge pull request #620 from pigeonhunter/stack_sizes" This reverts commit 3b31a6b1b9756eb191eaaafb1c137e6874079281, reversing changes made to 70afb3ca3b3f1844241c9c9312579bbb2475232c. --- src/drivers/gps/gps.cpp | 2 +- src/drivers/px4io/px4io.cpp | 2 +- src/modules/commander/commander.cpp | 4 ++-- src/modules/mavlink/mavlink.c | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/orb_listener.c | 2 +- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 8 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f2faf711ba..6b72d560fa 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -209,7 +209,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5da288661b..efcf4d12b9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -761,7 +761,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 52cf250860..2a2bcca727 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -250,7 +250,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2088, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -685,7 +685,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 1728); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ebf01a2f47..20853379de 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 2048, mavlink_thread_main, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9fc7b748ad..7b6fad6589 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -871,7 +871,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 1816); + pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index bfb824db7e..e1dabfd213 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -838,7 +838,7 @@ uorb_receive_start(void) pthread_attr_init(&uorb_attr); /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 1648); + pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a89c7eace0..3d23d0c096 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, - 2408, + 4096, multirotor_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); 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 eb5a23b697..3084b6d928 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From 9cdc13185b4a956c32a2281e637284a13bfd1a40 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 30 Jan 2014 23:09:20 +0100 Subject: [PATCH 142/222] Remove differential aileron mixing as this will result in a pitching effect on flying wings. --- ROMFS/px4fmu_common/mixers/FMU_Q.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index 17ff711513..b8ecbc879f 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -25,13 +25,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 8000 8000 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 -8000 -5000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 -6000 -6000 0 -10000 10000 Output 2 -------- From 83df116c7aa21b6d68f2aa31c4526dd822495d70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 23:11:37 +0100 Subject: [PATCH 143/222] Hotfix: Move mixer variables in test routine into function --- src/systemcmds/tests/test_mixer.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 2a47551ee6..df382e2c68 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -65,20 +65,20 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; -static bool should_arm = false; -uint16_t r_page_servo_disarmed[output_max]; -uint16_t r_page_servo_control_min[output_max]; -uint16_t r_page_servo_control_max[output_max]; -uint16_t r_page_servos[output_max]; -uint16_t servo_predicted[output_max]; - -/* - * PWM limit structure - */ -pwm_limit_t pwm_limit; int test_mixer(int argc, char *argv[]) { + /* + * PWM limit structure + */ + pwm_limit_t pwm_limit; + static bool should_arm = false; + uint16_t r_page_servo_disarmed[output_max]; + uint16_t r_page_servo_control_min[output_max]; + uint16_t r_page_servo_control_max[output_max]; + uint16_t r_page_servos[output_max]; + uint16_t servo_predicted[output_max]; + warnx("testing mixer"); char *filename = "/etc/mixers/IO_pass.mix"; From 498155cf676623abd955ba66bb3e90ab538383d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 00:46:28 +0100 Subject: [PATCH 144/222] mc_att_control: yaw dead zone fixed, added MC_YAW_FF (yaw feed-forward) parameter --- .../mc_att_control/mc_att_control_main.cpp | 26 ++++++++++++++++--- .../mc_att_control/mc_att_control_params.c | 1 + 2 files changed, 24 insertions(+), 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 e37fe18b8f..dc3da0f467 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -156,6 +156,9 @@ private: param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -163,6 +166,9 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; } _params; /** @@ -284,6 +290,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -342,6 +351,10 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + return OK; } @@ -461,9 +474,16 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - if (_manual_control_sp.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual_control_sp.yaw) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -592,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt) _rates_sp = _params.p.emult(e_R); /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w; + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } /* 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 a170365eec..0a89c3f4e5 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -51,3 +51,4 @@ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); From af3b24dc001bfe84ad783a8b85e3b576330ce257 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 00:47:31 +0100 Subject: [PATCH 145/222] mc_pos_control: removed unused parameter reading RC_SCALE_YAW --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fe8377a40d..e9b92119d6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -153,7 +153,6 @@ private: param_t rc_scale_pitch; param_t rc_scale_roll; - param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -165,7 +164,6 @@ private: float rc_scale_pitch; float rc_scale_roll; - float rc_scale_yaw; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -312,7 +310,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(true); @@ -361,7 +358,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); float v; param_get(_params_handles.xy_p, &v); From 88d3f875ffd36a3e05899222aa2eb27848dcf6d1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 08:48:00 +0100 Subject: [PATCH 146/222] attitude_estimator_ekf: enable acceleration compensation by default --- .../attitude_estimator_ekf/attitude_estimator_ekf_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 44f47b47ca..4154e3db4a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); -PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); +PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { From 282e0bb6703e6f013eee690bda97e09045837fd3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 11:35:11 +0100 Subject: [PATCH 147/222] mc_att_control: separate gains for roll and pitch --- .../mc_att_control/mc_att_control_main.cpp | 76 +++++++++++-------- .../mc_att_control/mc_att_control_params.c | 14 ++-- 2 files changed, 53 insertions(+), 37 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 dc3da0f467..4086a227a0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -148,11 +148,15 @@ private: bool _reset_yaw_sp; /**< reset yaw setpoint flag */ struct { - param_t att_p; + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; param_t yaw_p; - param_t att_rate_p; - param_t att_rate_i; - param_t att_rate_d; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; @@ -162,7 +166,7 @@ private: } _params_handles; /**< handles for interesting parameters */ struct { - math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ @@ -267,7 +271,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_armed, 0, sizeof(_armed)); - _params.p.zero(); + _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); @@ -282,15 +286,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : I.identity(); - _params_handles.att_p = param_find("MC_ATT_P"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); - _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); - _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -327,27 +335,33 @@ MulticopterAttitudeControl::parameters_update() { float v; - param_get(_params_handles.att_p, &v); - _params.p(0) = v; - _params.p(1) = v; - param_get(_params_handles.yaw_p, &v); - _params.p(2) = v; - - param_get(_params_handles.att_rate_p, &v); + /* roll */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; - - param_get(_params_handles.att_rate_i, &v); - _params.rate_i(0) = v; - _params.rate_i(1) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; - - param_get(_params_handles.att_rate_d, &v); - _params.rate_d(0) = v; - _params.rate_d(1) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; @@ -609,7 +623,7 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* calculate angular rates setpoint */ - _rates_sp = _params.p.emult(e_R); + _rates_sp = _params.att_p.emult(e_R); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; 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 0a89c3f4e5..27a45b6bbc 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,13 +41,15 @@ #include -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); From 103bece7266bbcbcd3f31aaf165ea6f902dec9f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 11:36:25 +0100 Subject: [PATCH 148/222] rc scripts cleanup, avoid duplicating parameters, use inheritance instead --- .../px4fmu_common/init.d/10015_tbs_discovery | 14 ++-- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 14 ++-- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 34 +------- .../{12001_octo_cox_pwm => 12001_octo_cox} | 18 ++-- ROMFS/px4fmu_common/init.d/4001_quad_x | 52 ++++++++++++ ROMFS/px4fmu_common/init.d/4008_ardrone | 57 ------------- ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 83 ------------------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 14 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 14 ++-- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 14 ++-- ROMFS/px4fmu_common/init.d/5001_quad_+ | 10 +++ ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 --------- ROMFS/px4fmu_common/init.d/6001_hexa_x | 12 +++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 --------- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 10 +++ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 --------- ROMFS/px4fmu_common/init.d/8001_octo_x | 12 +++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 --------- ROMFS/px4fmu_common/init.d/9001_octo_+ | 10 +++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 --------- ROMFS/px4fmu_common/init.d/rc.autostart | 21 ++--- 21 files changed, 164 insertions(+), 410 deletions(-) rename ROMFS/px4fmu_common/init.d/{12001_octo_cox_pwm => 12001_octo_cox} (63%) create mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/4008_ardrone delete mode 100644 ROMFS/px4fmu_common/init.d/4009_ardrone_flow create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+ delete mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x delete mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+ delete mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x delete mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+ delete mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 56c74a3b52..49a50e1309 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 5.0 - param set MC_ATT_I 0.0 + 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_YAW_I 0.15 - param set MC_ATTRATE_P 0.17 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.006 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index a3bcb63eb8..6c9cb6983a 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 9.0 - param set MC_ATT_I 0.0 + 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_YAW_I 0.15 - param set MC_ATTRATE_P 0.13 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 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 2d497374a5..84e2bc5d47 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -5,38 +5,6 @@ # Maintainers: Anton Babushkin # -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_YAW_P 2.0 - param set MC_YAW_I 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_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_TILT_MAX 1.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_D 0.01 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_VEL_P 0.1 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_VEL_P 0.1 -fi +sh /etc/init.d/4001_quad_x set HIL yes - -set VEHICLE_TYPE mc -set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox similarity index 63% rename from ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm rename to ROMFS/px4fmu_common/init.d/12001_octo_cox index 5f3cec4e0a..2bea6d4899 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -10,15 +10,15 @@ 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_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 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x new file mode 100644 index 0000000000..ca4694d81c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -0,0 +1,52 @@ +#!nsh +# +# Generic 10” Quad X geometry +# +# Maintainers: Lorenz Meier , 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.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 VEHICLE_TYPE mc +set MIXER FMU_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/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone deleted file mode 100644 index f6f09ac220..0000000000 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ /dev/null @@ -1,57 +0,0 @@ -#!nsh - -echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" - -# -# 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 - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -param set BAT_V_SCALING 0.008381 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow deleted file mode 100644 index e2cb8833dc..0000000000 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ /dev/null @@ -1,83 +0,0 @@ -#!nsh - -echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" - -# -# 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 - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -param set BAT_V_SCALING 0.008381 - -# -# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start the position estimator -# -flow_position_estimator start - -# -# Fire up the multi rotor attitude controller -# -mc_att_control_vector start - -# -# Fire up the flow position controller -# -flow_position_control start - -# -# Fire up the flow speed controller -# -flow_speed_control start - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index e0cf92d97f..6acdc9f81d 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 + 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_YAW_I 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_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ced69783db..cca635e661 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 + 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_YAW_I 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_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index e1423e0082..4fea828a75 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 + param set MC_ROLL_P 5.5 + param set MC_ROLLRATE_P 0.14 + param set MC_ROLLRATE_I 0 + param set MC_ROLLRATE_D 0.006 + param set MC_PITCH_P 5.5 + param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_I 0 + param set MC_PITCHRATE_D 0.006 param set MC_YAW_P 0.6 - param set MC_YAW_I 0 - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ new file mode 100644 index 0000000000..7f5a6fc078 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -0,0 +1,10 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm deleted file mode 100644 index 2e5f6ca4f5..0000000000 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Quad + geometry -# -# Maintainers: 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 FMU_quad_+ - -set PWM_OUTPUTS 1234 -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/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x new file mode 100644 index 0000000000..e72e15dd4d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -0,0 +1,12 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 123456 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm deleted file mode 100644 index ddec8f36ef..0000000000 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Hexa X geometry -# -# Maintainers: 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 FMU_hexa_x - -set PWM_OUTPUTS 1234 -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/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ new file mode 100644 index 0000000000..dade0630da --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -0,0 +1,10 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/6001_hexa_x + +set MIXER FMU_hexa_+ diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm deleted file mode 100644 index 106e0fb54c..0000000000 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Hexa + geometry -# -# Maintainers: 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 FMU_hexa_+ - -set PWM_OUTPUTS 1234 -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/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x new file mode 100644 index 0000000000..af632ed908 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -0,0 +1,12 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +set MIXER FMU_octo_x + +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm deleted file mode 100644 index f0eea339b8..0000000000 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Octo X geometry -# -# Maintainers: 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 FMU_octo_x - -set PWM_OUTPUTS 1234 -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/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ new file mode 100644 index 0000000000..9bf5e09328 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -0,0 +1,10 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/8001_octo_x + +set MIXER FMU_octo_+ diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm deleted file mode 100644 index 992a7aeba6..0000000000 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Octo + geometry -# -# Maintainers: 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 FMU_octo_+ - -set PWM_OUTPUTS 1234 -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/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 34da2dfef2..030806fd7e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -101,14 +101,9 @@ fi # Quad X # -if param compare SYS_AUTOSTART 4008 8 +if param compare SYS_AUTOSTART 4001 then - #sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4009 9 -then - #sh /etc/init.d/4009_ardrone_flow + sh /etc/init.d/4001_quad_x fi if param compare SYS_AUTOSTART 4010 10 @@ -132,7 +127,7 @@ fi if param compare SYS_AUTOSTART 5001 then - sh /etc/init.d/5001_quad_+_pwm + sh /etc/init.d/5001_quad_+ fi # @@ -141,7 +136,7 @@ fi if param compare SYS_AUTOSTART 6001 then - sh /etc/init.d/6001_hexa_x_pwm + sh /etc/init.d/6001_hexa_x fi # @@ -150,7 +145,7 @@ fi if param compare SYS_AUTOSTART 7001 then - sh /etc/init.d/7001_hexa_+_pwm + sh /etc/init.d/7001_hexa_+ fi # @@ -159,7 +154,7 @@ fi if param compare SYS_AUTOSTART 8001 then - sh /etc/init.d/8001_octo_x_pwm + sh /etc/init.d/8001_octo_x fi # @@ -168,7 +163,7 @@ fi if param compare SYS_AUTOSTART 9001 then - sh /etc/init.d/9001_octo_+_pwm + sh /etc/init.d/9001_octo_+ fi # @@ -191,5 +186,5 @@ fi if param compare SYS_AUTOSTART 12001 then - sh /etc/init.d/12001_octo_cox_pwm + sh /etc/init.d/12001_octo_cox fi From 9f88cbdc0b850f4136fd68a379663619fb42e150 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 12:18:52 +0100 Subject: [PATCH 149/222] autostart scripts: default MPC parameters updated --- .../px4fmu_common/init.d/10015_tbs_discovery | 18 +++++++++++ ROMFS/px4fmu_common/init.d/10016_3dr_iris | 18 ++++++----- ROMFS/px4fmu_common/init.d/12001_octo_cox | 31 ++----------------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 18 ++++++----- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 18 ++++++++++- 5 files changed, 59 insertions(+), 44 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 49a50e1309..3a8f14a9ed 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -22,6 +22,24 @@ then 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 fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 6c9cb6983a..2ce0334b4c 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -23,19 +23,23 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 1.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_D 0.01 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_MAX 5 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_D 0.0 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_MAX 3 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 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 2bea6d4899..77813268a6 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,36 +2,9 @@ # # Generic 10” Octo coaxial geometry # -# Maintainers: Lorenz Meier +# Maintainers: Lorenz Meier , 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.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/8001_octo_x -set VEHICLE_TYPE mc set MIXER FMU_octo_cox - -set PWM_OUTPUTS 1234 -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/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 6acdc9f81d..c78da2d6c5 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -23,19 +23,23 @@ then param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 1.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_D 0.01 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_MAX 5 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_D 0.0 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_MAX 3 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 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index cca635e661..8027b9d42a 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -23,7 +23,23 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 - # TODO add default MPC parameters + 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 From 5a7a356c2038544efbde562398aa5a50cb7e19d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 12:24:29 +0100 Subject: [PATCH 150/222] autostart 4012_hk_x550 script updated --- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 4fea828a75..98e1f80aa4 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -23,7 +23,23 @@ then param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 - # TODO add default MPC parameters + 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 @@ -31,3 +47,6 @@ set MIXER FMU_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 From d933d523eb74ee2290c56afcd11fe8e85c6e702b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 16:46:54 +0100 Subject: [PATCH 151/222] mc_att_control: att rate integral fix --- src/modules/mc_att_control/mc_att_control_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 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 4086a227a0..a0accb8552 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -654,13 +654,15 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f && _att_control.length() < _thrust_sp) { + if (_thrust_sp > 0.1f) { for (int i = 0; i < 3; i++) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } } } } From 7d17a48b3c38c90509963beb7d776a44295b82d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 17:34:46 +0100 Subject: [PATCH 152/222] param set: fixed float format --- src/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 580fdc62f4..0cbba0a374 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -320,7 +320,7 @@ do_set(const char* name, const char* val) char* end; f = strtod(val,&end); param_set(param, &f); - printf(" -> new: %f\n", f); + printf(" -> new: %4.4f\n", (double)f); } From 0be7bd3166969294bdd56b853b65248442219b80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 00:07:50 +0100 Subject: [PATCH 153/222] mc_pos_control: max position setpoint offset limiting fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 e9b92119d6..057cb051d8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -645,7 +645,7 @@ MulticopterPositionControl::task_main() if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); + add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); } From bb8be966fcaa484c0f8209da41760d4bc24d6c5f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:20:48 +0100 Subject: [PATCH 154/222] mc_pos_control: more safe tilt limiting --- .../mc_pos_control/mc_pos_control_main.cpp | 44 +++++++++++-------- 1 file changed, 26 insertions(+), 18 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 057cb051d8..3194534b9a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -839,30 +839,38 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } + float tilt_max = _params.tilt_max; + + /* adjust limits for landing mode */ + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + + /* limit min lift */ if (-thrust_sp(2) < thr_min) { thrust_sp(2) = -thr_min; saturation_z = true; } - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) - thr_min = 0.0f; - } - } - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } } } else { /* thrust compensation for altitude only control mode */ From 8a203951594282a297b2af402a82b85f0f927619 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:29:51 +0100 Subject: [PATCH 155/222] mc_pos_control: fixed yaw setpoint in AUTO --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 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 3194534b9a..0d65b5b03c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -688,10 +688,16 @@ MulticopterPositionControl::task_main() _reset_lat_lon_sp = true; _reset_alt_sp = true; + /* update position setpoint */ _lat_sp = _pos_sp_triplet.current.lat; _lon_sp = _pos_sp_triplet.current.lon; _alt_sp = _pos_sp_triplet.current.alt; + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + } else { /* no waypoint, loiter, reset position setpoint if needed */ reset_lat_lon_sp(); @@ -711,7 +717,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = 0.0f; + _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); From f835980b468fe44c49051df0b58f830bafb256f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:32:46 +0100 Subject: [PATCH 156/222] mc_pos_control: more correct control flags usage --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 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 0d65b5b03c..4fb9bd663e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -672,7 +672,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } - } else if (_control_mode.flag_control_auto_enabled) { + } else { /* always use AMSL altitude for AUTO */ select_alt(true); @@ -703,13 +703,9 @@ MulticopterPositionControl::task_main() reset_lat_lon_sp(); reset_alt_sp(); } - } else { - /* no control, reset setpoint */ - reset_lat_lon_sp(); - reset_alt_sp(); } - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); @@ -751,7 +747,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -848,7 +844,7 @@ MulticopterPositionControl::task_main() float tilt_max = _params.tilt_max; /* adjust limits for landing mode */ - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; From 8897894b19e8de4ad7960a0fa552ed12fc2f0200 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:14:21 +0100 Subject: [PATCH 157/222] commander, navigator, mc_att_control, mc_pos_control: code style fixed --- src/modules/commander/commander.cpp | 2 + .../commander/state_machine_helper.cpp | 2 + .../mc_att_control/mc_att_control_main.cpp | 6 ++- .../mc_pos_control/mc_pos_control_main.cpp | 11 ++++- src/modules/navigator/navigator_main.cpp | 5 ++- src/modules/navigator/navigator_mission.cpp | 44 ++++++++++++------- 6 files changed, 50 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713d..e9da69232d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,6 +369,7 @@ int arm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } @@ -381,6 +382,7 @@ int disarm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e3..e5d77b246a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -382,6 +382,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { status->set_nav_state = NAV_STATE_RTL; @@ -392,6 +393,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { status->set_nav_state = NAV_STATE_LAND; 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 a0accb8552..db5e2e9bb0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; + } else { yaw_sp_move_rate += YAW_DEADZONE; } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; @@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } 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 4fb9bd663e..25d34c872d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global) { if (global != _use_global_alt) { _use_global_alt = global; + if (global) { /* switch from barometric to global altitude */ _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { /* switch from global to barometric altitude */ _alt_sp += _global_pos.baro_alt - _global_pos.alt; @@ -589,6 +591,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* select altitude source and update setpoint */ select_alt(_global_pos.global_valid); + if (!_use_global_alt) { alt = _global_pos.baro_alt; } @@ -845,9 +848,10 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) thr_min = 0.0f; } @@ -863,9 +867,11 @@ MulticopterPositionControl::task_main() if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { /* max horizontal thrust for given vertical thrust*/ float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; @@ -874,15 +880,18 @@ MulticopterPositionControl::task_main() } } } + } else { /* thrust compensation for altitude only control mode */ float att_comp; if (_att.R[2][2] > TILT_COS_MAX) { att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; saturation_z = true; + } else { att_comp = 1.0f; saturation_z = true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e961a8f942..6e4b5f0a06 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)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -747,7 +747,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached() if (_rtl_state == RTL_STATE_DESCEND) { /* hovering above home position, land if needed or loiter */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { dispatch(EVENT_LAND_REQUESTED); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 6576aae707..e72caf98e9 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -52,8 +52,8 @@ static const int ERROR = -1; -Mission::Mission() : - +Mission::Mission() : + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), @@ -65,7 +65,7 @@ Mission::Mission() : Mission::~Mission() { - + } void @@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool { /* try onboard mission first */ if (current_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } + _current_mission_type = MISSION_TYPE_ONBOARD; *onboard = true; *index = _current_onboard_mission_index; - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ _current_mission_type = MISSION_TYPE_NONE; return ERROR; } + _current_mission_type = MISSION_TYPE_OFFBOARD; *onboard = false; *index = _current_offboard_mission_index; @@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { /* try onboard mission first */ if (next_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; @@ -244,14 +254,16 @@ void Mission::move_to_next() { switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - case MISSION_TYPE_NONE: - default: - break; + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } } \ No newline at end of file From e8dd70e815033ec1a291ac5190dacfeb7d728dba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 11:58:34 +0100 Subject: [PATCH 158/222] fw: fix global position lat/lon read-in (lat/lon have changed to double lately) --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 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 a62b53221c..3ef1871a8b 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 @@ -785,26 +785,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; if (pos_sp_triplet.previous.valid) { - prev_wp(0) = pos_sp_triplet.previous.lat; - prev_wp(1) = pos_sp_triplet.previous.lon; + prev_wp(0) = (float)pos_sp_triplet.previous.lat; + prev_wp(1) = (float)pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = pos_sp_triplet.current.lat; - prev_wp(1) = pos_sp_triplet.current.lon; + prev_wp(0) = (float)pos_sp_triplet.current.lat; + prev_wp(1) = (float)pos_sp_triplet.current.lon; } @@ -1263,7 +1263,7 @@ FixedwingPositionControl::task_main() // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); - math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* * Attempt to control position, on success (= sensors present and not in manual mode), From 589f6cdb44642f4b12aef9deffc8af11fa53b026 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 11:59:39 +0100 Subject: [PATCH 159/222] navigator: in start_loiter set _pos_sp_triplet.current.type to SETPOINT_TYPE_LOITER instead of SETPOINT_TYPE_NORMAL --- 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 e961a8f942..dace00ec5b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1078,7 +1078,7 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; From c2911cbecf06b18d581fd3e5407a1f525bd63d4b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 12:32:03 +0100 Subject: [PATCH 160/222] add malolo (flightgear HIL flying wing) autostart config --- .../init.d/1005_rc_fw_Malolo1.hil | 61 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++ 2 files changed, 66 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil new file mode 100644 index 0000000000..cc13110e40 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -0,0 +1,61 @@ +#!nsh +# +# HIL Malolo 1 (Flightgear) +# +# Maintainers: 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 SYS_AUTOCONFIG 0 + param save +fi + +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/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 030806fd7e..7fa2afbc8b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -46,6 +46,11 @@ then sh /etc/init.d/1004_rc_fw_Rascal110.hil fi +if param compare SYS_AUTOSTART 1005 +then + sh /etc/init.d/1005_rc_fw_Malolo1.hil +fi + # # Standard plane # From c1d1d6753410445ff4f16a988325cca1b7561a4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 13:21:51 +0100 Subject: [PATCH 161/222] Improved RC calibration behaviour, fully supported setting trim offsets --- src/modules/commander/commander.cpp | 49 ++++++++++++++++++---- src/modules/commander/commander_helper.cpp | 7 +++- src/modules/commander/commander_helper.h | 3 ++ src/modules/commander/rc_calibration.cpp | 11 +++-- src/modules/commander/rc_calibration.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 1 + 6 files changed, 56 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2a2bcca727..9211a512ef 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -214,7 +214,7 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); -void print_reject_mode(const char *msg); +void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); @@ -620,6 +620,8 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value + // We want to accept RC inputs as default + status.rc_input_blocked = false; /* armed topic */ orb_advert_t armed_pub; @@ -1076,7 +1078,7 @@ int commander_thread_main(int argc, char *argv[]) } /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0 && !status.rc_input_blocked) { /* start RC input check */ if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1470,7 +1472,7 @@ check_main_state_machine(struct vehicle_status_s *current_status) break; // changed successfully or already in this state // else fallback to SEATBELT - print_reject_mode("EASY"); + print_reject_mode(current_status, "EASY"); } res = main_state_transition(current_status, MAIN_STATE_SEATBELT); @@ -1479,7 +1481,7 @@ check_main_state_machine(struct vehicle_status_s *current_status) break; // changed successfully or already in this mode if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + print_reject_mode(current_status, "SEATBELT"); // else fallback to MANUAL res = main_state_transition(current_status, MAIN_STATE_MANUAL); @@ -1493,7 +1495,7 @@ check_main_state_machine(struct vehicle_status_s *current_status) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); + print_reject_mode(current_status, "AUTO"); res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) @@ -1512,16 +1514,25 @@ check_main_state_machine(struct vehicle_status_s *current_status) } void -print_reject_mode(const char *msg) +print_reject_mode(struct vehicle_status_s *current_status, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "#audio: warning: reject %s", msg); + sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + + // only buzz if armed, because else we're driving people nuts indoors + // they really need to look at the leds as well. + if (current_status->arming_state == ARMING_STATE_ARMED) { + tune_negative(); + } else { + + // Always show the led indication + led_negative(); + } } } @@ -1795,7 +1806,15 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_rc_calibration(mavlink_fd); + /* disable RC control input completely */ + status.rc_input_blocked = true; + calib_ret = OK; + mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1806,6 +1825,18 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (status.rc_input_blocked) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* enable RC control input */ + status.rc_input_blocked = false; + mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + } + + /* this always succeeds */ + calib_ret = OK; + } if (calib_ret == OK) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 21a1c4c2c7..033e7dc88e 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -123,11 +123,16 @@ void tune_neutral() } void tune_negative() +{ + led_negative(); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); +} + +void led_negative() { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d0393f45a9..af25a5e979 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,9 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); + +void led_negative(); + int blink_msg_state(); int led_init(void); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 90ede499a1..41f3ca0aa1 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -53,17 +53,16 @@ #endif static const int ERROR = -1; -int do_rc_calibration(int mavlink_fd) +int do_trim_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "trim calibration starting"); - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + usleep(200000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); if (!changed) { - mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + mavlink_log_critical(mavlink_fd, "no inputs, aborting"); return ERROR; } @@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd) int save_ret = param_save_default(); if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); close(sub_man); return ERROR; } - mavlink_log_info(mavlink_fd, "trim calibration done"); + mavlink_log_info(mavlink_fd, "trim cal done"); close(sub_man); return OK; } diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 9aa6faafa8..45efedf55c 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include -int do_rc_calibration(int mavlink_fd); +int do_trim_calibration(int mavlink_fd); #endif /* RC_CALIBRATION_H_ */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7a..6ea48a6800 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,7 @@ struct vehicle_status_s bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + bool rc_input_blocked; /**< set if RC input should be ignored */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; From 48f777d071652e382f930a4554ed809a60b7b0c5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 13:54:39 +0100 Subject: [PATCH 162/222] commander sets vstatus.condition_landed = false for fw systems (until we have a landing detector): solves multiple issues with the state machine in the navigator app --- src/modules/commander/commander.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 9 +-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e9da69232d..aca3f62565 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -926,10 +926,12 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + static bool published_condition_landed_fw = false; if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -938,6 +940,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { + if (!published_condition_landed_fw) { + status.condition_landed = false; // Fixedwing does not have a landing detector currently + published_condition_landed_fw = true; + status_changed = true; + } } /* update battery status */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 114776327e..3ef2009c71 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1443,14 +1443,7 @@ Navigator::check_mission_item_reached() } if (_mission_item.nav_cmd == NAV_CMD_LAND) { - if (_vstatus.is_rotary_wing) { - return _vstatus.condition_landed; - - } else { - /* For fw there is currently no landing detector: - * make sure control is not stopped when overshooting the landing waypoint */ - return false; - } + return _vstatus.condition_landed; } /* XXX TODO count turns */ From 723259d46dd7d205e48a9cd3ece4ab1628b7a3e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 14:36:34 +0100 Subject: [PATCH 163/222] update malolo parameters related to RTL --- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 3 +++ 1 file changed, 3 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 cc13110e40..36d32f3bea 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -49,6 +49,9 @@ then 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 From 44cd82e2fef20a3fc5aa61711b4cc06012a1e21d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 14:40:23 +0100 Subject: [PATCH 164/222] Set default autoland wait time to -1 (infinite wait) --- 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 af1d9d7d58..d5e00e35db 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -60,4 +60,4 @@ 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 PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing +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 From 71d0d1c4047c93be6cf9893d6fafc5e6d8822d02 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:01:11 +0100 Subject: [PATCH 165/222] Make MTD startup less verbose, there are diagnostic commands to read off its state --- src/systemcmds/mtd/mtd.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a2a0c109c6..0a88d40f39 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -257,7 +257,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) /* Now create MTD FLASH partitions */ - warnx("Creating partitions"); FAR struct mtd_dev_s *part[n_partitions]; char blockname[32]; @@ -266,9 +265,6 @@ mtd_start(char *partition_names[], unsigned n_partitions) for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) { - warnx(" Partition %d. Block offset=%lu, size=%lu", - i, (unsigned long)offset, (unsigned long)nblocks); - /* Create the partition */ part[i] = mtd_partition(mtd_dev, offset, nblocks); From 0982b081b8d67f6dea8240a6c2a4226bcce57932 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:46:04 +0100 Subject: [PATCH 166/222] ROMFS cleanup to eleminate excessive comments and resulting flash usage --- .../init.d/1000_rc_fw_easystar.hil | 2 - ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- .../px4fmu_common/init.d/1002_rc_fw_state.hil | 45 ------------------- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- .../init.d/1004_rc_fw_Rascal110.hil | 2 +- .../init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 5 --- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 7 --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 4 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 2 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 2 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 42 +---------------- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 2 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 2 +- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 11 ----- ROMFS/px4fmu_common/init.d/rc.io | 1 - ROMFS/px4fmu_common/init.d/rc.mc_apps | 17 +------ ROMFS/px4fmu_common/init.d/rc.sensors | 4 -- ROMFS/px4fmu_common/init.d/rcS | 3 +- 30 files changed, 24 insertions(+), 158 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil 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 ebe8a1a1ea..c8030d3965 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -26,8 +26,6 @@ then 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 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 2ce0334b4c..1c7ecb7129 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -2,7 +2,7 @@ # # 3DR Iris Quadcopter # -# Maintainers: Anton Babushkin +# Anton Babushkin # if [ $DO_AUTOCONFIG == yes ] 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 84e2bc5d47..45880f44be 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter X # -# Maintainers: Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil deleted file mode 100644 index 46da24d35f..0000000000 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ /dev/null @@ -1,45 +0,0 @@ -#!nsh -# -# HIL Rascal 110 (Flightgear) -# -# Maintainers: Thomas Gubler -# - -echo "HIL Rascal 110 starting.." - -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 - -set HIL yes - -set VEHICLE_TYPE fw -set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index e958448912..2b26810e71 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter + # -# Maintainers: Anton Babushkin +# Anton Babushkin # sh /etc/init.d/1001_rc_quad_x.hil 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 46da24d35f..fd7a6a3dab 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -2,7 +2,7 @@ # # HIL Rascal 110 (Flightgear) # -# Maintainers: Thomas Gubler +# Thomas Gubler # echo "HIL Rascal 110 starting.." 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 36d32f3bea..124bf63abf 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -2,7 +2,7 @@ # # HIL Malolo 1 (Flightgear) # -# Maintainers: Thomas Gubler +# Thomas Gubler # echo "HIL Malolo 1 starting.." diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 77813268a6..8a813595e4 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10” Octo coaxial geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Lorenz Meier , Anton Babushkin # sh /etc/init.d/8001_octo_x diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 1ed923b19c..de5e5a8d3a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,10 +1,5 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" - -# -# Load default params for this platform -# if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index 1ed923b19c..07f215f6cf 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -1,10 +1,5 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" - -# -# Load default params for this platform -# if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -23,8 +18,6 @@ then 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 diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index cbcc6189b6..3b7323ac4e 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -1,7 +1,5 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" - if [ $DO_AUTOCONFIG == yes ] then # @@ -26,7 +24,7 @@ then 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_MAX 1.0 param set FW_THR_MIN 0 param set FW_T_SINK_MAX 5 param set FW_T_SINK_MIN 2 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 4ebbe9c61e..a4ff61d937 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,7 +2,7 @@ # # Phantom FPV Flying Wing # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 143310af95..c1e78b6f1e 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -2,7 +2,7 @@ # # Skywalker X5 Flying Wing # -# Maintainers: Thomas Gubler , Julian Oes +# Thomas Gubler , Julian Oes # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index e537632784..2f68797999 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -2,7 +2,7 @@ # # Wing Wing (aka Z-84) Flying Wing # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index 8d179d1fd4..bc02d87f3d 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -2,7 +2,7 @@ # # FX-79 Buffalo Flying Wing # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index ca4694d81c..42b64a68ea 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -2,7 +2,7 @@ # # Generic 10” Quad X geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Lorenz Meier # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index c78da2d6c5..7829460339 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,7 +2,7 @@ # # DJI Flame Wheel F330 Quadcopter # -# Maintainers: Anton Babushkin +# Anton Babushkin # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 8027b9d42a..8eb53d1720 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,7 +2,7 @@ # # DJI Flame Wheel F450 Quadcopter # -# Maintainers: Lorenz Meier +# Lorenz Meier # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 98e1f80aa4..3d8e9fcf70 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -2,48 +2,10 @@ # # HobbyKing X550 Quadcopter # -# Maintainers: Todd Stellanova +# Todd Stellanova # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 5.5 - param set MC_ROLLRATE_P 0.14 - param set MC_ROLLRATE_I 0 - param set MC_ROLLRATE_D 0.006 - param set MC_PITCH_P 5.5 - param set MC_PITCHRATE_P 0.14 - param set MC_PITCHRATE_I 0 - param set MC_PITCHRATE_D 0.006 - param set MC_YAW_P 0.6 - param set MC_YAWRATE_P 0.08 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_D 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 - -set VEHICLE_TYPE mc -set MIXER FMU_quad_x +sh /etc/init.d/4001_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 7f5a6fc078..ff11bccfe6 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -2,7 +2,7 @@ # # Generic 10” Quad + geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index e72e15dd4d..270f51a582 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -2,7 +2,7 @@ # # Generic 10” Hexa X geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index dade0630da..45279ec390 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -2,7 +2,7 @@ # # Generic 10” Hexa + geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/6001_hexa_x diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index af632ed908..6fa9626684 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -2,7 +2,7 @@ # # Generic 10” Octo X geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 9bf5e09328..fa3869f9f9 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -2,7 +2,7 @@ # # Generic 10” Octo + geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/8001_octo_x diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 7fa2afbc8b..8d2fc5772d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -31,11 +31,6 @@ then sh /etc/init.d/1001_rc_quad_x.hil fi -if param compare SYS_AUTOSTART 1002 -then - sh /etc/init.d/1002_rc_fw_state.hil -fi - if param compare SYS_AUTOSTART 1003 then sh /etc/init.d/1003_rc_quad_+.hil diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index d354fb06fa..72327c554e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -3,17 +3,6 @@ # Standard apps for fixed wing # -# -# Start the attitude and position estimator -# att_pos_estimator_ekf start - -# -# Start attitude controller -# fw_att_control start - -# -# Start the position controller -# fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index c9d964f8e2..6e8fdbc0f7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -4,7 +4,6 @@ # # Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. # px4io recovery diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 96fe32c8a7..ed3939757a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -1,24 +1,11 @@ #!nsh # -# Standard apps for multirotors +# Standard apps for multirotors: +# att & pos estimator, att & pos control. # -# -# Start the attitude estimator -# attitude_estimator_ekf start - -# -# Start position estimator -# position_estimator_inav start -# -# Start attitude control -# mc_att_control start - -# -# Start position control -# mc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index badbf92c3c..235be24316 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -3,10 +3,6 @@ # Standard startup script for PX4FMU onboard sensor drivers. # -# -# Start sensor drivers here. -# - ms5611 start adc start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 88077c67f0..84dc7ec640 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -3,8 +3,7 @@ # PX4FMU startup script. # -# Default to auto-start mode. An init script on the microSD card -# can change this to prevent automatic startup of the flight script. +# Default to auto-start mode. # set MODE autostart From 2908ffb09293a9d65708a8e80de50bbdee69b9e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:46:51 +0100 Subject: [PATCH 167/222] Trim down FMUv1 config, remove not commonly used modules from default set --- makefiles/config_px4fmu-v1_default.mk | 4 ---- 1 file changed, 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 51be7e1a12..aff614cbb3 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -21,7 +21,6 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 -#MODULES += drivers/bma180 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param @@ -65,14 +63,12 @@ MODULES += systemcmds/hw_ver MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink -MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav #MODULES += examples/flow_position_estimator From b1539d8a74a1874abcb5e64f1813bd6ba9f0353f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:56:57 +0100 Subject: [PATCH 168/222] Removed further unneeded commands and other things eating space --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 5 +---- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 3 --- 3 files changed, 2 insertions(+), 8 deletions(-) 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 c8030d3965..d7e524b414 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -2,7 +2,7 @@ # # HILStar / X-Plane # -# Maintainers: Thomas Gubler +# Lorenz Meier # echo "HIL Rascal 110 starting.." @@ -32,9 +32,6 @@ then 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 set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 3a8f14a9ed..a3d7c3d977 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 # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] 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 fd7a6a3dab..c639bfc254 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -34,9 +34,6 @@ then 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 set HIL yes From 091fe61bf22cdc38462b2b3aacf253d60125db6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 16:04:51 +0100 Subject: [PATCH 169/222] Comment in HIL start script --- ROMFS/px4fmu_common/init.d/rc.autostart | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 34da2dfef2..00baed6468 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -23,7 +23,7 @@ if param compare SYS_AUTOSTART 1000 then - #sh /etc/init.d/1000_rc_fw_easystar.hil + sh /etc/init.d/1000_rc_fw_easystar.hil fi if param compare SYS_AUTOSTART 1001 From aa5d8a8732b1bed690360c25d725f2c13458528a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 16:06:09 +0100 Subject: [PATCH 170/222] remove printf in launchdetector --- src/lib/launchdetection/LaunchDetector.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 67b32ad82c..df9f2fe95c 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -83,14 +83,10 @@ bool LaunchDetector::getLaunchDetected() void LaunchDetector::updateParams() { - warnx(" LaunchDetector::updateParams()"); launchdetection_on.update(); throttlePreTakeoff.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { launchMethods[i]->updateParams(); - warnx("updating component %d", i); } - - warnx(" LaunchDetector::updateParams() ended"); } From 14bbecfd7a0c7a1e07ffd776aa2aec9ea1af2ce0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 18:59:26 +0100 Subject: [PATCH 171/222] Hotfix: Check all channel mappings for valid ranges --- src/modules/commander/commander.cpp | 2 +- src/modules/sensors/sensors.cpp | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2a2bcca727..1ed0a06575 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1403,7 +1403,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { - warnx("mode sw not finite"); + /* default to manual if signal is invalid */ current_status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index df6cbb7b2f..f98c79cd2d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1406,16 +1406,24 @@ Sensors::rc_poll() } /* mode switch input */ - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + } /* land switch input */ - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } /* assisted switch input */ - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } /* mission switch input */ - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } /* flaps */ if (_rc.function[FLAPS] >= 0) { From e1356f69b4a0c6c5cc8eaacb83f7c2791406d747 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 19:01:05 +0100 Subject: [PATCH 172/222] Hotfix: Check all channel mappings for valid ranges --- src/modules/sensors/sensors.cpp | 30 ++++++++++-------------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f98c79cd2d..b50a694eba 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1405,26 +1405,6 @@ Sensors::rc_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* land switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - - /* assisted switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1443,6 +1423,16 @@ Sensors::rc_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* land switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } + + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } From 243a28ff8b7643edeea57640ce2ce4db5a40cead Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 23:13:41 +0100 Subject: [PATCH 173/222] Do not send an error message for RX pairing commands --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ed0a06575..882e394519 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1864,6 +1864,10 @@ void *commander_low_prio_loop(void *arg) break; } + case VEHICLE_CMD_START_RX_PAIR: + /* handled in the IO driver */ + break; + default: answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; From efc62a6c864766ce211906860d6325e4ca089241 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 2 Feb 2014 00:43:41 +0100 Subject: [PATCH 174/222] fw landing: improve slope altitude calc to avoid climbout after waypoint. Throttle cut is now defined via altitude --- .../fw_pos_control_l1_main.cpp | 25 ++++++++++--------- .../fw_pos_control_l1_params.c | 2 +- .../fw_pos_control_l1/landingslope.cpp | 22 ++++++++++++---- src/modules/fw_pos_control_l1/landingslope.h | 21 ++++++++++++---- 4 files changed, 47 insertions(+), 23 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 3ef1871a8b..45fdaa355e 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 @@ -236,7 +236,7 @@ private: float land_slope_length; float land_H1_virt; float land_flare_alt_relative; - float land_thrust_lim_horizontal_distance; + float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; } _parameters; /**< local copies of interesting parameters */ @@ -281,7 +281,7 @@ private: param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; - param_t land_thrust_lim_horizontal_distance; + param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; } _parameter_handles; /**< handles for interesting parameters */ @@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _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_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update() 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_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + target_bearing = bearing_lastwp_currwp; } else { target_bearing = _att.yaw; } @@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - - + 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); if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out @@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { + if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); + 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) 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 7954d75c20..ee8721ff92 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 @@ -172,5 +172,5 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.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_TLDIST, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index b139a63978..e5f7023ae1 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -48,13 +48,13 @@ void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt) { _landing_slope_angle_rad = landing_slope_angle_rad; _flare_relative_alt = flare_relative_alt; - _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _motor_lim_relative_alt = motor_lim_relative_alt; _H1_virt = H1_virt; calculateSlopeValues(); @@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) { - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; - + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + else + return wp_altitude; +} + +float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + else + return wp_landing_altitude; } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 1a149fc7cd..76d65a55f2 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -49,7 +49,7 @@ private: /* see Documentation/fw_landing.png for a plot of the landing slope */ float _landing_slope_angle_rad; /**< phi in the plot */ float _flare_relative_alt; /**< h_flare,rel in the plot */ - float _motor_lim_horizontal_distance; + float _motor_lim_relative_alt; float _H1_virt; /**< H1 in the plot */ float _H0; /**< h_flare,rel + H1 in the plot */ float _d1; /**< d1 in the plot */ @@ -63,7 +63,18 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); /** * @@ -85,17 +96,17 @@ public: } - float getFlareCurveAltitude(float wp_distance, float wp_altitude); + float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} inline float flare_relative_alt() {return _flare_relative_alt;} - inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;} inline float H1_virt() {return _H1_virt;} inline float H0() {return _H0;} inline float d1() {return _d1;} From b92b08ae62b2da027e9fcd15a3a45a9125ea8355 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 2 Feb 2014 00:42:15 -0500 Subject: [PATCH 175/222] Optimize and update data manager docs Move repeated code to common function Update missing and misleading comments Fix data manager test compile errors No functional changes --- src/modules/dataman/dataman.c | 178 +++++++++++++++++----------- src/systemcmds/tests/test_dataman.c | 8 +- 2 files changed, 112 insertions(+), 74 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dc2d6c312a..fa88dfaffb 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman"; /* The data manager work queues */ typedef struct { - sq_queue_t q; - sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; - unsigned max_size; + sq_queue_t q; /* Nuttx queue */ + sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; /* Current size of queue */ + unsigned max_size; /* Maximum queue size reached */ } work_q_t; -static work_q_t g_free_q; -static work_q_t g_work_q; +static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ +static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ -sem_t g_work_queued_sema; +sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -#define DM_SECTOR_HDR_SIZE 4 -static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; +#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ +static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); - sem_init(&(q->mutex), 1, 1); - q->size = q->max_size = 0; + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ + q->size = q->max_size = 0; /* Queue is initially empty */ } -static void destroy_q(work_q_t *q) +static inline void +destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void lock_queue(work_q_t *q) { - sem_wait(&(q->mutex)); + sem_wait(&(q->mutex)); /* Acquire the queue lock */ } static inline void unlock_queue(work_q_t *q) { - sem_post(&(q->mutex)); + sem_post(&(q->mutex)); /* Release the queue lock */ } static work_q_item_t * @@ -172,54 +173,47 @@ create_work_item(void) { work_q_item_t *item; + /* Try to reuse item from free item queue */ lock_queue(&g_free_q); if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) g_free_q.size--; unlock_queue(&g_free_q); + /* If we there weren't any free items then obtain memory for a new one */ if (item == NULL) item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + /* If we got one then lock the item*/ if (item) sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + /* return the item pointer, or NULL if all failed */ return item; } /* Work queue management functions */ -static void -enqueue_work_item(work_q_item_t *item) -{ - /* put the work item on the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - if (++g_work_q.size > g_work_q.max_size) - g_work_q.max_size = g_work_q.size; - - unlock_queue(&g_work_q); - - /* tell the work thread that work is available */ - sem_post(&g_work_queued_sema); -} - -static void +static inline void destroy_work_item(work_q_item_t *item) { - sem_destroy(&item->wait_sem); + sem_destroy(&item->wait_sem); /* Destroy the item lock */ + /* Return the item to the free item queue for later reuse */ lock_queue(&g_free_q); sq_addfirst(&item->link, &(g_free_q.q)); + /* Update the queue size and potentially the maximum queue size */ if (++g_free_q.size > g_free_q.max_size) g_free_q.max_size = g_free_q.size; unlock_queue(&g_free_q); } -static work_q_item_t * +static inline work_q_item_t * dequeue_work_item(void) { work_q_item_t *work; + + /* retrieve the 1st item on the work queue */ lock_queue(&g_work_q); if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) @@ -229,6 +223,32 @@ dequeue_work_item(void) return work; } +static int +enqueue_work_item_and_wait_for_result(work_q_item_t *item) +{ + /* put the work item at the end of the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + /* Adjust the queue size and potentially the maximum queue size */ + if (++g_work_q.size > g_work_q.max_size) + g_work_q.max_size = g_work_q.size; + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + sem_post(&g_work_queued_sema); + + /* wait for the result */ + sem_wait(&item->wait_sem); + + int result = item->result; + + destroy_work_item(item); + + return result; +} + /* Calculate the offset in file of specific item */ static int calculate_offset(dm_item_t item, unsigned char index) @@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index) * * byte 0: Length of user data item * byte 1: Persistence of this data item + * byte 2: Unused (for future use) + * byte 3: Unused (for future use) * byte DM_SECTOR_HDR_SIZE... : data item value * * The total size must not exceed k_sector_size @@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v len = -1; + /* Seek to the right spot in the data manager file and write the data item */ if (lseek(g_task_fd, offset, SEEK_SET) == offset) if ((len = write(g_task_fd, buffer, count)) == count) - fsync(g_task_fd); + fsync(g_task_fd); /* Make sure data is written to physical media */ + /* Make sure the write succeeded */ if (len != count) return -1; @@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) if (lseek(g_task_fd, offset, SEEK_SET) == offset) len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); - /* Check for length issues */ + /* Check for read error */ if (len < 0) return -1; + /* A zero length entry is a empty entry */ if (len == 0) buffer[0] = 0; + /* See if we got data */ if (buffer[0] > 0) { + /* We got more than requested!!! */ if (buffer[0] > count) return -1; @@ -340,11 +369,14 @@ _clear(dm_item_t item) { int i, result = 0; + /* Get the offset of 1st item of this type */ int offset = calculate_offset(item, 0); + /* Check for item type out of range */ if (offset < 0) return -1; + /* Clear all items of this type */ for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { char buf[1]; @@ -353,9 +385,11 @@ _clear(dm_item_t item) break; } + /* Avoid SD flash wear by only doing writes where necessary */ if (read(g_task_fd, buf, 1) < 1) break; + /* If item has length greater than 0 it needs to be overwritten */ if (buf[0]) { if (lseek(g_task_fd, offset, SEEK_SET) != offset) { result = -1; @@ -373,6 +407,7 @@ _clear(dm_item_t item) offset += k_sector_size; } + /* Make sure data is actually written to physical media */ fsync(g_task_fd); return result; } @@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a write request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_write_func; work->write_params.item = item; @@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const work->write_params.persistence = persistence; work->write_params.buf = buf; work->write_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } /* Retrieve from the data manager file */ @@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a read request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_read_func; work->read_params.item = item; work->read_params.index = index; work->read_params.buf = buf; work->read_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } __EXPORT int @@ -504,21 +535,19 @@ dm_clear(dm_item_t item) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a clear request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_clear_func; work->clear_params.item = item; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } /* Tell the data manager about the type of the last reset */ @@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a restart request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_restart_func; work->restart_params.reason = reason; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } static int @@ -570,24 +597,29 @@ task_main(int argc, char *argv[]) sem_init(&g_work_queued_sema, 1, 0); + /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } fsync(g_task_fd); + /* We use two file descriptors, one for the caller context and one for the worker thread */ + /* They are actually the same but we need to some way to reject caller request while the */ + /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + /* Tell startup that the worker thread has completed its initialization */ sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ @@ -595,7 +627,7 @@ task_main(int argc, char *argv[]) /* do we need to exit ??? */ if ((g_task_should_exit) && (g_fd >= 0)) { - /* Close the file handle to stop further queueing */ + /* Close the file handle to stop further queuing */ g_fd = -1; } @@ -607,6 +639,7 @@ task_main(int argc, char *argv[]) /* Empty the work queue */ while ((work = dequeue_work_item())) { + /* handle each work item with the appropriate handler */ switch (work->func) { case dm_write_func: g_func_counts[dm_write_func]++; @@ -647,7 +680,7 @@ task_main(int argc, char *argv[]) close(g_task_fd); g_task_fd = -1; - /* Empty the work queue */ + /* The work queue is now empty, empty the free queue */ for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) break; @@ -669,7 +702,7 @@ start(void) sem_init(&g_init_sema, 1, 0); - /* start the task */ + /* start the worker thread */ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { warn("task start failed"); return -1; @@ -704,7 +737,7 @@ stop(void) static void usage(void) { - errx(1, "usage: dataman {start|stop|status}"); + errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); } int @@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[]) exit(0); } + /* Worker thread should be running for all other commands */ if (g_fd < 0) errx(1, "not running"); @@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[]) stop(); else if (!strcmp(argv[1], "status")) status(); + else if (!strcmp(argv[1], "poweronrestart")) + dm_restart(DM_INIT_REASON_POWER_ON); + else if (!strcmp(argv[1], "inflightrestart")) + dm_restart(DM_INIT_REASON_IN_FLIGHT); else usage(); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 5b121e34ed..1f844a97de 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } From 9defc6cb235e13ef912a70466acf1d14314f1e3d Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 14:26:17 +0100 Subject: [PATCH 176/222] 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 150bac4b51492b556b4e5e60236b3c239a1921db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 15:17:49 +0100 Subject: [PATCH 177/222] Sensors messaging cleanup --- src/modules/sensors/sensors.cpp | 39 +++++++++++++++++---------------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bd665b592b..b176d7417a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -636,41 +635,43 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + const char *paramerr = "FAIL PARM LOAD"; + /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx("Failed getting roll chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx("Failed getting pitch chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx("Failed getting yaw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx("Failed getting throttle chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx("Failed getting return sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx("Failed getting assisted sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx("Failed getting mission sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); + warnx(paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -742,12 +743,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx("Failed updating voltage scaling param"); + warnx(paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx("Failed updating current scaling param"); + warnx(paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -1650,17 +1651,17 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(0, "sensors task already running"); + errx(0, "already running"); sensors::g_sensors = new Sensors; if (sensors::g_sensors == nullptr) - errx(1, "sensors task alloc failed"); + errx(1, "alloc failed"); if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; sensors::g_sensors = nullptr; - err(1, "sensors task start failed"); + err(1, "start failed"); } exit(0); @@ -1668,7 +1669,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) - errx(1, "sensors task not running"); + errx(1, "not running"); delete sensors::g_sensors; sensors::g_sensors = nullptr; @@ -1677,10 +1678,10 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - errx(0, "task is running"); + errx(0, "is running"); } else { - errx(1, "task is not running"); + errx(1, "not running"); } } From 0089db7ef3961c36d6513877b7681ab548d20ccf Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 16:28:56 +0100 Subject: [PATCH 178/222] 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 179/222] 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 3d21a73ddf18b89552aa9bd65965ff6b311487b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 22:04:11 +0100 Subject: [PATCH 180/222] navigator: fixed infinite RTL->LOITER->RTL... loop on failsafe --- src/modules/navigator/navigator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3ef2009c71..e0d5df5f78 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -688,7 +688,8 @@ Navigator::task_main() /* RETURN switch, overrides MISSION switch */ 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)) && + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -746,7 +747,8 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } From 8de35025b5b28a0f40b5c17ca9baa5acefdc20ca Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 22:06:40 +0100 Subject: [PATCH 181/222] navigator: avoid climbing up for LOITER after RTL --- src/modules/navigator/navigator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0d5df5f78..5139ae6cd2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached() dispatch(EVENT_LAND_REQUESTED); } else { + _reset_loiter_pos = false; dispatch(EVENT_LOITER_REQUESTED); } From a7218770b3cb6c45927d5c791aa863ffccba6b89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Feb 2014 15:33:56 +0100 Subject: [PATCH 182/222] Updated MAVLink version, no functional changes --- .../v1.0/ardupilotmega/ardupilotmega.h | 7 +- .../v1.0/ardupilotmega/mavlink_msg_ahrs2.h | 287 ++++++++++++ .../mavlink/v1.0/ardupilotmega/testsuite.h | 54 +++ .../mavlink/v1.0/ardupilotmega/version.h | 4 +- .../include/mavlink/v1.0/autoquad/autoquad.h | 6 +- .../include/mavlink/v1.0/autoquad/version.h | 4 +- mavlink/include/mavlink/v1.0/common/common.h | 10 +- .../mavlink_msg_data_transmission_handshake.h | 68 +-- .../mavlink_msg_encapsulated_data.h | 6 +- .../v1.0/common/mavlink_msg_gps2_raw.h | 419 ++++++++++++++++++ .../v1.0/common/mavlink_msg_gps_inject_data.h | 237 ++++++++++ .../include/mavlink/v1.0/common/testsuite.h | 218 +++++++++ mavlink/include/mavlink/v1.0/common/version.h | 4 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 6 +- .../mavlink/v1.0/matrixpilot/version.h | 4 +- mavlink/include/mavlink/v1.0/mavlink_types.h | 2 +- .../include/mavlink/v1.0/pixhawk/pixhawk.h | 8 +- .../include/mavlink/v1.0/pixhawk/testsuite.h | 102 ----- .../include/mavlink/v1.0/pixhawk/version.h | 2 +- .../mavlink/v1.0/sensesoar/sensesoar.h | 6 +- .../mavlink/v1.0/sensesoar/testsuite.h | 26 +- .../include/mavlink/v1.0/sensesoar/version.h | 4 +- 22 files changed, 1300 insertions(+), 184 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h rename mavlink/include/mavlink/v1.0/{pixhawk => common}/mavlink_msg_data_transmission_handshake.h (89%) rename mavlink/include/mavlink/v1.0/{pixhawk => common}/mavlink_msg_encapsulated_data.h (98%) create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 4dc9aed260..2acb7965ca 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -197,6 +197,7 @@ enum RALLY_FLAGS #include "./mavlink_msg_airspeed_autocal.h" #include "./mavlink_msg_rally_point.h" #include "./mavlink_msg_rally_fetch_point.h" +#include "./mavlink_msg_ahrs2.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h new file mode 100644 index 0000000000..f6fde9590c --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h @@ -0,0 +1,287 @@ +// MESSAGE AHRS2 PACKING + +#define MAVLINK_MSG_ID_AHRS2 178 + +typedef struct __mavlink_ahrs2_t +{ + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float altitude; ///< Altitude (MSL) + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 +} mavlink_ahrs2_t; + +#define MAVLINK_MSG_ID_AHRS2_LEN 24 +#define MAVLINK_MSG_ID_178_LEN 24 + +#define MAVLINK_MSG_ID_AHRS2_CRC 47 +#define MAVLINK_MSG_ID_178_CRC 47 + + + +#define MAVLINK_MESSAGE_INFO_AHRS2 { \ + "AHRS2", \ + 6, \ + { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \ + { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \ + } \ +} + + +/** + * @brief Pack a ahrs2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} + +/** + * @brief Pack a ahrs2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} + +/** + * @brief Encode a ahrs2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) +{ + return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); +} + +/** + * @brief Encode a ahrs2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) +{ + return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); +} + +/** + * @brief Send a ahrs2 message + * @param chan MAVLink channel to send the message + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#endif +} + +#endif + +// MESSAGE AHRS2 UNPACKING + + +/** + * @brief Get field roll from ahrs2 message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field pitch from ahrs2 message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from ahrs2 message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field altitude from ahrs2 message + * + * @return Altitude (MSL) + */ +static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field lat from ahrs2 message + * + * @return Latitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field lng from ahrs2 message + * + * @return Longitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Decode a ahrs2 message into a struct + * + * @param msg The message to decode + * @param ahrs2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg); + ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg); + ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg); + ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg); + ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg); + ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg); +#else + memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index eb3bc6a759..b2496334f7 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_ahrs2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ahrs2_t packet_in = { + 17.0, + }45.0, + }73.0, + }101.0, + }963498296, + }963498504, + }; + mavlink_ahrs2_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.altitude = packet_in.altitude; + packet1.lat = packet_in.lat; + packet1.lng = packet_in.lng; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ahrs2_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ahrs2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ahrs2_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng ); + mavlink_msg_ahrs2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ahrs2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng ); + mavlink_msg_ahrs2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; isize = mavlink_msg_data_transmission_handshake_get_size(msg); data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg); data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg); - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); + data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); #else diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h similarity index 98% rename from mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h rename to mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h index 40001dc309..5900ea8388 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h @@ -1,6 +1,6 @@ // MESSAGE ENCAPSULATED_DATA PACKING -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131 typedef struct __mavlink_encapsulated_data_t { @@ -9,10 +9,10 @@ typedef struct __mavlink_encapsulated_data_t } mavlink_encapsulated_data_t; #define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 -#define MAVLINK_MSG_ID_194_LEN 255 +#define MAVLINK_MSG_ID_131_LEN 255 #define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223 -#define MAVLINK_MSG_ID_194_CRC 223 +#define MAVLINK_MSG_ID_131_CRC 223 #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h new file mode 100644 index 0000000000..17e5bd0026 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h @@ -0,0 +1,419 @@ +// MESSAGE GPS2_RAW PACKING + +#define MAVLINK_MSG_ID_GPS2_RAW 124 + +typedef struct __mavlink_gps2_raw_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 + int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 + int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) + uint32_t dgps_age; ///< Age of DGPS info + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 + uint8_t dgps_numch; ///< Number of DGPS satellites +} mavlink_gps2_raw_t; + +#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35 +#define MAVLINK_MSG_ID_124_LEN 35 + +#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 +#define MAVLINK_MSG_ID_124_CRC 87 + + + +#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ + "GPS2_RAW", \ + 12, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \ + { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \ + { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \ + { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ + { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ + { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ + } \ +} + + +/** + * @brief Pack a gps2_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + +/** + * @brief Pack a gps2_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + +/** + * @brief Encode a gps2_raw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); +} + +/** + * @brief Encode a gps2_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); +} + +/** + * @brief Send a gps2_raw message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} + +#endif + +// MESSAGE GPS2_RAW UNPACKING + + +/** + * @brief Get field time_usec from gps2_raw message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from gps2_raw message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field lat from gps2_raw message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from gps2_raw message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from gps2_raw message + * + * @return Altitude (WGS84), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field eph from gps2_raw message + * + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field epv from gps2_raw message + * + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field vel from gps2_raw message + * + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field cog from gps2_raw message + * + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field satellites_visible from gps2_raw message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field dgps_numch from gps2_raw message + * + * @return Number of DGPS satellites + */ +static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field dgps_age from gps2_raw message + * + * @return Age of DGPS info + */ +static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Decode a gps2_raw message into a struct + * + * @param msg The message to decode + * @param gps2_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg); + gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg); + gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg); + gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg); + gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg); + gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg); + gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg); + gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg); + gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg); + gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg); + gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); + gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); +#else + memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h new file mode 100644 index 0000000000..485d8a4afc --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h @@ -0,0 +1,237 @@ +// MESSAGE GPS_INJECT_DATA PACKING + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123 + +typedef struct __mavlink_gps_inject_data_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t len; ///< data length + uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2) +} mavlink_gps_inject_data_t; + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113 +#define MAVLINK_MSG_ID_123_LEN 113 + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250 +#define MAVLINK_MSG_ID_123_CRC 250 + +#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110 + +#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \ + "GPS_INJECT_DATA", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \ + } \ +} + + +/** + * @brief Pack a gps_inject_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + +/** + * @brief Pack a gps_inject_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + +/** + * @brief Encode a gps_inject_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + +/** + * @brief Encode a gps_inject_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + +/** + * @brief Send a gps_inject_data message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} + +#endif + +// MESSAGE GPS_INJECT_DATA UNPACKING + + +/** + * @brief Get field target_system from gps_inject_data message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from gps_inject_data message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field len from gps_inject_data message + * + * @return data length + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field data from gps_inject_data message + * + * @return raw data (110 is enough for 12 satellites of RTCMv2) + */ +static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 110, 3); +} + +/** + * @brief Decode a gps_inject_data message into a struct + * + * @param msg The message to decode + * @param gps_inject_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg); + gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg); + gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg); + mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data); +#else + memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index 16250ab427..c5aa9ddf3a 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -4839,6 +4839,220 @@ static void mavlink_test_log_request_end(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_gps_inject_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_inject_data_t packet_in = { + 5, + }72, + }139, + }{ 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59 }, + }; + mavlink_gps_inject_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.len = packet_in.len; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*110); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_inject_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_inject_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_inject_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data ); + mavlink_msg_gps_inject_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data ); + mavlink_msg_gps_inject_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i Date: Tue, 4 Feb 2014 16:50:22 +0100 Subject: [PATCH 183/222] 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 d5caffa8450798f4bf65ff518395152136b75df4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 4 Feb 2014 16:52:47 +0100 Subject: [PATCH 184/222] startup: make sdlog2 working again on FMUv1 --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dcf5bbced8..c5aef8273c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 16 -t + sdlog2 start -r 50 -a -b 8 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t From b457e714972649f5a39fa3f2f9463bdfe6971fcc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 4 Feb 2014 16:53:36 +0100 Subject: [PATCH 185/222] position_estimator_inav: lower RAM --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..87f2c3330e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -133,7 +133,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 3000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From 4499919f76376f5f9904d672ad5fd85e465badac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Feb 2014 18:27:06 +0100 Subject: [PATCH 186/222] Fix for hexa mixer --- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 +++- ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 11 ----------- ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 11 ----------- 3 files changed, 3 insertions(+), 23 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 270f51a582..fd57565860 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -9,4 +9,6 @@ sh /etc/init.d/4001_quad_x set MIXER FMU_hexa_x -set PWM_OUTPUTS 123456 +# We only can run one channel group with one rate, +# so all 8 at 400 Hz +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index f8f9f0e4dc..b5e38ce9ef 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -5,14 +5,3 @@ This file defines a single mixer for a hexacopter in the + configuration. All co are mixed 100%. 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..8e8d122adc 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -5,14 +5,3 @@ This file defines a single mixer for a hexacopter in the X configuration. All c are mixed 100%. 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 From 94b162d0e076a872af9d1b1538d7f688d51bfef0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 09:34:21 +0100 Subject: [PATCH 187/222] 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 188/222] 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 d92903720d09c2d3ed273620ac8a817cff3ee42d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 10:22:10 +0100 Subject: [PATCH 189/222] Revert "position_estimator_inav: lower RAM" This reverts commit b457e714972649f5a39fa3f2f9463bdfe6971fcc. --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 87f2c3330e..e045ce4cca 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -133,7 +133,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 3000, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); 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 190/222] 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 15f021eec2042b81f286ac26a7dc9eb86cd89442 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 19:03:26 +0100 Subject: [PATCH 191/222] 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 84dc7ec640..178bcaeba5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -474,7 +474,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 0f0c5f74b3341f12f38ebe06f3a8ec402ba49f43 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 19:44:31 +0100 Subject: [PATCH 192/222] Startup: Raise min throttle --- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 42b64a68ea..0f1288dec3 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -48,5 +48,5 @@ set MIXER FMU_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 set PWM_DISARMED 900 -set PWM_MIN 1000 +set PWM_MIN 1100 set PWM_MAX 2000 From 3392086f8f317823061363296c8d3e18be4ee88e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 21:19:25 +0100 Subject: [PATCH 193/222] Mixer load: don't upload empty mixers from non-existing files --- src/modules/systemlib/mixer/mixer_load.c | 6 ++++-- src/systemcmds/mixer/mixer.cpp | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index a55ddf8a35..b05273c0de 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); if (fp == NULL) { - return 1; + warnx("file not found"); + return -1; } /* read valid lines from the file into a buffer */ @@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - return 1; + warnx("line too long"); + return -1; } /* add the line to the buffer */ diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 6da39d371b..2f5ed3265d 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,8 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0], sizeof(buf)); + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) + err(1, "can't load mixer: %s", fname); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); From 8ab94150db31e270b34cecaa4701628df873042c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 20:23:23 +0100 Subject: [PATCH 194/222] commander: print_reject_mode() bug fixed --- 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 246efd61e6..2c2b680b47 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1624,7 +1624,7 @@ set_main_state_rc(struct vehicle_status_s *status) if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { res = main_state_transition(status, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); + print_reject_mode(status, "OFFBOARD"); } else { return res; From 70e1bfa4d69e967b309d177060804e7567f148b2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 22:28:42 +0100 Subject: [PATCH 195/222] 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 196/222] 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 197/222] 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 198/222] 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 199/222] 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 200/222] 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 201/222] 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 202/222] 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 203/222] 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 204/222] 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 0dc7f223ea26a09cd538114f54d99a7ab130efba Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 10 Feb 2014 12:08:39 +1100 Subject: [PATCH 205/222] 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 b0c60296f58dfec02f115d0033f61e95389f5931 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 10 Feb 2014 12:08:39 +1100 Subject: [PATCH 206/222] 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 207/222] 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 208/222] 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 209/222] 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 210/222] 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 211/222] 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 212/222] 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 213/222] 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 214/222] 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 16908f9aff0e7ad0f967613adf2be9a00c1c6cce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 23:24:49 +0100 Subject: [PATCH 215/222] 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 179aa17a3842bb68fa3849e890d20cfb9a1a5392 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 12:21:23 +0100 Subject: [PATCH 216/222] 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 217/222] 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 218/222] 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 4982e819837195aa512fb977639ce1dd0c0cec3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 15:30:06 +0100 Subject: [PATCH 219/222] 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 220/222] 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 ccfe476326d8b01e33a3a7ea115054a31fa7a2b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 20:53:47 +0100 Subject: [PATCH 221/222] 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 66f270dd7e2a8668ae7a4f3387e5098381eff84d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 14 Feb 2014 13:39:57 +0100 Subject: [PATCH 222/222] commander: double subscribing to offboard setpoint fixed --- src/modules/commander/commander.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c2b680b47..bf7bf2ef32 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -761,11 +761,6 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); - /* Subscribe to offboard control data */ - int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); struct offboard_control_setpoint_s sp_offboard; @@ -883,10 +878,6 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); - } - orb_check(sp_offboard_sub, &updated); if (updated) { @@ -1285,7 +1276,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check offboard signal */ - if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (sp_offboard.timestamp != 0 && hrt_absolute_time() < sp_offboard.timestamp + OFFBOARD_TIMEOUT) { if (!status.offboard_control_signal_found_once) { status.offboard_control_signal_found_once = true; mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); @@ -1301,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; if (status.main_state == MAIN_STATE_OFFBOARD) { - if (offboard_sp.armed && !armed.armed) { + if (sp_offboard.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1310,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[]) } } - } else if (!offboard_sp.armed && armed.armed) { + } else if (!sp_offboard.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed);