fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 14:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta' into ready_rtl_fix

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-18 19:09:33 +04:00
parent 1d40582cc0 3508a42f6a
commit 1eecc73483
107 changed files with 2632 additions and 2205 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+23 -9
View File
@@ -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
@@ -76,8 +76,8 @@
#include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
I2C("Airspeed", path, bus, address, 100000),
_reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0),
@@ -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,13 +130,23 @@ 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?");
/* 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 */
src/drivers/airspeed/airspeed.h
+4 -2
View File
@@ -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
@@ -90,7 +90,7 @@ static const int ERROR = -1;
class __EXPORT Airspeed : public device::I2C
{
public:
Airspeed(int bus, int address, unsigned conversion_interval);
Airspeed(int bus, int address, unsigned conversion_interval, const char* path);
virtual ~Airspeed();
virtual int init();
@@ -127,6 +127,8 @@ protected:
orb_advert_t _airspeed_pub;
int _class_instance;
unsigned _conversion_interval;
perf_counter_t _sample_perf;
src/drivers/bma180/bma180.cpp
+18 -7
View File
@@ -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
@@ -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,8 @@ BMA180::measure()
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
if (_accel_topic > 0 && !(_pub_blocked))
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
/* stop the perf counter */
perf_end(_sample_perf);
src/drivers/device/cdev.cpp
+11 -1
View File
@@ -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 <sys/ioctl.h>
#include <arch/irq.h>
@@ -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,14 @@ 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);
return OK;
break;
case DEVIOCGPUBBLOCK:
return _pub_blocked;
break;
}
return -ENOTTY;
src/drivers/device/device.cpp
+1 -1
View File
@@ -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
src/drivers/device/device.h
+3 -1
View File
@@ -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;
src/drivers/drv_device.h
+62
View File
@@ -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 <stdint.h>
#include <sys/ioctl.h>
#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 */
src/drivers/ets_airspeed/ets_airspeed.cpp
+9 -6
View File
@@ -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
@@ -77,6 +77,7 @@
/* I2C bus address */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
#define ETS_PATH "/dev/ets_airspeed"
/* Register address */
#define READ_CMD 0x07 /* Read the data */
@@ -93,7 +94,7 @@
class ETSAirspeed : public Airspeed
{
public:
ETSAirspeed(int bus, int address = I2C_ADDRESS);
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
protected:
@@ -112,8 +113,8 @@ protected:
*/
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
CONVERSION_INTERVAL)
ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
{
}
@@ -184,8 +185,10 @@ ETSAirspeed::collect()
report.voltage = 0;
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 (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
src/drivers/gps/gps.cpp
+13 -8
View File
@@ -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++;
src/drivers/gps/gps_helper.cpp
+4 -3
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* 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 <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
float
src/drivers/gps/gps_helper.h
+3 -3
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* 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 <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef GPS_HELPER_H
src/drivers/gps/module.mk
+1 -1
View File
@@ -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
src/drivers/gps/mtk.cpp
+7 -4
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* 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 mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <unistd.h>
#include <stdio.h>
src/drivers/gps/mtk.h
+7 -4
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* 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 mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef MTK_H_
#define MTK_H_
src/drivers/gps/ubx.cpp
+6 -4
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* 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 <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
*/
#include <assert.h>
#include <math.h>
#include <poll.h>
src/drivers/gps/ubx.h
+12 -5
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* 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 <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
*/
#ifndef UBX_H_
#define UBX_H_
src/drivers/hmc5883/hmc5883.cpp
+18 -18
View File
@@ -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 */
@@ -867,9 +857,17 @@ HMC5883::collect()
/* 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);
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 */
@@ -1140,10 +1138,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);
}
}
}
src/drivers/l3gd20/l3gd20.cpp
+19 -8
View File
@@ -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,15 +379,24 @@ 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();
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;
@@ -894,8 +903,10 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0)
if (_gyro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
src/drivers/lsm303d/lsm303d.cpp
+44 -27
View File
@@ -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,13 +597,37 @@ 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);
/* 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:
@@ -727,7 +751,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,8 +1541,8 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic != -1) {
/* publish for subscribers */
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
@@ -1591,8 +1615,8 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic != -1) {
/* publish for subscribers */
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
@@ -1707,13 +1731,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;
src/drivers/meas_airspeed/meas_airspeed.cpp
+15 -10
View File
@@ -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
@@ -50,6 +50,7 @@
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -89,8 +90,10 @@
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */
#define PATH_MS4525 "/dev/ms4525"
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
#define PATH_MS5525 "/dev/ms5525"
/* Register address */
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
@@ -101,7 +104,7 @@
class MEASAirspeed : public Airspeed
{
public:
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
protected:
@@ -120,8 +123,8 @@ protected:
*/
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
CONVERSION_INTERVAL)
MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
{
}
@@ -208,8 +211,10 @@ MEASAirspeed::collect()
report.voltage = 0;
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 (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
@@ -302,7 +307,7 @@ start(int i2c_bus)
errx(1, "already started");
/* create the driver, try the MS4525DO first */
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr)
@@ -311,7 +316,7 @@ start(int i2c_bus)
/* try the MS5525DSO next if init fails */
if (OK != g_dev->Airspeed::init()) {
delete g_dev;
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
/* check if the MS5525DSO was instantiated */
if (g_dev == nullptr)
@@ -384,7 +389,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -409,7 +414,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
src/drivers/mkblctrl/mkblctrl.cpp
+95 -365
View File
@@ -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 <marco@wtns.de>
*
* Redistribution and use in source and binary forms, with or without
@@ -65,7 +65,6 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
@@ -93,16 +92,12 @@
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
class MK : public device::I2C
{
public:
enum Mode {
MODE_NONE,
MODE_2PWM,
MODE_4PWM,
MODE_6PWM,
};
enum MappingMode {
MAPPING_MK = 0,
MAPPING_PX4,
@@ -120,8 +115,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);
@@ -133,7 +127,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;
@@ -180,33 +173,15 @@ private:
static const GPIOConfig _gpio_tab[];
static const unsigned _ngpio;
void gpio_reset(void);
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[] = {
{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},
};
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
@@ -247,8 +222,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),
@@ -317,26 +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;
}
}
/* reset GPIOs */
gpio_reset();
}
/* 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) {
@@ -354,43 +325,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;
@@ -621,11 +556,13 @@ MK::task_main()
}
}
if(!_overrideSecurityChecks) {
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 */
@@ -675,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);
}
}
@@ -728,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();
}
@@ -765,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
foundMotorCount++;
if (Motor[i].MaxPWM == 250) {
if ((Motor[i].MaxPWM & 252) == 248) {
Motor[i].Version = BLCTRL_NEW;
} else {
@@ -781,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;
}
@@ -811,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;
@@ -1019,28 +959,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 */
@@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = _update_rate;
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK;
break;
@@ -1084,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;
}
@@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len)
return count * 2;
}
void
MK::gpio_reset(void)
{
/*
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
* to input mode.
*/
for (unsigned i = 0; i < _ngpio; i++)
stm32_configgpio(_gpio_tab[i].input);
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
}
void
MK::gpio_set_function(uint32_t gpios, int function)
{
/*
* 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);
}
/* 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;
}
}
}
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
}
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,
@@ -1341,20 +1135,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);
@@ -1368,7 +1153,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) {
@@ -1383,86 +1167,54 @@ 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;
}
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;
if (g_mk == nullptr) {
// try i2c3 first
g_mk = new MK(3, device_path);
g_mk = new MK(3, device_path);
if (!g_mk)
return -ENOMEM;
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;
}
}
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;
}
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;
}
}
return -1;
}
delete g_mk;
g_mk = nullptr;
// fallback to bus 1
g_mk = new MK(1, device_path);
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;
}
}
delete g_mk;
g_mk = nullptr;
return -ENXIO;
}
} // namespace
@@ -1472,10 +1224,8 @@ 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;
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
@@ -1489,18 +1239,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) {
@@ -1560,51 +1298,43 @@ 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");
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 (bus == -1) {
bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
}
if (g_mk == nullptr) {
if (mk_start(motorcount, devicepath) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
}
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)");
}
/* 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(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
exit(0);
exit(0);
} else {
errx(1, "MK-BLCtrl driver already running");
}
} 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.");
} else {
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);
}
}
}
}
}
src/drivers/mpu6000/mpu6000.cpp
+34 -16
View File
@@ -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,36 @@ MPU6000::init()
return ret;
}
/* fetch an initial set of measurements for advertisement */
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
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);
}
/* 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;
@@ -1307,10 +1326,13 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (_accel_topic != -1) {
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (_gyro->_gyro_topic != -1) {
if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
@@ -1331,6 +1353,7 @@ MPU6000::print_info()
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
_parent(parent),
_gyro_topic(-1),
_gyro_class_instance(-1)
{
}
@@ -1356,11 +1379,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;
src/drivers/ms5611/ms5611.cpp
+68 -19
View File
@@ -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
@@ -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
{
@@ -132,6 +133,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;
@@ -192,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),
@@ -204,6 +207,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 +222,9 @@ MS5611::~MS5611()
/* make sure we are truly inactive */
stop_cycle();
if (_class_instance != -1)
unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
/* free any existing reports */
if (_reports != nullptr)
delete _reports;
@@ -251,18 +258,57 @@ 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);
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_DEVICE_PATH);
if (_baro_topic < 0) {
debug("failed to create sensor_baro object");
ret = -ENOSPC;
goto out;
}
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);
ret = OK;
out:
return ret;
}
@@ -670,7 +716,10 @@ 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 (_baro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
@@ -812,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;
@@ -846,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));
@@ -905,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 ");
@@ -944,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))
src/lib/launchdetection/launchdetection_params.c
+33 -15
View File
@@ -45,28 +45,46 @@
#include <systemlib/param/param.h>
/*
* Launch detection parameters, accessible via MAVLink
* Catapult launch detection parameters, accessible via MAVLink
*
*/
/* Catapult Launch detection */
// @DisplayName Switch to enable launchdetection
// @Description if set to 1 launchdetection is enabled
// @Range 0 or 1
/**
* Enable launch detection.
*
* @min 0
* @max 1
* @group Launch detection
*/
PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
// @DisplayName Catapult Accelerometer Threshold
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
// @Range > 0
/**
* Catapult accelerometer theshold.
*
* LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
*
* @min 0
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
// @DisplayName Catapult Time Threshold
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
// @Range > 0, in seconds
/**
* Catapult time theshold.
*
* LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
*
* @min 0
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
// @DisplayName Throttle setting while detecting the launch
// @Description The throttle is set to this value while the system is waiting for the takeoff
// @Range 0 to 1
/**
* Throttle setting while detecting launch.
*
* The throttle is set to this value while the system is waiting for the take-off.
*
* @min 0
* @max 1
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
src/modules/commander/commander.cpp
+12 -8
View File
@@ -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;
@@ -429,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
@@ -515,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
if (safety->safety_switch_available && !safety->safety_off) {
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
@@ -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, &parachute_enabled);
}
orb_check(sp_man_sub, &updated);
@@ -1152,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
@@ -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();
}
src/modules/commander/commander_params.c
+32
View File
@@ -48,7 +48,39 @@
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
/**
* Empty cell voltage.
*
* Defines the voltage where a single cell of the battery is considered empty.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
/**
* Full cell voltage.
*
* Defines the voltage where a single cell of the battery is considered full.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
/**
* Number of cells.
*
* Defines the number of cells the attached battery consists of.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
/**
* Battery capacity.
*
* Defines the capacity of the attached battery.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
src/modules/commander/state_machine_helper.cpp
+30
View File
@@ -42,6 +42,8 @@
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -50,6 +52,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
@@ -332,6 +335,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
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) {
int sensfd = ::open(direntry->d_name, 0);
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
close(sensfd);
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "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;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+13 -12
View File
@@ -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> &current_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> &current_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> &current_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> &current_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> &current_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)
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+246 -38
View File
@@ -40,12 +40,10 @@
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
/**
@@ -119,58 +117,268 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
/**
* Controller roll limit
*
* The maximum roll the controller will output.
*
* @unit degrees
* @min 0.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
/**
* Throttle limit max
*
* This is the maximum throttle % that can be used by the controller.
* For overpowered aircraft, this should be reduced to a value that
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
/**
* Throttle limit min
*
* This is the minimum throttle % that can be used by the controller.
* For electric aircraft this will normally be set to zero, but can be set
* to a small non-zero value if a folding prop is fitted to prevent the
* prop from folding and unfolding repeatedly in-flight or to provide
* some aerodynamic drag from a turning prop to improve the descent rate.
*
* For aircraft with internal combustion engine this parameter should be set
* for desired idle rpm.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
/**
* Throttle limit value before flare
*
* This throttle value will be set as throttle limit at FW_LND_TLALT,
* before arcraft will flare.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
* Maximum climb rate
*
* This is the best climb rate that the aircraft can achieve with
* the throttle set to THR_MAX and the airspeed set to the
* default value. For electric aircraft make sure this number can be
* achieved towards the end of flight when the battery voltage has reduced.
* The setting of this parameter can be checked by commanding a positive
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
* required to climb is close to THR_MAX and the aircraft is maintaining
* airspeed, then this parameter is set correctly. If the airspeed starts
* to reduce, then the parameter is set to high, and if the throttle
* demand required to climb and maintain speed is noticeably less than
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* FW_THR_MAX reduced.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
/**
* Minimum descent rate
*
* This is the sink rate of the aircraft with the throttle
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Maximum descent rate
*
* This sets the maximum descent rate that the controller will use.
* If this value is too large, the aircraft can over-speed on descent.
* This should be set to a value that can be achieved without
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
/**
* TECS time constant
*
* This is the time constant of the TECS control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
/**
* Integrator gain
*
* This is the integrator gain on the control loop.
* Increasing this gain increases the speed at which speed
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
/**
* Maximum vertical acceleration
*
* This is the maximum vertical acceleration (in metres/second^2)
* either up or down that the controller will use to correct speed
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
/**
* Complementary filter "omega" parameter for height
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse vertical acceleration and barometric height to obtain
* an estimate of height rate and height. Increasing this frequency weights
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
/**
* Complementary filter "omega" parameter for speed
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* improved airspeed estimate. Increasing this frequency weights the solution
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
/**
* Roll -> Throttle feedforward
*
* Increasing this gain turn increases the amount of throttle that will
* be used to compensate for the additional drag created by turning.
* Ideally this should be set to approximately 10 x the extra sink rate
* in m/s created by a 45 degree bank turn. Increase this gain if
* the aircraft initially loses energy in turns and reduce if the
* aircraft initially gains energy in turns. Efficient high aspect-ratio
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
/**
* Speed <--> Altitude priority
*
* This parameter adjusts the amount of weighting that the pitch control
* applies to speed vs height errors. Setting it to 0.0 will cause the
* pitch control to control height and ignore speed errors. This will
* normally improve height accuracy but give larger airspeed errors.
* Setting it to 2.0 will cause the pitch control loop to control speed
* and ignore height errors. This will normally reduce airspeed errors,
* but give larger height errors. The default value of 1.0 allows the pitch
* control to simultaneously control height and speed.
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
/**
* Pitch damping factor
*
* This is the damping gain for the pitch demand loop. Increase to add
* damping to correct for oscillations in height. The default value of 0.0
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate P factor
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Speed rate P factor
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
/**
* Landing slope angle
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
/**
* Landing slope length
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
/**
*
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
/**
* Landing flare altitude (relative)
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
/**
* Landing throttle limit altitude (relative)
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
/**
* Landing heading hold horizontal distance
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
src/modules/fw_pos_control_l1/landingslope.cpp
+17 -5
View File
@@ -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;
}
src/modules/fw_pos_control_l1/landingslope.h
+16 -5
View File
@@ -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;}
src/modules/mavlink/mavlink.c
+12
View File
@@ -76,8 +76,20 @@
#include <uORB/topics/mission_result.h>
/* define MAVLink specific parameters */
/**
* MAVLink system ID
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
/**
* MAVLink component ID
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
/**
* MAVLink type
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
__EXPORT int mavlink_main(int argc, char *argv[]);
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -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 {
src/modules/mc_att_control/mc_att_control_main.cpp
+28 -33
View File
@@ -53,11 +53,9 @@
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
@@ -71,7 +69,6 @@
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
@@ -84,9 +81,9 @@
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define MIN_TAKEOFF_THROTTLE 0.3f
#define YAW_DEADZONE 0.05f
#define RATES_I_LIMIT 0.5f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
class MulticopterAttitudeControl
{
@@ -135,15 +132,13 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */
math::Matrix<3, 3> _R; /**< rotation matrix for current state */
math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */
math::Matrix<3, 3> I; /**< identity matrix */
math::Matrix<3, 3> _I; /**< identity matrix */
bool _reset_yaw_sp; /**< reset yaw setpoint flag */
@@ -262,7 +257,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_actuators_0_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
memset(&_v_att, 0, sizeof(_v_att));
@@ -276,15 +271,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_i.zero();
_params.rate_d.zero();
_R_sp.identity();
_R.identity();
_rates_prev.zero();
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
I.identity();
_I.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
@@ -535,16 +528,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
if (_v_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
_R_sp.set(&_v_att_sp.R_body[0][0]);
R_sp.set(&_v_att_sp.R_body[0][0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
_v_att_sp.R_valid = true;
}
@@ -561,23 +556,24 @@ MulticopterAttitudeControl::control_attitude(float dt)
}
/* rotation matrix for current state */
_R.set(_v_att.R);
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
@@ -600,15 +596,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = _R;
R_rp = R;
}
/* R_rp and _R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
@@ -616,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(_R.transposed() * _R_sp);
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
@@ -658,7 +654,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
_rates_prev = rates;
/* update integral only if not saturated on low limit */
if (_thrust_sp > 0.1f) {
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
@@ -695,9 +691,6 @@ MulticopterAttitudeControl::task_main()
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
orb_set_interval(_v_att_sub, 5);
/* initialize parameters cache */
parameters_update();
@@ -767,10 +760,12 @@ MulticopterAttitudeControl::task_main()
}
} else {
/* attitude controller disabled */
// TODO poll 'attitude_rates_setpoint' topic
_rates_sp.zero();
_thrust_sp = 0.0f;
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
if (_v_control_mode.flag_control_rates_enabled) {
src/modules/mc_att_control/mc_att_control_params.c
+119
View File
@@ -41,16 +41,135 @@
#include <systemlib/param/param.h>
/**
* Roll P gain
*
* Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
/**
* Roll rate P gain
*
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
/**
* Roll rate I gain
*
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
/**
* Roll rate D gain
*
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
/**
* Pitch P gain
*
* Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
/**
* Pitch rate P gain
*
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
/**
* Pitch rate I gain
*
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
/**
* Pitch rate D gain
*
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
/**
* Yaw P gain
*
* Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
/**
* Yaw rate P gain
*
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
/**
* Yaw rate I gain
*
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
/**
* Yaw rate D gain
*
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
/**
* Yaw feed forward
*
* Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
src/modules/mc_pos_control/mc_pos_control_main.cpp
-2
View File
@@ -51,7 +51,6 @@
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
@@ -68,7 +67,6 @@
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
src/modules/mc_pos_control/mc_pos_control_params.c
+145 -1
View File
@@ -39,20 +39,164 @@
#include <systemlib/param/param.h>
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
/**
* Minimum thrust
*
* Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
/**
* Maximum thrust
*
* Limit max allowed thrust.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
/**
* Proportional gain for vertical position error
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
/**
* Proportional gain for vertical velocity error
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
/**
* Integral gain for vertical velocity error
*
* Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
/**
* Differential gain for vertical velocity error
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
/**
* Maximum vertical velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
/**
* Vertical velocity feed forward
*
* Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
/**
* Proportional gain for horizontal position error
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
/**
* Proportional gain for horizontal velocity error
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
/**
* Integral gain for horizontal velocity error
*
* Non-zero value allows to resist wind.
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
/**
* Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
/**
* Maximum horizontal velocity
*
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
/**
* Horizontal velocity feed forward
*
* Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
/**
* Maximum tilt
*
* Limits maximum tilt in AUTO and EASY modes.
*
* @min 0.0
* @max 1.57
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
/**
* Landing descend rate
*
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
/**
* Maximum landing tilt
*
* Limits maximum tilt on landing.
*
* @min 0.0
* @max 1.57
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);
src/modules/navigator/geofence_params.c
+11 -5
View File
@@ -45,11 +45,17 @@
#include <systemlib/param/param.h>
/*
* geofence parameters, accessible via MAVLink
*
* Geofence parameters, accessible via MAVLink
*/
// @DisplayName Switch to enable geofence
// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
// @Range 0 or 1
/**
* Enable geofence.
*
* Set to 1 to enable geofence.
* Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ON, 1);
src/modules/navigator/navigator_main.cpp
+37 -34
View File
@@ -305,6 +305,12 @@ private:
void start_land();
void start_land_home();
/**
* Fork for state transitions
*/
void request_loiter_or_ready();
void request_mission_if_available();
/**
* Guards offboard mission
*/
@@ -699,24 +705,17 @@ Navigator::task_main()
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
request_loiter_or_ready();
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_mission_if_available();
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
request_mission_if_available();
stick_mode = true;
}
}
@@ -733,17 +732,11 @@ Navigator::task_main()
break;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
request_loiter_or_ready();
break;
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_mission_if_available();
break;
case NAV_STATE_RTL:
@@ -768,12 +761,7 @@ Navigator::task_main()
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_mission_if_available();
}
}
}
@@ -1069,7 +1057,7 @@ Navigator::start_loiter()
float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
_pos_sp_triplet.current.alt = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
@@ -1078,9 +1066,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;
@@ -1396,6 +1383,28 @@ Navigator::set_rtl_item()
_pos_sp_triplet_updated = true;
}
void
Navigator::request_loiter_or_ready()
{
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
void
Navigator::request_mission_if_available()
{
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
request_loiter_or_ready();
}
}
void
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
{
@@ -1554,13 +1563,7 @@ Navigator::on_mission_item_reached()
/* loiter at last waypoint */
_reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_loiter_or_ready();
}
} else if (myState == NAV_STATE_RTL) {
src/modules/navigator/navigator_params.c
+83 -6
View File
@@ -50,14 +50,91 @@
/*
* Navigator parameters, accessible via MAVLink
*
*/
/**
* Minimum altitude (fixed wing only)
*
* Minimum altitude above home for LOITER.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
/**
* Waypoint acceptance radius
*
* Default value of acceptance radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
/**
* Loiter radius (fixed wing only)
*
* Default value of loiter radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Enable onboard mission
*
* @group Navigation
*/
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, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
/**
* Landing altitude
*
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
/**
* Return-To-Launch altitude
*
* Minimum altitude above home position for going home in RTL mode.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
/**
* Return-To-Launch delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
/**
* Enable parachute deployment
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+4 -7
View File
@@ -42,14 +42,11 @@
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <string.h>
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
@@ -527,13 +524,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 +586,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 {
src/modules/position_estimator_inav/position_estimator_inav_params.c
+1 -1
View File
@@ -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);
src/modules/px4iofirmware/mixer.cpp
+1 -1
View File
@@ -179,7 +179,7 @@ mixer_tick(void)
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
src/modules/sdlog2/sdlog2.c
+24
View File
@@ -82,6 +82,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -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 */
src/modules/sdlog2/sdlog2_messages.h
+14
View File
@@ -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"),
src/modules/sensors/sensor_params.c
+268 -35
View File
@@ -42,13 +42,10 @@
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Gyro X offset
*
* This is an X-axis offset for the gyro. Adjust it according to the calibration data.
* Gyro X-axis offset
*
* @min -10.0
* @max 10.0
@@ -57,7 +54,7 @@
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
/**
* Gyro Y offset
* Gyro Y-axis offset
*
* @min -10.0
* @max 10.0
@@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
/**
* Gyro Z offset
* Gyro Z-axis offset
*
* @min -5.0
* @max 5.0
@@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
/**
* Gyro X scaling
*
* X-axis scaling.
* Gyro X-axis scaling factor
*
* @min -1.5
* @max 1.5
@@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
/**
* Gyro Y scaling
*
* Y-axis scaling.
* Gyro Y-axis scaling factor
*
* @min -1.5
* @max 1.5
@@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
/**
* Gyro Z scaling
*
* Z-axis scaling.
* Gyro Z-axis scaling factor
*
* @min -1.5
* @max 1.5
@@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
/**
* Magnetometer X offset
*
* This is an X-axis offset for the magnetometer.
* Magnetometer X-axis offset
*
* @min -500.0
* @max 500.0
@@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
/**
* Magnetometer Y offset
*
* This is an Y-axis offset for the magnetometer.
* Magnetometer Y-axis offset
*
* @min -500.0
* @max 500.0
@@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
/**
* Magnetometer Z offset
*
* This is an Z-axis offset for the magnetometer.
* Magnetometer Z-axis offset
*
* @min -500.0
* @max 500.0
@@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
/**
* Magnetometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
/**
* Magnetometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
/**
* Magnetometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
/**
* Accelerometer X-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
/**
* Accelerometer Y-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
/**
* Accelerometer Z-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
/**
* Accelerometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
/**
* Accelerometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
/**
* Accelerometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
/**
* Differential pressure sensor offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
/**
* Differential pressure sensor analog enabled
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
/**
* Board rotation
*
* This parameter defines the rotation of the FMU board relative to the platform.
* Possible values are:
* 0 = No rotation
* 1 = Yaw 45°
* 2 = Yaw 90°
* 3 = Yaw 135°
* 4 = Yaw 180°
* 5 = Yaw 225°
* 6 = Yaw 270°
* 7 = Yaw 315°
* 8 = Roll 180°
* 9 = Roll 180°, Yaw 45°
* 10 = Roll 180°, Yaw 90°
* 11 = Roll 180°, Yaw 135°
* 12 = Pitch 180°
* 13 = Roll 180°, Yaw 225°
* 14 = Roll 180°, Yaw 270°
* 15 = Roll 180°, Yaw 315°
* 16 = Roll 90°
* 17 = Roll 90°, Yaw 45°
* 18 = Roll 90°, Yaw 90°
* 19 = Roll 90°, Yaw 135°
* 20 = Roll 270°
* 21 = Roll 270°, Yaw 45°
* 22 = Roll 270°, Yaw 90°
* 23 = Roll 270°, Yaw 135°
* 24 = Pitch 90°
* 25 = Pitch 270°
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
/**
* External magnetometer rotation
*
* This parameter defines the rotation of the external magnetometer relative
* to the platform (not relative to the FMU).
* See SENS_BOARD_ROT for possible values.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
/**
* RC Channel 1 Minimum
*
@@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
#endif
PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
/**
* DSM binding trigger.
*
* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind
*
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
/**
* Scaling factor for battery voltage sensor on PX4IO.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
/**
* Scaling factor for battery voltage sensor on FMU v2.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
#else
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
/* PX4IOAR: 0.00838095238 */
/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
/**
* Scaling factor for battery voltage sensor on FMU v1.
*
* FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659
* FMUv1 with PX4IO: 0.00459340659
* FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
/**
* Scaling factor for battery current sensor.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
/**
* Roll control channel mapping.
*
@@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
/**
* Return switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
/**
* Assist switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
/**
* Mission switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
/**
* Flaps channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */
/**
* Auxiliary switch 1 channel mapping.
*
* Default function: Camera pitch
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);
/**
* Auxiliary switch 2 channel mapping.
*
* Default function: Camera roll
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
/**
* Auxiliary switch 3 channel mapping.
*
* Default function: Camera azimuth / yaw
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
/**
* Roll scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
/**
* Pitch scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
/**
* Yaw scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */
PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */
/**
* Failsafe channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_CH, 0);
/**
* Failsafe channel mode.
*
* 0 = too low means signal loss,
* 1 = too high means signal loss
*
* @min 0
* @max 1
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_MODE, 0);
/**
* Failsafe channel PWM threshold.
*
* @min 0
* @max 1
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
src/modules/systemlib/system_params.c
+17 -2
View File
@@ -40,8 +40,23 @@
#include <nuttx/config.h>
#include <systemlib/param/param.h>
// Auto-start script with index #n
/**
* Auto-start script index.
*
* Defines the auto-start script used to bootstrap the system.
*
* @group System
*/
PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
// Automatically configure default values
/**
* Automatically configure default values.
*
* Set to 1 to set platform-specific parameters to their default
* values on next system startup.
*
* @min 0
* @max 1
* @group System
*/
PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
src/modules/uORB/topics/telemetry_status.h
+5 -5
View File
@@ -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 */
#endif /* TOPIC_TELEMETRY_STATUS_H */
src/systemcmds/config/config.c
+49 -7
View File
@@ -56,6 +56,7 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#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) {