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

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2013-12-21 19:07:24 +01:00
parent c033443208 831f153b73
commit b84c9f962b
92 changed files with 6012 additions and 2001 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.h
+1 -1
View File
@@ -119,7 +119,7 @@ protected:
virtual int collect() = 0;
work_s _work;
float _max_differential_pressure_pa;
float _max_differential_pressure_pa;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
src/drivers/ardrone_interface/ardrone_motor_control.c
-23
View File
@@ -46,7 +46,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
/* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) {
@@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
actuator_controls_effective.control_effective[1] = pitch_control;
/* yaw output after limiting */
actuator_controls_effective.control_effective[2] = yaw_control;
/* possible motor thrust limiting */
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
if (!initialized) {
/* advertise and publish */
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
initialized = true;
} else {
/* already initialized, just publishing */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
}
/* set the motor values */
/* scale up from 0..1 to 10..500) */
src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+5
View File
@@ -83,6 +83,11 @@ __EXPORT void weak_function stm32_spiinitialize(void)
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
stm32_configgpio(GPIO_EXTI_MAG_DRDY);
stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
src/drivers/device/cdev.cpp
+36
View File
@@ -108,6 +108,42 @@ CDev::~CDev()
unregister_driver(_devname);
}
int
CDev::register_class_devname(const char *class_devname)
{
if (class_devname == nullptr) {
return -EINVAL;
}
int class_instance = 0;
int ret = -ENOSPC;
while (class_instance < 4) {
if (class_instance == 0) {
ret = register_driver(class_devname, &fops, 0666, (void *)this);
if (ret == OK) break;
} else {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
}
class_instance++;
}
if (class_instance == 4)
return ret;
return class_instance;
}
int
CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
{
if (class_instance > 0) {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
return unregister_driver(name);
}
return unregister_driver(class_devname);
}
int
CDev::init()
{
src/drivers/device/device.h
+22
View File
@@ -396,6 +396,25 @@ protected:
*/
virtual int close_last(struct file *filp);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @return class_instamce Class instance created, or -errno on failure
*/
virtual int register_class_devname(const char *class_devname);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @param class_instance Device class instance from register_class_devname()
* @return OK on success, -errno otherwise
*/
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
private:
static const unsigned _max_pollwaiters = 8;
@@ -488,4 +507,7 @@ private:
} // namespace device
// class instance for primary driver of each class
#define CLASS_DEVICE_PRIMARY 0
#endif /* _DEVICE_DEVICE_H */
src/drivers/drv_hrt.h
+14
View File
@@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry);
*/
__EXPORT extern void hrt_cancel(struct hrt_call *entry);
/*
* initialise a hrt_call structure
*/
__EXPORT extern void hrt_call_init(struct hrt_call *entry);
/*
* delay a hrt_call_every() periodic call by the given number of
* microseconds. This should be called from within the callout to
* cause the callout to be re-scheduled for a later time. The periodic
* callouts will then continue from that new base time at the
* previously specified period.
*/
__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
/*
* Initialise the HRT.
*/
src/drivers/drv_rc_input.h
+3
View File
@@ -89,6 +89,9 @@ struct rc_input_values {
/** number of channels actually being seen */
uint32_t channel_count;
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
int32_t rssi;
/** Input source */
enum RC_INPUT_SOURCE input_source;
src/drivers/hmc5883/hmc5883.cpp
+27 -15
View File
@@ -77,6 +77,7 @@
*/
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
#define HMC5883L_DEVICE_PATH "/dev/hmc5883"
/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
@@ -154,6 +155,7 @@ private:
float _range_scale;
float _range_ga;
bool _collect_phase;
int _class_instance;
orb_advert_t _mag_topic;
@@ -315,12 +317,13 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_mag_topic(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -351,6 +354,9 @@ HMC5883::~HMC5883()
if (_reports != nullptr)
delete _reports;
if (_class_instance != -1)
unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@@ -374,13 +380,17 @@ HMC5883::init()
/* reset the device configuration */
reset();
/* get a publish handle on the mag topic */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
_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");
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
}
ret = OK;
/* sensor is ok, but not calibrated */
@@ -875,8 +885,10 @@ HMC5883::collect()
}
#endif
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
}
/* post a report to the ring */
if (_reports->force(&new_report)) {
@@ -1256,7 +1268,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@@ -1288,10 +1300,10 @@ test()
ssize_t sz;
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@@ -1388,10 +1400,10 @@ int calibrate()
{
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
@@ -1413,7 +1425,7 @@ int calibrate()
void
reset()
{
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
src/drivers/l3gd20/l3gd20.cpp
+80 -16
View File
@@ -66,6 +66,8 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#define L3GD20_DEVICE_PATH "/dev/l3gd20"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -93,10 +95,15 @@ static const int ERROR = -1;
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define ADDR_CTRL_REG2 0x21
@@ -194,6 +201,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _class_instance;
unsigned _current_rate;
unsigned _orientation;
@@ -201,6 +209,8 @@ private:
unsigned _read;
perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
@@ -306,16 +316,19 @@ private:
};
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call_interval(0),
_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
@@ -341,8 +354,13 @@ L3GD20::~L3GD20()
if (_reports != nullptr)
delete _reports;
if (_class_instance != -1)
unregister_class_devname(GYRO_DEVICE_PATH, _class_instance);
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors);
}
int
@@ -360,10 +378,13 @@ L3GD20::init()
if (_reports == nullptr)
goto out;
/* 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);
_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();
@@ -662,15 +683,15 @@ L3GD20::set_samplerate(unsigned frequency)
} else if (frequency <= 200) {
_current_rate = 190;
bits |= RATE_190HZ_LP_70HZ;
bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) {
_current_rate = 380;
bits |= RATE_380HZ_LP_100HZ;
bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) {
_current_rate = 760;
bits |= RATE_760HZ_LP_100HZ;
bits |= RATE_760HZ_LP_50HZ;
} else {
return -EINVAL;
}
@@ -710,8 +731,16 @@ L3GD20::stop()
void
L3GD20::disable_i2c(void)
{
uint8_t a = read_reg(0x05);
write_reg(0x05, (0x20 | a));
uint8_t retries = 10;
while (retries--) {
// add retries
uint8_t a = read_reg(0x05);
write_reg(0x05, (0x20 | a));
if (read_reg(0x05) == (a | 0x20)) {
return;
}
}
debug("FAILED TO DISABLE I2C");
}
void
@@ -723,7 +752,7 @@ L3GD20::reset()
/* set default configuration */
write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
@@ -750,9 +779,26 @@ L3GD20::measure_trampoline(void *arg)
dev->measure();
}
#ifdef GPIO_EXTI_GYRO_DRDY
# define L3GD20_USE_DRDY 1
#else
# define L3GD20_USE_DRDY 0
#endif
void
L3GD20::measure()
{
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
@@ -775,6 +821,16 @@ L3GD20::measure()
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if ((raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@@ -852,6 +908,8 @@ L3GD20::print_info()
{
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors);
_reports->print_info("report queue");
}
@@ -902,7 +960,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
if (g_dev == nullptr)
goto fail;
@@ -911,7 +969,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@@ -919,6 +977,8 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
@@ -943,10 +1003,10 @@ test()
ssize_t sz;
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", L3GD20_DEVICE_PATH);
/* reset to manual polling */
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -967,6 +1027,8 @@ test()
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset();
@@ -979,7 +1041,7 @@ test()
void
reset()
{
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@@ -990,6 +1052,8 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel pollrate reset failed");
close(fd);
exit(0);
}
src/drivers/lsm303d/lsm303d.cpp
+286 -37
View File
@@ -39,6 +39,7 @@
#include <nuttx/config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
@@ -63,6 +64,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_tone_alarm.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -78,7 +80,8 @@ static const int ERROR = -1;
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel"
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
/* register addresses: A: accel, M: mag, T: temp */
#define ADDR_WHO_AM_I 0x0F
@@ -231,6 +234,16 @@ public:
*/
void print_registers();
/**
* toggle logging
*/
void toggle_logging();
/**
* check for extreme accel values
*/
void check_extremes(const accel_report *arb);
protected:
virtual int probe();
@@ -264,7 +277,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_advert_t _mag_topic;
int _class_instance;
unsigned _accel_read;
unsigned _mag_read;
@@ -273,6 +286,8 @@ private:
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
perf_counter_t _extreme_values;
perf_counter_t _accel_reschedules;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@@ -283,6 +298,15 @@ private:
uint8_t _reg7_expected;
uint8_t _reg1_expected;
// accel logging
int _accel_log_fd;
bool _accel_logging_enabled;
uint64_t _last_extreme_us;
uint64_t _last_log_us;
uint64_t _last_log_sync_us;
uint64_t _last_log_reg_us;
uint64_t _last_log_alarm_us;
/**
* Start automatic measurement.
*/
@@ -443,6 +467,8 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class LSM303D;
@@ -450,6 +476,9 @@ protected:
private:
LSM303D *_parent;
orb_advert_t _mag_topic;
int _mag_class_instance;
void measure();
void measure_trampoline(void *arg);
@@ -457,7 +486,7 @@ private:
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
_mag(new LSM303D_mag(this)),
_call_accel_interval(0),
_call_mag_interval(0),
@@ -471,18 +500,26 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_mag_topic(-1),
_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0)
_reg7_expected(0),
_accel_log_fd(-1),
_accel_logging_enabled(false),
_last_log_us(0),
_last_log_sync_us(0),
_last_log_reg_us(0),
_last_log_alarm_us(0)
{
// enable debug() calls
_debug_enabled = true;
@@ -514,11 +551,17 @@ LSM303D::~LSM303D()
if (_mag_reports != nullptr)
delete _mag_reports;
if (_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
delete _mag;
/* delete the perf counter */
perf_free(_accel_sample_perf);
perf_free(_mag_sample_perf);
perf_free(_reg1_resets);
perf_free(_reg7_resets);
perf_free(_extreme_values);
}
int
@@ -540,10 +583,6 @@ LSM303D::init()
goto out;
/* advertise accel topic */
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
@@ -551,19 +590,22 @@ LSM303D::init()
reset();
/* advertise mag topic */
struct mag_report zero_mag_report;
memset(&zero_mag_report, 0, sizeof(zero_mag_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
/* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init();
if (mag_ret != OK) {
_mag_topic = -1;
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
warnx("MAG init failed");
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);
}
ret = OK;
out:
return ret;
}
@@ -595,11 +637,18 @@ LSM303D::reset()
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz
// we setup the anti-alias on-chip filter as 50Hz. We believe
// this operates in the analog domain, and is critical for
// anti-aliasing. The 2 pole software filter is designed to
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
@@ -623,6 +672,122 @@ LSM303D::probe()
return -EIO;
}
#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
/**
check for extreme accelerometer values and log to a file on the SD card
*/
void
LSM303D::check_extremes(const accel_report *arb)
{
const float extreme_threshold = 30;
static bool boot_ok = false;
bool is_extreme = (fabsf(arb->x) > extreme_threshold &&
fabsf(arb->y) > extreme_threshold &&
fabsf(arb->z) > extreme_threshold);
if (is_extreme) {
perf_count(_extreme_values);
// force accel logging on if we see extreme values
_accel_logging_enabled = true;
} else {
boot_ok = true;
}
if (! _accel_logging_enabled) {
// logging has been disabled by user, close
if (_accel_log_fd != -1) {
::close(_accel_log_fd);
_accel_log_fd = -1;
}
return;
}
if (_accel_log_fd == -1) {
// keep last 10 logs
::unlink(ACCEL_LOGFILE ".9");
for (uint8_t i=8; i>0; i--) {
uint8_t len = strlen(ACCEL_LOGFILE)+3;
char log1[len], log2[len];
snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
::rename(log1, log2);
}
::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
// open the new logfile
_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
if (_accel_log_fd == -1) {
return;
}
}
uint64_t now = hrt_absolute_time();
// log accels at 1Hz
if (_last_log_us == 0 ||
now - _last_log_us > 1000*1000) {
_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,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
(unsigned)boot_ok);
}
const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1,
ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6,
ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M,
ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A,
ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL,
ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2,
ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC,
ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW,
ADDR_ACT_THS, ADDR_ACT_DUR,
ADDR_OUT_X_L_M, ADDR_OUT_X_H_M,
ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
uint8_t regval[sizeof(reglist)];
for (uint8_t i=0; i<sizeof(reglist); i++) {
regval[i] = read_reg(reglist[i]);
}
// log registers at 10Hz when we have extreme values, or 0.5 Hz without
if (_last_log_reg_us == 0 ||
(is_extreme && (now - _last_log_reg_us > 250*1000)) ||
(now - _last_log_reg_us > 10*1000*1000)) {
_last_log_reg_us = now;
::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
for (uint8_t i=0; i<sizeof(reglist); i++) {
::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
}
::dprintf(_accel_log_fd, "\n");
}
// fsync at 0.1Hz
if (now - _last_log_sync_us > 10*1000*1000) {
_last_log_sync_us = now;
::fsync(_accel_log_fd);
}
// play alarm every 10s if we have had an extreme value
if (perf_event_count(_extreme_values) != 0 &&
(now - _last_log_alarm_us > 10*1000*1000)) {
_last_log_alarm_us = now;
int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
if (tfd != -1) {
uint8_t tone = 3;
if (!is_extreme) {
tone = 3;
} else if (boot_ok) {
tone = 4;
} else {
tone = 5;
}
::ioctl(tfd, TONE_SET_ALARM, tone);
::close(tfd);
}
}
}
ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{
@@ -641,6 +806,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
*/
while (count--) {
if (_accel_reports->get(arb)) {
check_extremes(arb);
ret += sizeof(*arb);
arb++;
}
@@ -1003,6 +1169,7 @@ LSM303D::read_reg(unsigned reg)
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
cmd[1] = 0;
transfer(cmd, cmd, sizeof(cmd));
@@ -1275,6 +1442,14 @@ LSM303D::mag_measure_trampoline(void *arg)
void
LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
}
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
@@ -1342,8 +1517,10 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
if (_accel_topic != -1) {
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
@@ -1414,8 +1591,10 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
if (_mag->_mag_topic != -1) {
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
@@ -1441,6 +1620,8 @@ LSM303D::print_registers()
const char *name;
} regmap[] = {
{ ADDR_WHO_AM_I, "WHO_AM_I" },
{ 0x02, "I2C_CONTROL1" },
{ 0x15, "I2C_CONTROL2" },
{ ADDR_STATUS_A, "STATUS_A" },
{ ADDR_STATUS_M, "STATUS_M" },
{ ADDR_CTRL_REG0, "CTRL_REG0" },
@@ -1490,14 +1671,52 @@ LSM303D::print_registers()
printf("_reg7_expected=0x%02x\n", _reg7_expected);
}
void
LSM303D::toggle_logging()
{
if (! _accel_logging_enabled) {
_accel_logging_enabled = true;
printf("Started logging to %s\n", ACCEL_LOGFILE);
} else {
_accel_logging_enabled = false;
printf("Stopped logging\n");
}
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", MAG_DEVICE_PATH),
_parent(parent)
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_class_instance(-1)
{
}
LSM303D_mag::~LSM303D_mag()
{
if (_mag_class_instance != -1)
unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
}
int
LSM303D_mag::init()
{
int ret;
ret = CDev::init();
if (ret != OK)
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;
}
void
@@ -1543,6 +1762,7 @@ void test();
void reset();
void info();
void regdump();
void logging();
/**
* Start the driver.
@@ -1556,7 +1776,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");
@@ -1567,7 +1787,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@@ -1575,7 +1795,7 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
@@ -1584,6 +1804,8 @@ start()
}
}
close(fd);
close(fd_mag);
exit(0);
fail:
@@ -1610,10 +1832,10 @@ test()
int ret;
/* get the driver */
fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0)
err(1, "%s open failed", ACCEL_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(accel_report));
@@ -1639,10 +1861,10 @@ test()
struct mag_report m_report;
/* get the driver */
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0)
err(1, "%s open failed", MAG_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
@@ -1665,6 +1887,9 @@ test()
/* XXX add poll-rate tests here too */
close(fd_accel);
close(fd_mag);
reset();
errx(0, "PASS");
}
@@ -1675,7 +1900,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@@ -1686,7 +1911,9 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel pollrate reset failed");
fd = open(MAG_DEVICE_PATH, O_RDONLY);
close(fd);
fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
warnx("mag could not be opened, external mag might be used");
@@ -1696,6 +1923,8 @@ reset()
err(1, "mag pollrate reset failed");
}
close(fd);
exit(0);
}
@@ -1729,6 +1958,20 @@ regdump()
exit(0);
}
/**
* toggle logging
*/
void
logging()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
g_dev->toggle_logging();
exit(0);
}
} // namespace
@@ -1766,5 +2009,11 @@ lsm303d_main(int argc, char *argv[])
if (!strcmp(argv[1], "regdump"))
lsm303d::regdump();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
/*
* dump device registers
*/
if (!strcmp(argv[1], "logging"))
lsm303d::logging();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
}
src/drivers/mkblctrl/mkblctrl.cpp
+141 -57
View File
@@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Marco Bauer <marco@wtns.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,7 +36,8 @@
* @file mkblctrl.cpp
*
* Driver/configurator for the Mikrokopter BL-Ctrl.
* Marco Bauer
*
* @author Marco Bauer <marco@wtns.de>
*
*/
@@ -73,7 +75,6 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
@@ -89,8 +90,8 @@
#define BLCTRL_MIN_VALUE -0.920F
#define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_SPINUP_COUNTER 2500
#define ESC_UORB_PUBLISH_DELAY 200
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
class MK : public device::I2C
{
@@ -112,7 +113,7 @@ public:
FRAME_X,
};
MK(int bus);
MK(int bus, const char *_device_path);
~MK();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
@@ -126,7 +127,7 @@ public:
int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode);
int set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
private:
static const unsigned _max_actuators = MAX_MOTORS;
@@ -141,9 +142,9 @@ private:
unsigned int _motor;
int _px4mode;
int _frametype;
char _device[20]; ///< device
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
orb_advert_t _t_esc_status;
unsigned int _num_outputs;
@@ -244,7 +245,7 @@ MK *g_mk;
} // namespace
MK::MK(int bus) :
MK::MK(int bus, const char *_device_path) :
I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
_mode(MODE_NONE),
_update_rate(50),
@@ -252,7 +253,6 @@ MK::MK(int bus) :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
@@ -265,6 +265,10 @@ MK::MK(int bus) :
_armed(false),
_mixers(nullptr)
{
strncpy(_device, _device_path, sizeof(_device));
/* enforce null termination */
_device[sizeof(_device) - 1] = '\0';
_debug_enabled = true;
}
@@ -291,7 +295,7 @@ MK::~MK()
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
unregister_driver(_device);
g_mk = nullptr;
}
@@ -313,13 +317,15 @@ MK::init(unsigned motors)
usleep(500000);
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (sizeof(_device) > 0) {
ret = register_driver(_device, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
if (ret == OK) {
log("creating alternate output device");
_primary_pwm_device = true;
}
}
/* reset GPIOs */
gpio_reset();
@@ -525,13 +531,6 @@ MK::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
/* advertise the blctrl status */
esc_status_s esc;
memset(&esc, 0, sizeof(esc));
@@ -595,9 +594,6 @@ MK::task_main()
outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
outputs.timestamp = hrt_absolute_time();
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
/* iterate actuators */
for (unsigned int i = 0; i < _num_outputs; i++) {
@@ -633,10 +629,7 @@ MK::task_main()
}
/* output to BLCtrl's */
if (_motortest == true) {
mk_servo_test(i);
} else {
if (_motortest != true) {
//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
// 11 Bit
Motor[i].SetPoint_PX4 = outputs.output[i];
@@ -668,7 +661,7 @@ MK::task_main()
* Only update esc topic every half second.
*/
if (hrt_absolute_time() - esc.timestamp > 500000) {
if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) {
esc.counter++;
esc.timestamp = hrt_absolute_time();
esc.esc_count = (uint8_t) _num_outputs;
@@ -692,16 +685,22 @@ MK::task_main()
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);
}
}
orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
}
}
//::close(_t_esc_status);
::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed);
@@ -727,8 +726,12 @@ MK::mk_servo_arm(bool status)
unsigned int
MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
{
if(initI2C) {
I2C::init();
}
_retries = 50;
uint8_t foundMotorCount = 0;
@@ -952,6 +955,7 @@ MK::mk_servo_test(unsigned int chan)
if (_motor >= _num_outputs) {
_motor = -1;
_motortest = false;
fprintf(stderr, "[mkblctrl] Motortest finished...\n");
}
}
@@ -1367,7 +1371,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* count used motors */
do {
if (g_mk->mk_check_for_blctrl(8, false) != 0) {
if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
shouldStop = 4;
} else {
@@ -1377,7 +1381,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
sleep(1);
} while (shouldStop < 3);
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
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);
@@ -1390,13 +1394,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
}
int
mk_start(unsigned bus, unsigned motors)
mk_start(unsigned bus, unsigned motors, char *device_path)
{
int ret = OK;
if (g_mk == nullptr) {
g_mk = new MK(bus);
g_mk = new MK(bus, device_path);
if (g_mk == nullptr) {
ret = -ENOMEM;
@@ -1415,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors)
}
int
mk_check_for_i2c_esc_bus(char *device_path, int motors)
{
int ret;
if (g_mk == nullptr) {
g_mk = new MK(3, device_path);
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 3;
}
}
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;
}
}
}
return -1;
}
} // namespace
extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
@@ -1425,13 +1475,14 @@ 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 bus = -1;
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
char *devicepath = "";
/*
* optional parameters
@@ -1491,36 +1542,69 @@ mkblctrl_main(int argc, char *argv[])
newMode = true;
}
/* look for the optional device parameter */
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
devicepath = argv[i + 1];
newMode = true;
} else {
errx(1, "missing the devicename (-d)");
return 1;
}
}
}
if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode frame {+/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 1).\n");
fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\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");
exit(1);
}
if (g_mk == nullptr) {
if (mk_start(bus, motorcount) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
if (!motortest) {
if (g_mk == nullptr) {
if (bus == -1) {
bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
}
} else {
newMode = true;
}
}
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(port_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);
} else {
errx(1, "MK-BLCtrl driver already running");
}
/* test, etc. here g*/
} else {
if (g_mk == nullptr) {
errx(1, "MK-BLCtrl driver not running. You have to start it first.");
exit(1);
} else {
g_mk->set_motor_test(motortest);
exit(0);
}
}
}
src/drivers/mpu6000/mpu6000.cpp
+117 -46
View File
@@ -75,6 +75,9 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
@@ -167,7 +170,7 @@
SPI speed
*/
#define MPU6000_LOW_BUS_SPEED 1000*1000
#define MPU6000_HIGH_BUS_SPEED 10*1000*1000
#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
class MPU6000_gyro;
@@ -208,17 +211,19 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_class_instance;
RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
unsigned _reads;
unsigned _sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@@ -346,12 +351,17 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class MPU6000;
void parent_poll_notify();
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
int _gyro_class_instance;
};
@@ -359,7 +369,7 @@ private:
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, spi_dev_e device) :
SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
@@ -367,13 +377,15 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_reads(0),
_sample_rate(1000),
_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -417,8 +429,14 @@ MPU6000::~MPU6000()
if (_gyro_reports != nullptr)
delete _gyro_reports;
if (_accel_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
}
int
@@ -463,24 +481,23 @@ MPU6000::init()
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
gyro_ret = _gyro->init();
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
/* fetch an initial set of measurements for advertisement */
measure();
if (gyro_ret != OK) {
_gyro_topic = -1;
} else {
gyro_report gr;
_gyro_reports->get(&gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
}
out:
return ret;
@@ -488,17 +505,26 @@ out:
void MPU6000::reset()
{
// if the mpu6000 is initialised after the l3gd20 and lsm303d
// then if we don't do an irqsave/irqrestore here the mpu6000
// frequenctly comes up in a bad state where all transfers
// come as zero
irqstate_t state;
state = irqsave();
// Chip reset
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock (better performance)
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
up_udelay(1000);
// SAMPLE RATE
@@ -660,6 +686,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
if (_accel_reports->empty())
return -EAGAIN;
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
@@ -677,12 +705,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
int
MPU6000::self_test()
{
if (_reads == 0) {
if (perf_event_count(_sample_perf) == 0) {
measure();
}
/* return 0 on success, 1 else */
return (_reads > 0) ? 0 : 1;
return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}
int
@@ -754,6 +782,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
if (_gyro_reports->empty())
return -EAGAIN;
perf_count(_gyro_reads);
/* copy reports out of our buffer to the caller */
gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
@@ -995,9 +1025,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
uint8_t
MPU6000::read_reg(unsigned reg)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
@@ -1010,9 +1038,7 @@ MPU6000::read_reg(unsigned reg)
uint16_t
MPU6000::read_reg16(unsigned reg)
{
uint8_t cmd[3];
cmd[0] = reg | DIR_READ;
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
@@ -1163,9 +1189,6 @@ MPU6000::measure()
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return;
/* count measurement */
_reads++;
/*
* Convert from big to little endian
*/
@@ -1180,6 +1203,20 @@ MPU6000::measure()
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
return;
}
/*
* Swap axes and negate y
*/
@@ -1270,10 +1307,11 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
if (_accel_topic != -1) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (_gyro->_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@@ -1284,19 +1322,48 @@ void
MPU6000::print_info()
{
perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
_parent(parent)
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
_parent(parent),
_gyro_class_instance(-1)
{
}
MPU6000_gyro::~MPU6000_gyro()
{
if (_gyro_class_instance != -1)
unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance);
}
int
MPU6000_gyro::init()
{
int ret;
// do base class init
ret = CDev::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
_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;
}
void
@@ -1352,7 +1419,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@@ -1360,6 +1427,8 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
@@ -1384,17 +1453,17 @@ test()
ssize_t sz;
/* get the driver */
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
MPU_DEVICE_PATH_ACCEL);
/* get the driver */
int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1452,7 +1521,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@@ -1463,6 +1532,8 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
close(fd);
exit(0);
}
src/drivers/ms5611/ms5611_spi.cpp
+16 -8
View File
@@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf)
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000),
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
_prom(prom_buf)
{
}
@@ -134,7 +134,6 @@ int
MS5611_SPI::init()
{
int ret;
irqstate_t flags;
ret = SPI::init();
if (ret != OK) {
@@ -167,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count)
uint8_t b[4];
uint32_t w;
} *cvt = (_cvt *)data;
uint8_t buf[4];
uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
/* read the most recent measurement */
buf[0] = 0 | DIR_WRITE;
int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
if (ret == OK) {
@@ -238,21 +236,31 @@ MS5611_SPI::_read_prom()
usleep(3000);
/* read and convert PROM words */
bool all_zero = true;
for (int i = 0; i < 8; i++) {
uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
_prom.c[i] = _reg16(cmd);
if (_prom.c[i] != 0)
all_zero = false;
//debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
}
/* calculate CRC and return success/failure accordingly */
return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
if (ret != OK) {
debug("crc failed");
}
if (all_zero) {
debug("prom all zero");
ret = -EIO;
}
return ret;
}
uint16_t
MS5611_SPI::_reg16(unsigned reg)
{
uint8_t cmd[3];
cmd[0] = reg | DIR_READ;
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
_transfer(cmd, cmd, sizeof(cmd));
src/drivers/px4fmu/fmu.cpp
+1 -19
View File
@@ -69,7 +69,6 @@
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -123,7 +122,6 @@ private:
int _t_actuators;
int _t_actuator_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@@ -220,7 +218,6 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@@ -471,13 +468,6 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@@ -550,7 +540,7 @@ PX4FMU::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
@@ -599,13 +589,6 @@ PX4FMU::task_main()
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
@@ -670,7 +653,6 @@ PX4FMU::task_main()
}
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed);
/* make sure servos are off */
src/drivers/px4io/px4io.cpp
+148 -63
View File
@@ -54,6 +54,7 @@
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <crc32.h>
#include <arch/board/board.h>
@@ -72,7 +73,6 @@
#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
@@ -95,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2)
#define PX4IO_CHECK_CRC _IOC(0xff00, 3)
#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz
#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz
@@ -235,6 +237,7 @@ private:
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
@@ -242,7 +245,9 @@ private:
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
perf_counter_t _perf_update; ///< local performance counter
perf_counter_t _perf_update; ///<local performance counter for status updates
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
/* cached IO state */
uint16_t _status; ///< Various IO status flags
@@ -260,14 +265,12 @@ private:
/* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///< mixed outputs
actuator_controls_effective_s _controls_effective; ///< effective controls
actuator_outputs_s _outputs; ///<mixed outputs
bool _primary_pwm_device; ///< true if we are the default PWM output
@@ -335,11 +338,6 @@ private:
*/
int io_publish_raw_rc();
/**
* Fetch and publish the mixed control values.
*/
int io_publish_mixed_controls();
/**
* Fetch and publish the PWM servo outputs.
*/
@@ -467,11 +465,14 @@ PX4IO::PX4IO(device::Device *interface) :
_max_transfer(16), /* sensible default */
_update_interval(0),
_rc_handling_disabled(false),
_rc_chan_count(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
_thread_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
_status(0),
_alarms(0),
_t_actuator_controls_0(-1),
@@ -483,7 +484,6 @@ PX4IO::PX4IO(device::Device *interface) :
_t_param(-1),
_t_vehicle_command(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_to_servorail(0),
@@ -863,8 +863,7 @@ PX4IO::task_main()
/* get raw R/C input from IO */
io_publish_raw_rc();
/* fetch mixed servo controls and PWM outputs from IO */
io_publish_mixed_controls();
/* fetch PWM outputs from IO */
io_publish_pwm_outputs();
}
@@ -924,7 +923,23 @@ PX4IO::task_main()
/* re-upload RC input config as it may have changed */
io_set_rc_config();
/* re-set the battery scaling */
int32_t voltage_scaling_val;
param_t voltage_scaling_param;
/* set battery voltage scaling */
param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
/* send scaling voltage to IO */
uint16_t scaling = voltage_scaling_val;
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
if (pret != OK) {
log("voltage scaling upload failed");
}
}
}
perf_end(_perf_update);
@@ -1108,7 +1123,12 @@ PX4IO::io_set_rc_config()
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 3;
ichan = 4;
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 4;
ichan = 5;
for (unsigned i = 0; i < _max_rc_input; i++)
if (input_map[i] == -1)
@@ -1386,6 +1406,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*/
channel_count = regs[0];
if (channel_count != _rc_chan_count)
perf_count(_perf_chan_count);
_rc_chan_count = channel_count;
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@@ -1440,50 +1465,6 @@ PX4IO::io_publish_raw_rc()
return OK;
}
int
PX4IO::io_publish_mixed_controls()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* if not taking raw PPM from us, must be mixing */
if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
return OK;
/* data we are going to fetch */
actuator_controls_effective_s controls_effective;
controls_effective.timestamp = hrt_absolute_time();
/* get actuator controls from IO */
uint16_t act[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
if (ret != OK)
return ret;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
/* laxily advertise on first publication */
if (_to_actuators_effective == 0) {
_to_actuators_effective =
orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
&controls_effective);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
_to_actuators_effective, &controls_effective);
}
return OK;
}
int
PX4IO::io_publish_pwm_outputs()
{
@@ -1736,11 +1717,13 @@ void
PX4IO::print_status()
{
/* basic configuration */
printf("protocol %u hardware %u bootloader %u buffer %uB\n",
printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1));
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
@@ -2223,6 +2206,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
break;
case PX4IO_REBOOT_BOOTLOADER:
if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
return -EINVAL;
/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
// we don't expect a reply from this operation
ret = OK;
break;
case PX4IO_CHECK_CRC: {
/* check IO firmware CRC against passed value */
uint32_t io_crc = 0;
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
if (ret != OK)
return ret;
if (io_crc != arg) {
debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
return -EINVAL;
}
break;
}
case PX4IO_INAIR_RESTART_ENABLE:
/* set/clear the 'in-air restart' bit */
@@ -2253,7 +2259,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
count = _max_actuators;
if (count > 0) {
perf_begin(_perf_write);
int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
perf_end(_perf_write);
if (ret != OK)
return ret;
@@ -2332,7 +2341,7 @@ void
start(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(1, "already loaded");
errx(0, "already loaded");
/* allocate the interface */
device::Device *interface = get_interface();
@@ -2665,6 +2674,39 @@ px4io_main(int argc, char *argv[])
if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
}
if (!strcmp(argv[1], "forceupdate")) {
/*
force update of the IO firmware without requiring
the user to hold the safety switch down
*/
if (argc <= 3) {
warnx("usage: px4io forceupdate MAGIC filename");
exit(1);
}
if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade");
} else {
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
}
// tear down the px4io instance
delete g_dev;
}
// upload the specified firmware
const char *fn[2];
fn[0] = argv[3];
fn[1] = nullptr;
PX4IO_Uploader *up = new PX4IO_Uploader;
up->upload(&fn[0]);
delete up;
exit(0);
}
/* commands below here require a started driver */
if (g_dev == nullptr)
@@ -2750,6 +2792,49 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "checkcrc")) {
/*
check IO CRC against CRC of a file
*/
if (argc <= 2) {
printf("usage: px4io checkcrc filename\n");
exit(1);
}
if (g_dev == nullptr) {
printf("px4io is not started\n");
exit(1);
}
int fd = open(argv[2], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[2], errno);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
uint32_t nbytes = 0;
while (true) {
uint8_t buf[16];
int n = read(fd, buf, sizeof(buf));
if (n <= 0) break;
fw_crc = crc32part(buf, n, fw_crc);
nbytes += n;
}
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
}
printf("CRCs match\n");
exit(0);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
@@ -2767,5 +2852,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
}
src/drivers/px4io/px4io_uploader.cpp
+4 -1
View File
@@ -274,7 +274,10 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 1000);
// the small recv timeout here is to allow for fast
// drain when rebooting the io board for a forced
// update of the fw without using the safety switch
ret = recv(c, 40);
#ifdef UDEBUG
if (ret == OK) {
src/drivers/stm32/drv_hrt.c
+24 -1
View File
@@ -733,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqstate_t flags = irqsave();
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
@@ -839,7 +846,12 @@ hrt_call_invoke(void)
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
call->deadline = deadline + call->period;
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
}
hrt_call_enter(call);
}
}
@@ -906,5 +918,16 @@ hrt_latency_update(void)
latency_counters[index]++;
}
void
hrt_call_init(struct hrt_call *entry)
{
memset(entry, 0, sizeof(*entry));
}
void
hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
}
#endif /* HRT_TIMER */
src/lib/external_lgpl/tecs/tecs.cpp
+1 -1
View File
@@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) < 0.01f) {
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
_throttle_dem = constrain(_throttle_dem,
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+8
View File
@@ -46,6 +46,10 @@ namespace math
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
if (_cutoff_freq <= 0.0f) {
// no filtering
return;
}
float fr = sample_freq/_cutoff_freq;
float ohm = tanf(M_PI_F/fr);
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
float LowPassFilter2p::apply(float sample)
{
if (_cutoff_freq <= 0.0f) {
// no filtering
return sample;
}
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
src/modules/attitude_estimator_so3/README
+3
View File
@@ -0,0 +1,3 @@
Synopsis
nsh> attitude_estimator_so3_comp start
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp → src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+171 -328
View File
@@ -1,16 +1,49 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
/****************************************************************************
*
* @file attitude_estimator_so3_comp_main.c
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 attitude_estimator_so3_main.cpp
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
@@ -46,94 +79,91 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "attitude_estimator_so3_comp_params.h"
#include "attitude_estimator_so3_params.h"
#ifdef __cplusplus
}
#endif
extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */
static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */
//! Auxiliary variables to reduce number of repeated operations
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
static bool bFilterInit = false;
//! Auxiliary variables to reduce number of repeated operations
static float q0q0, q0q1, q0q2, q0q3;
static float q1q1, q1q2, q1q3;
static float q2q2, q2q3;
static float q3q3;
//! Serial packet related
static int uart;
static int baudrate;
static bool bFilterInit = false;
/**
* Mainloop of attitude_estimator_so3_comp.
* Mainloop of attitude_estimator_so3.
*/
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
int attitude_estimator_so3_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Function prototypes */
float invSqrt(float number);
void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
exit(1);
}
/**
* The attitude_estimator_so3_comp app only briefly exists to start
* The attitude_estimator_so3 app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int attitude_estimator_so3_comp_main(int argc, char *argv[])
int attitude_estimator_so3_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("attitude_estimator_so3_comp already running\n");
warnx("already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12400,
attitude_estimator_so3_comp_thread_main,
(const char **)argv);
14000,
attitude_estimator_so3_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
while(thread_running){
while (thread_running){
usleep(200000);
printf(".");
}
printf("terminated.");
warnx("stopped");
exit(0);
}
@@ -157,7 +187,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float number) {
float invSqrt(float number)
{
volatile long i;
volatile float x, y;
volatile const float f = 1.5F;
@@ -221,48 +252,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa
q3q3 = q3 * q3;
}
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
float recipNorm;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
//! Make filter converge to initial solution faster
//! This function assumes you are in static position.
//! WARNING : in case air reboot, this can cause problem. But this is very
//! unlikely happen.
if(bFilterInit == false)
{
// Make filter converge to initial solution faster
// This function assumes you are in static position.
// WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
if(bFilterInit == false) {
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = true;
}
//! If magnetometer measurement is available, use it.
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
float hx, hy, hz, bx, bz;
float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement
// Will sqrt work better? PX4 system is powerful enough?
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy);
bz = hz;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy);
bz = hz;
// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx);
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
@@ -293,7 +323,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt;
gx += gyro_bias[0]; // apply integral feedback
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1];
gz += gyro_bias[2];
}
@@ -337,208 +369,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
q3 *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
void send_uart_byte(char c)
{
write(uart,&c,1);
}
void send_uart_bytes(uint8_t *data, int length)
{
write(uart,data,(size_t)(sizeof(uint8_t)*length));
}
void send_uart_float(float f) {
uint8_t * b = (uint8_t *) &f;
//! Assume float is 4-bytes
for(int i=0; i<4; i++) {
uint8_t b1 = (b[i] >> 4) & 0x0f;
uint8_t b2 = (b[i] & 0x0f);
uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
send_uart_bytes(&c1,1);
send_uart_bytes(&c2,1);
}
}
void send_uart_float_arr(float *arr, int length)
{
for(int i=0;i<length;++i)
{
send_uart_float(arr[i]);
send_uart_byte(',');
}
}
int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL;
}
printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
*is_usb = false;
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
} else {
*is_usb = true;
}
return uart;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
/*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
*/
/*
* EKF Attitude Estimator main function.
* Nonliner complementary filter on SO(3), attitude estimator main function.
*
* Estimates the attitude recursively once started.
* Estimates the attitude once started.
*
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
int attitude_estimator_so3_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Serial debug related
int ch;
struct termios uart_config_original;
bool usb_uart;
bool debug_mode = false;
char *device_name = "/dev/ttyS2";
baudrate = 115200;
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Time constant
float dt = 0.005f;
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
/* Initialization */
float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */
float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f};
float mag[3] = {0.0f, 0.0f, 0.0f};
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
//! -d <device_name>, default : /dev/ttyS2
//! -b <baud_rate>, default : 115200
while ((ch = getopt(argc,argv,"d:b:")) != EOF){
switch(ch){
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if(baudrate == 0)
printf("invalid baud rate '%s'",optarg);
break;
case 'd':
device_name = optarg;
debug_mode = true;
break;
default:
usage("invalid argument");
}
}
if(debug_mode){
printf("Opening debugging port for 3D visualization\n");
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
printf("could not open %s", device_name);
else
printf("Open port success\n");
}
// print text
printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
warnx("main thread started");
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
@@ -555,8 +422,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 4);
/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 3);
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -565,17 +432,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
//orb_advert_t att_pub = -1;
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
@@ -583,20 +448,22 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_comp_params so3_comp_params;
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
struct attitude_estimator_so3_params so3_comp_params;
struct attitude_estimator_so3_param_handles so3_comp_param_handles;
/* initialize parameter handles */
parameters_init(&so3_comp_param_handles);
parameters_update(&so3_comp_param_handles, &so3_comp_params);
uint64_t start_time = hrt_absolute_time();
bool initialized = false;
bool state_initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* register the perf counter */
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
/* Main loop*/
while (!thread_should_exit) {
@@ -615,12 +482,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
warnx("WARNING: Not getting sensors - sensor app running?");
}
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -644,11 +508,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) {
if (hrt_absolute_time() > start_time + 3000000l) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
}
} else {
@@ -668,9 +533,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
sensor_last_timestamp[0] = raw.timestamp;
}
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -696,31 +561,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
mag[1] = raw.magnetometer_ga[1];
mag[2] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
dt = 0.005f;
parameters_update(&so3_comp_param_handles, &so3_comp_params);
const_initialized = true;
if (!state_initialized && dt < 0.05f && dt > 0.001f) {
state_initialized = true;
warnx("state initialized");
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
if (!state_initialized) {
continue;
}
@@ -728,18 +576,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
-acc[0], -acc[1], -acc[2],
mag[0], mag[1], mag[2],
so3_comp_params.Kp,
so3_comp_params.Ki,
dt);
// Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation.
//Equation (290)
@@ -747,29 +600,42 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw
euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
/* Do something */
// Publish only finite euler angles
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
// Don't publish anything
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
if (last_data > 0 && raw.timestamp > last_data + 12000) {
warnx("sensor data missed");
}
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
// Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
//! Euler angle rate. But it needs to be investigated again.
// Euler angle rate. But it needs to be investigated again.
/*
att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
@@ -783,53 +649,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.pitchacc = 0;
att.yawacc = 0;
//! Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
/* TODO: Bias estimation required */
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(float)*9);
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
// Publish
if (att_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
orb_advertise(ORB_ID(vehicle_attitude), &att);
}
perf_end(so3_comp_loop_perf);
//! This will print out debug packet to visualization software
if(debug_mode)
{
float quat[4];
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
send_uart_float_arr(quat,4);
send_uart_byte('\n');
}
}
}
}
loopcounter++;
}// while
}
thread_running = false;
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
return 0;
}
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c
Executable
+86
View File
@@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 attitude_estimator_so3_params.c
*
* Parameters for nonlinear complementary filters on the SO(3).
*/
#include "attitude_estimator_so3_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("SO3_ROLL_OFFS");
h->pitch_off = param_find("SO3_PITCH_OFFS");
h->yaw_off = param_find("SO3_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h
Executable
+67
View File
@@ -0,0 +1,67 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 attitude_estimator_so3_params.h
*
* Parameters for nonlinear complementary filters on the SO(3).
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);
src/modules/attitude_estimator_so3/module.mk
+8
View File
@@ -0,0 +1,8 @@
#
# Attitude estimator (Nonlinear SO(3) complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3
SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c
src/modules/attitude_estimator_so3_comp/README
-5
View File
@@ -1,5 +0,0 @@
Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
Option -d is for debugging packet. See code for detailed packet structure.
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
-63
View File
@@ -1,63 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include "attitude_estimator_so3_comp_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
-44
View File
@@ -1,44 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.h
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_comp_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_comp_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
src/modules/attitude_estimator_so3_comp/module.mk
-8
View File
@@ -1,8 +0,0 @@
#
# Attitude estimator (Nonlinear SO3 complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3_comp
SRCS = attitude_estimator_so3_comp_main.cpp \
attitude_estimator_so3_comp_params.c
src/modules/commander/commander.cpp
+6 -5
View File
@@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) {
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
@@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
// TODO AUTO_LAND handling
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
if (local_pos->z > -takeoff_alt || status->condition_landed) {
// XXX: only respect the condition_landed when the local position is actually valid
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
return TRANSITION_NOT_CHANGED;
}
}
@@ -1549,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */
if (local_pos->z > -takeoff_alt || status->condition_landed) {
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res;
}
@@ -1597,8 +1598,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
/* switch to failsafe mode */
bool manual_control_old = control_mode->flag_control_manual_enabled;
if (!status->condition_landed) {
/* in air: try to hold position */
if (!status->condition_landed && status->condition_local_position_valid) {
/* in air: try to hold position if possible */
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
} else {
src/modules/fw_att_control/fw_att_control_params.c
+2 -2
View File
@@ -33,9 +33,9 @@
****************************************************************************/
/**
* @file fw_pos_control_l1_params.c
* @file fw_att_control_params.c
*
* Parameters defined by the L1 position control task
* Parameters defined by the fixed-wing attitude control task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
src/modules/mavlink/mavlink_receiver.cpp
+4 -7
View File
@@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg)
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_counter = hil_counter;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -401,6 +400,7 @@ handle_message(mavlink_message_t *msg)
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
hil_sensors.accelerometer_counter = hil_counter;
/* adc */
hil_sensors.adc_voltage_v[0] = 0.0f;
@@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg)
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
@@ -419,15 +418,13 @@ handle_message(mavlink_message_t *msg)
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
hil_sensors.magnetometer_counter = hil_counter;
/* baro */
hil_sensors.baro_pres_mbar = imu.abs_pressure;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.accelerometer_counter = hil_counter;
hil_sensors.baro_counter = hil_counter;
/* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
src/modules/mavlink/orb_listener.c
+29 -65
View File
@@ -54,6 +54,7 @@
#include <sys/prctl.h>
#include <stdlib.h>
#include <poll.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
@@ -72,7 +73,6 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct airspeed_s airspeed;
@@ -119,7 +119,6 @@ static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
static void l_vehicle_attitude_controls_effective(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l);
@@ -147,7 +146,6 @@ static const struct listener listeners[] = {
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
@@ -242,16 +240,29 @@ l_vehicle_attitude(const struct listener *l)
att.rollspeed,
att.pitchspeed,
att.yawspeed);
/* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
}
/* send quaternion values if it exists */
if(att.q_valid) {
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
att.q[0],
att.q[1],
att.q[2],
att.q[3],
att.rollspeed,
att.pitchspeed,
att.yawspeed);
}
}
attitude_counter++;
@@ -266,13 +277,7 @@ l_vehicle_gps_position(const struct listener *l)
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad;
if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F;
cog_deg *= M_RAD_TO_DEG_F;
float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
@@ -365,28 +370,16 @@ l_global_position(const struct listener *l)
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp;
int32_t lat = global_pos.lat;
int32_t lon = global_pos.lon;
int32_t alt = (int32_t)(global_pos.alt * 1000);
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
int16_t vx = (int16_t)(global_pos.vx * 100.0f);
int16_t vy = (int16_t)(global_pos.vy * 100.0f);
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
/* heading in degrees * 10, from 0 to 36.000) */
uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
timestamp / 1000,
lat,
lon,
alt,
relative_alt,
vx,
vy,
vz,
hdg);
global_pos.timestamp / 1000,
global_pos.lat,
global_pos.lon,
global_pos.alt * 1000.0f,
global_pos.relative_alt * 1000.0f,
global_pos.vx * 100.0f,
global_pos.vy * 100.0f,
global_pos.vz * 100.0f,
_wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
void
@@ -424,8 +417,8 @@ l_global_position_setpoint(const struct listener *l)
coordinate_frame,
global_sp.lat,
global_sp.lon,
global_sp.altitude,
global_sp.yaw);
global_sp.altitude * 1000.0f,
global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
}
void
@@ -603,32 +596,6 @@ l_manual_control_setpoint(const struct listener *l)
0);
}
void
l_vehicle_attitude_controls_effective(const struct listener *l)
{
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
actuators_effective_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
actuators_effective_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
actuators_effective_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
actuators_effective_0.control_effective[3]);
}
}
void
l_vehicle_attitude_controls(const struct listener *l)
{
@@ -839,9 +806,6 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
src/modules/position_estimator_inav/position_estimator_inav_main.c
+5 -6
View File
@@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
errx(1, "subscriptions poll error on init.");
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
} else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_counter > baro_counter) {
if (wait_baro && sensor.baro_counter != baro_counter) {
baro_counter = sensor.baro_counter;
/* mean calculation over several measurements */
@@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
warnx("subscriptions poll error.");
thread_should_exit = true;
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
continue;
} else if (ret > 0) {
@@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accel_counter) {
if (sensor.accelerometer_counter != accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2];
@@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
accel_updates++;
}
if (sensor.baro_counter > baro_counter) {
if (sensor.baro_counter != baro_counter) {
baro_corr = - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
src/modules/px4iofirmware/controls.c
+17 -4
View File
@@ -94,6 +94,9 @@ controls_tick() {
* other. Don't do that.
*/
/* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
uint16_t rssi = 0;
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
@@ -104,14 +107,15 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
rssi = 1000;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
r_raw_rc_count = 8;
}
perf_end(c_gather_sbus);
@@ -122,10 +126,19 @@ controls_tick() {
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
if (ppm_updated)
if (ppm_updated) {
/* XXX sample RSSI properly here */
rssi = 1000;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_INPUT_CHANNELS;
/*
* In some cases we may have received a frame, but input has still
* been lost.
@@ -221,7 +234,7 @@ controls_tick() {
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if (assigned_channels == 0) {
if (assigned_channels == 0 || rssi == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag */
src/modules/px4iofirmware/mixer.cpp
+5 -1
View File
@@ -195,7 +195,7 @@ mixer_tick(void)
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
@@ -211,6 +211,10 @@ mixer_tick(void)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
src/modules/px4iofirmware/protocol.h
+6
View File
@@ -128,6 +128,7 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -190,6 +191,11 @@ enum { /* DSM bind states */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
src/modules/px4iofirmware/px4io.c
+20
View File
@@ -45,6 +45,7 @@
#include <string.h>
#include <poll.h>
#include <signal.h>
#include <crc32.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
@@ -124,6 +125,22 @@ heartbeat_blink(void)
LED_BLUE(heartbeat = !heartbeat);
}
static void
calculate_fw_crc(void)
{
#define APP_SIZE_MAX 0xf000
#define APP_LOAD_ADDRESS 0x08001000
// compute CRC of the current firmware
uint32_t sum = 0;
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
}
r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
}
int
user_start(int argc, char *argv[])
{
@@ -136,6 +153,9 @@ user_start(int argc, char *argv[])
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
src/modules/px4iofirmware/px4io.h
+1 -1
View File
@@ -213,7 +213,7 @@ extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
src/modules/px4iofirmware/registers.c
+27
View File
@@ -45,6 +45,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/systemlib.h>
#include <stm32_pwr.h>
#include "px4io.h"
#include "protocol.h"
@@ -154,6 +156,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
@@ -501,6 +505,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
// don't allow reboot while armed
break;
}
// check the magic value
if (value != PX4IO_REBOOT_BL_MAGIC)
break;
// note that we don't set BL_WAIT_MAGIC in
// BKP_DR1 as that is not necessary given the
// timing of the forceupdate command. The
// bootloader on px4io waits for enough time
// anyway, and this method works with older
// bootloader versions (tested with both
// revision 3 and revision 4).
up_systemreset();
break;
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;
src/modules/px4iofirmware/sbus.c
+48 -17
View File
@@ -54,6 +54,27 @@
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
/*
Measured values with Futaba FX-30/R6108SB:
-+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
*/
/* define range mapping here, -+100% -> 1000..2000 */
#define SBUS_RANGE_MIN 200.0f
#define SBUS_RANGE_MAX 1800.0f
#define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
static int sbus_fd = -1;
@@ -66,7 +87,7 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
int
sbus_init(const char *device)
@@ -97,7 +118,7 @@ sbus_init(const char *device)
}
bool
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
* decode it.
*/
partial_frame_count = 0;
return sbus_decode(now, values, num_values, max_channels);
return sbus_decode(now, values, num_values, rssi, max_channels);
}
/*
@@ -194,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values)
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
return false;
}
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
if ((frame[23] & (1 << 2)) && /* signal lost */
(frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */
*values = 0;
return false;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
@@ -234,8 +246,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
values[channel] = (value / 2) + 998;
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
@@ -243,13 +256,31 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
chancount = 18;
/* channel 17 (index 16) */
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
*num_values = chancount;
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
*rssi = 0;
return false;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
}
*rssi = 1000;
return true;
}
src/modules/sdlog2/sdlog2.c
+3 -18
View File
@@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
@@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[])
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_effective_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
@@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* Sanity check variable and index */
/* number of subscriptions */
const ssize_t fdsc = 19;
/* sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE --- */
subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.act_controls_effective_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
@@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- ACTUATOR CONTROL EFFECTIVE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
// TODO not implemented yet
}
/* --- LOCAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
src/modules/sensors/sensor_params.c
+5
View File
@@ -195,6 +195,7 @@ 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 */
PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
#else
@@ -226,3 +227,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
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 */
src/modules/sensors/sensors.cpp
+28
View File
@@ -260,6 +260,10 @@ private:
float rc_scale_yaw;
float rc_scale_flaps;
int rc_fs_ch;
int rc_fs_mode;
float rc_fs_thr;
float battery_voltage_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -305,6 +309,10 @@ private:
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t rc_fs_ch;
param_t rc_fs_mode;
param_t rc_fs_thr;
param_t battery_voltage_scaling;
param_t board_rotation;
@@ -517,6 +525,11 @@ Sensors::Sensors() :
_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* RC failsafe */
_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
@@ -668,6 +681,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -1256,6 +1272,18 @@ Sensors::rc_poll()
if (rc_input.channel_count < 4)
return;
/* failsafe check */
if (_parameters.rc_fs_ch != 0) {
if (_parameters.rc_fs_mode == 0) {
if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
return;
} else if (_parameters.rc_fs_mode == 1) {
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
return;
}
}
unsigned channel_limit = rc_input.channel_count;
if (channel_limit > _rc_max_chan_count)
src/modules/systemlib/perf_counter.c
+2 -1
View File
@@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle)
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
handle->name,
pce->event_count,
pce->time_total,
pce->time_total / pce->event_count,
pce->time_least,
pce->time_most);
break;
src/modules/uORB/objects_common.cpp
-7
View File
@@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
/* actuator controls, as set by actuators / mixers after limiting */
#include "topics/actuator_controls_effective.h"
ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
#include "topics/actuator_outputs.h"
ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
src/modules/uORB/topics/actuator_controls_effective.h
+30 -30
View File
@@ -46,34 +46,34 @@
#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
#include <stdint.h>
#include "../uORB.h"
#include "actuator_controls.h"
//#include <stdint.h>
//#include "../uORB.h"
//#include "actuator_controls.h"
//
//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
//
///**
// * @addtogroup topics
// * @{
// */
//
//struct actuator_controls_effective_s {
// uint64_t timestamp;
// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
//};
//
///**
// * @}
// */
//
///* actuator control sets; this list can be expanded as more controllers emerge */
//ORB_DECLARE(actuator_controls_effective_0);
//ORB_DECLARE(actuator_controls_effective_1);
//ORB_DECLARE(actuator_controls_effective_2);
//ORB_DECLARE(actuator_controls_effective_3);
//
///* control sets with pre-defined applications */
//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0)
#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
/**
* @addtogroup topics
* @{
*/
struct actuator_controls_effective_s {
uint64_t timestamp;
float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
};
/**
* @}
*/
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_effective_0);
ORB_DECLARE(actuator_controls_effective_1);
ORB_DECLARE(actuator_controls_effective_2);
ORB_DECLARE(actuator_controls_effective_3);
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0)
#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
src/systemcmds/preflight_check/preflight_check.c
+1 -1
View File
@@ -200,7 +200,7 @@ system_eval:
led_toggle(leds, LED_BLUE);
/* display and sound error */
for (int i = 0; i < 50; i++)
for (int i = 0; i < 14; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);
src/systemcmds/tests/module.mk
+2 -1
View File
@@ -27,4 +27,5 @@ SRCS = test_adc.c \
test_file.c \
tests_main.c \
test_param.c \
test_ppm_loopback.c
test_ppm_loopback.c \
test_rc.c
src/systemcmds/tests/test_rc.c
+146
View File
@@ -0,0 +1,146 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_ppm_loopback.c
* Tests the PWM outputs and PPM input
*
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <systemlib/err.h>
#include "tests.h"
#include <math.h>
#include <float.h>
int test_rc(int argc, char *argv[])
{
int _rc_sub = orb_subscribe(ORB_ID(input_rc));
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
struct rc_input_values rc_input;
struct rc_input_values rc_last;
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
usleep(100000);
/* open PPM input and expect values close to the output values */
bool rc_updated;
orb_check(_rc_sub, &rc_updated);
warnx("Reading PPM values - press any key to abort");
warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes.");
if (rc_updated) {
/* copy initial set */
for (unsigned i = 0; i < rc_input.channel_count; i++) {
rc_last.values[i] = rc_input.values[i];
}
rc_last.channel_count = rc_input.channel_count;
/* poll descriptor */
struct pollfd fds[2];
fds[0].fd = _rc_sub;
fds[0].events = POLLIN;
fds[1].fd = 0;
fds[1].events = POLLIN;
while (true) {
int ret = poll(fds, 2, 200);
if (ret > 0) {
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
/* go and check values */
for (unsigned i = 0; i < rc_input.channel_count; i++) {
if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) {
warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]);
(void)close(_rc_sub);
return ERROR;
}
rc_last.values[i] = rc_input.values[i];
}
if (rc_last.channel_count != rc_input.channel_count) {
warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count);
(void)close(_rc_sub);
return ERROR;
}
if (hrt_absolute_time() - rc_input.timestamp > 100000) {
warnx("TIMEOUT, less than 10 Hz updates");
(void)close(_rc_sub);
return ERROR;
}
} else {
/* key pressed, bye bye */
return 0;
}
}
}
} else {
warnx("failed reading RC input data");
return ERROR;
}
warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!");
return 0;
}
src/systemcmds/tests/test_sensors.c
+51 -36
View File
@@ -78,7 +78,8 @@ static int accel(int argc, char *argv[]);
static int gyro(int argc, char *argv[]);
static int mag(int argc, char *argv[]);
static int baro(int argc, char *argv[]);
static int mpu6k(int argc, char *argv[]);
static int accel1(int argc, char *argv[]);
static int gyro1(int argc, char *argv[]);
/****************************************************************************
* Private Data
@@ -93,7 +94,8 @@ struct {
{"gyro", "/dev/gyro", gyro},
{"mag", "/dev/mag", mag},
{"baro", "/dev/baro", baro},
{"mpu6k", "/dev/mpu6k", mpu6k},
{"accel1", "/dev/accel1", accel1},
{"gyro1", "/dev/gyro1", gyro1},
{NULL, NULL, NULL}
};
@@ -137,7 +139,7 @@ accel(int argc, char *argv[])
}
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
warnx("MPU6K acceleration values out of range!");
warnx("ACCEL1 acceleration values out of range!");
return ERROR;
}
@@ -149,20 +151,19 @@ accel(int argc, char *argv[])
}
static int
mpu6k(int argc, char *argv[])
accel1(int argc, char *argv[])
{
printf("\tMPU6K: test start\n");
printf("\tACCEL1: test start\n");
fflush(stdout);
int fd;
struct accel_report buf;
struct gyro_report gyro_buf;
int ret;
fd = open("/dev/accel_mpu6k", O_RDONLY);
fd = open("/dev/accel1", O_RDONLY);
if (fd < 0) {
printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n");
return ERROR;
}
@@ -173,45 +174,21 @@ mpu6k(int argc, char *argv[])
ret = read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tMPU6K: read1 fail (%d)\n", ret);
printf("\tACCEL1: read1 fail (%d)\n", ret);
return ERROR;
} else {
printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
warnx("MPU6K acceleration values out of range!");
warnx("ACCEL1 acceleration values out of range!");
return ERROR;
}
/* Let user know everything is ok */
printf("\tOK: MPU6K ACCEL passed all tests successfully\n");
printf("\tOK: ACCEL1 passed all tests successfully\n");
close(fd);
fd = open("/dev/gyro_mpu6k", O_RDONLY);
if (fd < 0) {
printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = read(fd, &gyro_buf, sizeof(gyro_buf));
if (ret != sizeof(gyro_buf)) {
printf("\tMPU6K GYRO: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z);
}
/* Let user know everything is ok */
printf("\tOK: MPU6K GYRO passed all tests successfully\n");
close(fd);
return OK;
@@ -255,6 +232,44 @@ gyro(int argc, char *argv[])
return OK;
}
static int
gyro1(int argc, char *argv[])
{
printf("\tGYRO1: test start\n");
fflush(stdout);
int fd;
struct gyro_report buf;
int ret;
fd = open("/dev/gyro1", O_RDONLY);
if (fd < 0) {
printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tGYRO1: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
/* Let user know everything is ok */
printf("\tOK: GYRO1 passed all tests successfully\n");
close(fd);
return OK;
}
static int
mag(int argc, char *argv[])
{
src/systemcmds/tests/tests.h
+1
View File
@@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]);
extern int test_bson(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]);
__END_DECLS
src/systemcmds/tests/tests_main.c
+1
View File
@@ -105,6 +105,7 @@ const struct {
{"bson", test_bson, 0},
{"file", test_file, 0},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0}
};