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

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-28 23:05:58 +02:00
parent c0dc9f99a6 700594e743
commit 361f858c14
137 changed files with 3716 additions and 2380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+45 -29
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,8 +32,9 @@
****************************************************************************/
/**
* @file ets_airspeed.cpp
* @author Simon Wilks
* @file airspeed.cpp
* @author Simon Wilks <simon@px4.io>
* @author Lorenz Meier <lorenz@px4.io>
*
* Driver for the Eagle Tree Airspeed V3 connected via I2C.
*/
@@ -76,7 +77,7 @@
#include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, 100000),
_reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
@@ -105,12 +106,14 @@ Airspeed::~Airspeed()
/* make sure we are truly inactive */
stop();
if (_class_instance != -1)
if (_class_instance != -1) {
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
}
/* free any existing reports */
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
// free perf counters
perf_free(_sample_perf);
@@ -124,13 +127,15 @@ Airspeed::init()
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
@@ -146,8 +151,9 @@ Airspeed::init()
/* measurement will have generated a report, publish */
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub == nullptr)
if (_airspeed_pub == nullptr) {
warnx("uORB started?");
}
}
ret = OK;
@@ -166,7 +172,7 @@ Airspeed::probe()
_retries = 4;
int ret = measure();
// drop back to 2 retries once initialised
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}
@@ -179,20 +185,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
@@ -202,13 +208,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@@ -217,15 +224,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval))
if (ticks < USEC2TICK(_conversion_interval)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
@@ -233,21 +242,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
@@ -261,16 +274,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
_diff_pres_offset = s->offset_pa;
return OK;
struct airspeed_scale *s = (struct airspeed_scale *)arg;
_diff_pres_offset = s->offset_pa;
return OK;
}
case AIRSPEEDIOCGSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
struct airspeed_scale *s = (struct airspeed_scale *)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
}
default:
@@ -287,8 +300,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
@@ -369,6 +383,7 @@ Airspeed::update_status()
if (_subsys_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
@@ -402,6 +417,7 @@ Airspeed::print_info()
void
Airspeed::new_report(const differential_pressure_s &report)
{
if (!_reports->force(&report))
if (!_reports->force(&report)) {
perf_count(_buffer_overflows);
}
}
src/drivers/device/vdev.cpp
+3 -2
View File
@@ -367,6 +367,8 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
/* yes? post the notification */
if (fds->revents != 0)
sem_post(fds->sem);
} else {
PX4_WARN("Store Poll Waiter error.");
}
} else {
@@ -524,5 +526,4 @@ const char *VDev::devList(unsigned int *next)
return NULL;
}
} // namespace device
} // namespace device
src/drivers/device/vdev.h
+7 -7
View File
@@ -324,6 +324,13 @@ public:
static const char *devList(unsigned int *next);
static const char *topicList(unsigned int *next);
/**
* Get the device name.
*
* @return the file system string of the device handle
*/
const char* get_devname() { return _devname; }
protected:
int register_driver(const char *name, void *data);
@@ -406,13 +413,6 @@ protected:
*/
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
/**
* Get the device name.
*
* @return the file system string of the device handle
*/
const char* get_devname() { return _devname; }
bool _pub_blocked; /**< true if publishing should be blocked */
private:
src/drivers/drv_accel.h
+2 -28
View File
@@ -51,29 +51,8 @@
#define ACCEL1_DEVICE_PATH "/dev/accel1"
#define ACCEL2_DEVICE_PATH "/dev/accel2"
/**
* accel report structure. Reads from the device must be in multiples of this
* structure.
*/
struct accel_report {
uint64_t timestamp;
uint64_t integral_dt; /**< integration time */
uint64_t error_count;
float x; /**< acceleration in the NED X board axis in m/s^2 */
float y; /**< acceleration in the NED Y board axis in m/s^2 */
float z; /**< acceleration in the NED Z board axis in m/s^2 */
float x_integral; /**< velocity in the NED X board axis in m/s over the integration time frame */
float y_integral; /**< velocity in the NED Y board axis in m/s over the integration time frame */
float z_integral; /**< velocity in the NED Z board axis in m/s over the integration time frame */
float temperature; /**< temperature in degrees celsius */
float range_m_s2; /**< range in m/s^2 (+- this value) */
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
int16_t temperature_raw;
};
#include <uORB/topics/sensor_accel.h>
#define accel_report sensor_accel_s
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
struct accel_scale {
@@ -85,11 +64,6 @@ struct accel_scale {
float z_scale;
};
/*
* ObjDev tag for raw accelerometer data.
*/
ORB_DECLARE(sensor_accel);
/*
* ioctl() definitions
*
src/drivers/drv_baro.h
+2 -16
View File
@@ -50,22 +50,8 @@
#define BARO_BASE_DEVICE_PATH "/dev/baro"
#define BARO0_DEVICE_PATH "/dev/baro0"
/**
* baro report structure. Reads from the device must be in multiples of this
* structure.
*/
struct baro_report {
float pressure;
float altitude;
float temperature;
uint64_t timestamp;
uint64_t error_count;
};
/*
* ObjDev tag for raw barometer data.
*/
ORB_DECLARE(sensor_baro);
#include <uORB/topics/sensor_baro.h>
#define baro_report sensor_baro_s
/*
* ioctl() definitions
src/drivers/drv_gyro.h
+2 -28
View File
@@ -51,29 +51,8 @@
#define GYRO1_DEVICE_PATH "/dev/gyro1"
#define GYRO2_DEVICE_PATH "/dev/gyro2"
/**
* gyro report structure. Reads from the device must be in multiples of this
* structure.
*/
struct gyro_report {
uint64_t timestamp;
uint64_t integral_dt; /**< integration time */
uint64_t error_count;
float x; /**< angular velocity in the NED X board axis in rad/s */
float y; /**< angular velocity in the NED Y board axis in rad/s */
float z; /**< angular velocity in the NED Z board axis in rad/s */
float x_integral; /**< delta angle in the NED X board axis in rad/s in the integration time frame */
float y_integral; /**< delta angle in the NED Y board axis in rad/s in the integration time frame */
float z_integral; /**< delta angle in the NED Z board axis in rad/s in the integration time frame */
float temperature; /**< temperature in degrees celcius */
float range_rad_s;
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
int16_t temperature_raw;
};
#include <uORB/topics/sensor_gyro.h>
#define gyro_report sensor_gyro_s
/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
struct gyro_scale {
@@ -85,11 +64,6 @@ struct gyro_scale {
float z_scale;
};
/*
* ObjDev tag for raw gyro data.
*/
ORB_DECLARE(sensor_gyro);
/*
* ioctl() definitions
*/
src/drivers/drv_mag.h
+2 -26
View File
@@ -49,26 +49,8 @@
#define MAG1_DEVICE_PATH "/dev/mag1"
#define MAG2_DEVICE_PATH "/dev/mag2"
/**
* mag report structure. Reads from the device must be in multiples of this
* structure.
*
* Output values are in gauss.
*/
struct mag_report {
uint64_t timestamp;
uint64_t error_count;
float x;
float y;
float z;
float range_ga;
float scaling;
float temperature;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
};
#include <uORB/topics/sensor_mag.h>
#define mag_report sensor_mag_s
/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
struct mag_scale {
@@ -80,12 +62,6 @@ struct mag_scale {
float z_scale;
};
/*
* ObjDev tag for raw magnetometer data.
*/
ORB_DECLARE(sensor_mag);
/*
* ioctl() definitions
*/
src/drivers/drv_pwm_output.h
+8 -19
View File
@@ -61,10 +61,17 @@ __BEGIN_DECLS
#define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output"
#define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0"
#include <uORB/topics/output_pwm.h>
#define pwm_output_values output_pwm_s
#ifndef PWM_OUTPUT_MAX_CHANNELS
#define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS
#endif
/**
* Maximum number of PWM output channels supported by the device.
*/
#define PWM_OUTPUT_MAX_CHANNELS 16
//#define PWM_OUTPUT_MAX_CHANNELS 16
/**
* Lowest minimum PWM in us
@@ -107,19 +114,6 @@ __BEGIN_DECLS
*/
typedef uint16_t servo_position_t;
/**
* Servo output status structure.
*
* May be published to output_pwm, or written to a PWM output
* device.
*/
struct pwm_output_values {
/** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_CHANNELS];
unsigned channel_count;
};
/**
* RC config values for a channel
*
@@ -136,11 +130,6 @@ struct pwm_output_rc_config {
bool rc_reverse;
};
/*
* ORB tag for PWM outputs.
*/
ORB_DECLARE(output_pwm);
/*
* ioctl() definitions
*
src/drivers/drv_px4flow.h
-5
View File
@@ -46,11 +46,6 @@
#define PX4FLOW0_DEVICE_PATH "/dev/px4flow0"
/*
* ObjDev tag for px4flow data.
*/
ORB_DECLARE(optical_flow);
/*
* ioctl() definitions
*
src/drivers/drv_range_finder.h
-5
View File
@@ -48,11 +48,6 @@
#define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0"
#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus
/*
* ObjDev tag for distance sensor data.
*/
ORB_DECLARE(distance_sensor);
/*
* ioctl() definitions
*
src/drivers/drv_rc_input.h
+3 -88
View File
@@ -57,11 +57,6 @@
*/
#define RC_INPUT0_DEVICE_PATH "/dev/input_rc0"
/**
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
*/
#define RC_INPUT_MAX_CHANNELS 18
/**
* Maximum RSSI value
*/
@@ -82,10 +77,9 @@
*/
#define RC_INPUT_MAX_DEADZONE_US 500
/**
* @addtogroup topics
* @{
*/
#include <uORB/topics/input_rc.h>
#define pwm_output_values output_pwm_s
#define rc_input_values input_rc_s
/**
* Input signal type, value is a control position from zero to 100
@@ -93,85 +87,6 @@
*/
typedef uint16_t rc_input_t;
enum RC_INPUT_SOURCE {
RC_INPUT_SOURCE_UNKNOWN = 0,
RC_INPUT_SOURCE_PX4FMU_PPM,
RC_INPUT_SOURCE_PX4IO_PPM,
RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
RC_INPUT_SOURCE_PX4IO_SBUS,
RC_INPUT_SOURCE_PX4IO_ST24,
RC_INPUT_SOURCE_MAVLINK
};
/**
* R/C input status structure.
*
* Published to input_rc, may also be published to other names depending
* on the board involved.
*/
struct rc_input_values {
/** publication time */
uint64_t timestamp_publication;
/** last valid reception time */
uint64_t timestamp_last_signal;
/** number of channels actually being seen */
uint32_t channel_count;
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception */
int32_t rssi;
/**
* explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
* Only the true state is reliable, as there are some (PPM) receivers on the market going
* into failsafe without telling us explicitly.
* */
bool rc_failsafe;
/**
* RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
* True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
* Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
* */
bool rc_lost;
/**
* Number of lost RC frames.
* Note: intended purpose: observe the radio link quality if RSSI is not available
* This value must not be used to trigger any failsafe-alike funtionality.
* */
uint16_t rc_lost_frame_count;
/**
* Number of total RC frames.
* Note: intended purpose: observe the radio link quality if RSSI is not available
* This value must not be used to trigger any failsafe-alike funtionality.
* */
uint16_t rc_total_frame_count;
/**
* Length of a single PPM frame.
* Zero for non-PPM systems
*/
uint16_t rc_ppm_frame_length;
/** Input source */
enum RC_INPUT_SOURCE input_source;
/** measured pulse widths for each of the supported channels */
rc_input_t values[RC_INPUT_MAX_CHANNELS];
};
/**
* @}
*/
/*
* ObjDev tag for R/C inputs.
*/
ORB_DECLARE(input_rc);
#define _RC_INPUT_BASE 0x2b00
/** Fetch R/C input values into (rc_input_values *)arg */
src/drivers/ets_airspeed/ets_airspeed.cpp
+49 -26
View File
@@ -94,7 +94,7 @@
class ETSAirspeed : public Airspeed
{
public:
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH);
protected:
@@ -113,8 +113,8 @@ protected:
*/
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
{
}
@@ -155,7 +155,8 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
if (diff_pres_pa_raw == 0) {
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
@@ -163,7 +164,7 @@ ETSAirspeed::collect()
perf_count(_comms_errors);
DEVICE_LOG("zero value from sensor");
return -1;
}
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
@@ -175,7 +176,7 @@ ETSAirspeed::collect()
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.error_count = perf_event_count(_comms_errors);
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
@@ -210,6 +211,7 @@ ETSAirspeed::cycle()
/* perform collection */
ret = collect();
if (OK != ret) {
perf_count(_comms_errors);
/* restart the measurement state machine */
@@ -239,6 +241,7 @@ ETSAirspeed::cycle()
/* measurement phase */
ret = measure();
if (OK != ret) {
DEVICE_DEBUG("measure error");
}
@@ -287,26 +290,31 @@ start(int i2c_bus)
{
int fd;
if (g_dev != nullptr)
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new ETSAirspeed(i2c_bus);
if (g_dev == nullptr)
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->Airspeed::init())
if (OK != g_dev->Airspeed::init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
@@ -351,21 +359,24 @@ test()
int fd = open(ETS_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
@@ -376,22 +387,25 @@ test()
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
}
/* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default rate");
}
errx(0, "PASS");
}
@@ -404,14 +418,17 @@ reset()
{
int fd = open(ETS_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
@@ -422,8 +439,9 @@ reset()
void
info()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
@@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
if (!strcmp(argv[1], "start")) {
ets_airspeed::start(i2c_bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
if (!strcmp(argv[1], "stop")) {
ets_airspeed::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
ets_airspeed::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(argv[1], "reset")) {
ets_airspeed::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
ets_airspeed::info();
}
ets_airspeed_usage();
exit(0);
src/drivers/irlock/irlock.cpp
+2 -2
View File
@@ -110,7 +110,7 @@ private:
int read_device_block(struct irlock_s *block);
/** internal variables **/
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
work_s _work;
uint32_t _read_failures;
@@ -158,7 +158,7 @@ int IRLOCK::init()
}
/** allocate buffer storing values read from sensor **/
_reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s));
_reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s));
if (_reports == nullptr) {
return ENOTTY;
src/drivers/meas_airspeed/meas_airspeed.cpp
+21 -6
View File
@@ -138,7 +138,7 @@ protected:
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path),
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1),
system_power{}
@@ -189,9 +189,11 @@ MEASAirspeed::collect()
break;
case 1:
/* fallthrough */
/* fallthrough */
case 2:
/* fallthrough */
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
@@ -219,11 +221,11 @@ MEASAirspeed::collect()
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
@@ -276,6 +278,7 @@ MEASAirspeed::cycle()
/* perform collection */
ret = collect();
if (OK != ret) {
/* restart the measurement state machine */
start();
@@ -304,6 +307,7 @@ MEASAirspeed::cycle()
/* measurement phase */
ret = measure();
if (OK != ret) {
DEVICE_DEBUG("measure error");
}
@@ -332,18 +336,23 @@ void
MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (_t_system_power == -1) {
_t_system_power = orb_subscribe(ORB_ID(system_power));
}
if (_t_system_power == -1) {
// not available
return;
}
bool updated = false;
orb_check(_t_system_power, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
}
if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
// not valid, skip correction
return;
@@ -354,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
apply a piecewise linear correction, flattening at 0.5V from 5V
*/
float voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -0.5f) {
voltage_diff = -0.5f;
}
diff_press_pa -= voltage_diff * slope;
/*
@@ -367,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
*/
const float temp_slope = 0.887f;
voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
src/drivers/pwm_out_sim/module.mk
+1
View File
@@ -38,6 +38,7 @@
MODULE_COMMAND = pwm_out_sim
SRCS = pwm_out_sim.cpp
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1200
src/drivers/pwm_out_sim/pwm_out_sim.cpp
+2 -2
View File
@@ -234,7 +234,7 @@ PWMSim::init()
_task = px4_task_spawn_cmd("pwm_out_sim",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1200,
1000,
(px4_main_t)&PWMSim::task_main_trampoline,
nullptr);
@@ -406,7 +406,7 @@ PWMSim::task_main()
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
if (_armed && _mixers != nullptr) {
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
src/drivers/px4fmu/fmu.cpp
+4 -4
View File
@@ -354,7 +354,7 @@ PX4FMU::init()
_task = px4_task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1600,
1200,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
@@ -607,7 +607,7 @@ PX4FMU::task_main()
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
/* initialize PWM limit lib */
@@ -772,8 +772,8 @@ PX4FMU::task_main()
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
if (rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < rc_in.channel_count; i++) {
src/drivers/px4io/px4io.cpp
+18 -18
View File
@@ -672,8 +672,8 @@ PX4IO::init()
return -1;
}
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
_max_rc_input = RC_INPUT_MAX_CHANNELS;
if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS)
_max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS;
param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);
param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max);
@@ -1675,10 +1675,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
int ret;
/* we don't have the status bits, so input_source has to be set elsewhere */
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
uint16_t regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog];
/*
* Read the channel count and the first 9 channels.
@@ -1697,8 +1697,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
channel_count = regs[PX4IO_P_RAW_RC_COUNT];
/* limit the channel count */
if (channel_count > RC_INPUT_MAX_CHANNELS) {
channel_count = RC_INPUT_MAX_CHANNELS;
if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
_rc_chan_count = channel_count;
@@ -1736,7 +1736,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
}
/* get RSSI from input channel */
if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) /
((_rssi_pwm_max - _rssi_pwm_min) / 100);
rssi = rssi > 100 ? 100 : rssi;
@@ -1764,19 +1764,19 @@ PX4IO::io_publish_raw_rc()
/* sort out the source of the values */
if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24;
rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
/* only keep publishing RC input if we ever got a valid input */
if (_rc_last_valid == 0) {
@@ -2660,19 +2660,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
/* sort out the source of the values */
if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
} else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24;
rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
} else {
rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
}
/* read raw R/C input values */
@@ -2757,7 +2757,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= RC_INPUT_MAX_CHANNELS) {
if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) {
/* fail with error */
return -E2BIG;
}
src/examples/px4_daemon_app/px4_daemon_app.c
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/examples/px4_mavlink_debug/px4_mavlink_debug.c
+3 -2
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +34,8 @@
/**
* @file px4_mavlink_debug.c
* Debug application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <px4_config.h>
src/examples/px4_simple_app/px4_simple_app.c
+3 -2
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +34,8 @@
/**
* @file px4_simple_app.c
* Minimal application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <px4_config.h>
src/lib/ecl/attitude_fw/ecl_controller.cpp
+1 -1
View File
@@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate()
float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
float airspeed_result = airspeed;
if (!isfinite(airspeed)) {
if (!PX4_ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed_result = 0.5f * (minspeed + maxspeed);
} else if (airspeed < minspeed) {
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+54 -55
View File
@@ -60,62 +60,24 @@ ECL_PitchController::~ECL_PitchController()
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
float roll = ctl_data.roll;
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.pitch_setpoint) &&
isfinite(roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.airspeed))) {
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
}
/* flying inverted (wings upside down) ? */
bool inverted = false;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
} else {
/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
if (roll > 0.0f) {
/* right hemisphere */
roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
} else {
/* left hemisphere */
roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
}
}
/* input conditioning */
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted) {
turn_offset = -turn_offset;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* add turn offset */
_rate_setpoint += turn_offset;
/* limit the rate */ //XXX: move to body angluar rates
/* limit the rate */
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
@@ -132,14 +94,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -160,11 +122,48 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
/* Transform estimation to body angular rates (jacobian) */
float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
/* apply turning offset to desired bodyrate setpoint*/
/* flying inverted (wings upside down)*/
bool inverted = false;
float constrained_roll;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
} else {
/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
inverted = true;
/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
if (ctl_data.roll > 0.0f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
} else {
/* left hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
}
}
/* input conditioning */
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
Availible on google books 8/11/2015:
https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(constrained_roll) * sinf(constrained_roll)));
if (inverted) {
body_fixed_turn_offset = -body_fixed_turn_offset;
}
/* Finally add the turn offset to your bodyrate setpoint*/
_bodyrate_setpoint += body_fixed_turn_offset;
_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
if (!lock_integrator && _k_i > 0.0f) {
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+9 -12
View File
@@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController()
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) {
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
@@ -83,13 +83,13 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.pitch) &&
isfinite(ctl_data.roll_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
if (!(PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
/* Transform estimation to body angular rates (jacobian) */
float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
_rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f) {
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+14 -18
View File
@@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.speed_body_u) &&
isfinite(ctl_data.speed_body_v) &&
isfinite(ctl_data.speed_body_w) &&
isfinite(ctl_data.roll_rate_setpoint) &&
isfinite(ctl_data.pitch_rate_setpoint))) {
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.speed_body_u) &&
PX4_ISFINITE(ctl_data.speed_body_v) &&
PX4_ISFINITE(ctl_data.speed_body_w) &&
PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
@@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
// counter++;
if (!isfinite(_rate_setpoint)) {
if (!PX4_ISFINITE(_rate_setpoint)) {
warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
}
@@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
if (!PX4_ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
@@ -198,12 +198,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
}
/* Transform estimation to body angular rates (jacobian) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
_rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
src/lib/eigen
+1 -1
Submodule src/lib/eigen updated: e7850ed81f...240599e148
src/lib/external_lgpl/tecs/tecs.cpp
+36 -24
View File
@@ -62,6 +62,10 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
_integ3_state = baro_altitude;
_integ2_state = 0.0f;
_integ1_state = 0.0f;
// Reset the filter state as we just switched from non-altitude control
// to altitude control mode
_states_initalized = false;
}
_update_50hz_last_usec = now;
@@ -94,7 +98,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
// Only required if airspeed is being measured and controlled
float temp = 0;
if (isfinite(airspeed) && airspeed_sensor_enabled()) {
if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
// Get DCM
// Calculate speed rate of change
// XXX check
@@ -138,7 +142,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
// Get airspeed or default to halfway between min and max if
// airspeed is not being used and set speed rate to zero
if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
// If no airspeed available use average of min and max
_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
@@ -224,12 +228,12 @@ void TECS::_update_speed_demand(void)
void TECS::_update_height_demand(float demand, float state)
{
// Handle initialization
if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
_hgt_dem_in_old = demand;
}
// Apply 2 point moving average to demanded height
// This is required because height demand is updated in steps
if (isfinite(demand)) {
if (PX4_ISFINITE(demand)) {
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
} else {
_hgt_dem = _hgt_dem_in_old;
@@ -309,12 +313,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate total energy values
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
_STEdotErrLast = STEdot_error;
_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
_STEdotErrLast = _STEdot_error;
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
@@ -342,7 +346,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
}
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
@@ -438,11 +442,11 @@ void TECS::_update_pitch(void)
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
// Calculate integrator state, constraining input if pitch limits are exceeded
float integ7_input = SEB_error * _integGain;
float integ7_input = _SEB_error * _integGain;
if (_pitch_dem_unc > _PITCHmaxf) {
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
@@ -460,7 +464,7 @@ void TECS::_update_pitch(void)
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem)
{
temp += _PITCHminf * gainInv;
@@ -598,18 +602,26 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
_tecs_state.altitude_sp = _hgt_dem_adj;
_tecs_state.altitude_filtered = _integ3_state;
_tecs_state.altitude_rate_sp = _hgt_rate_dem;
_tecs_state.altitude_rate = _integ2_state;
_tecs_state.airspeed_sp = _TAS_dem_adj;
_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
_tecs_state.airspeed_filtered = _integ5_state;
_tecs_state.airspeed_rate = _vel_dot;
_tecs_state.total_energy_error = _STE_error;
_tecs_state.energy_distribution_error = _SEB_error;
_tecs_state.total_energy_rate_error = _STEdot_error;
_tecs_state.energy_distribution_error = _SEBdot_error;
_tecs_state.energy_error_integ = _integ6_state;
_tecs_state.energy_distribution_error_integ = _integ7_state;
_tecs_state.throttle_integ = _integ6_state;
_tecs_state.pitch_integ = _integ7_state;
_update_pitch_throttle_last_usec = now;
src/lib/external_lgpl/tecs/tecs.h
+29 -12
View File
@@ -79,6 +79,10 @@ public:
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f),
_STE_error(0.0f),
_STEdot_error(0.0f),
_SEB_error(0.0f),
_SEBdot_error(0.0f),
_airspeed_enabled(false),
_states_initalized(false),
_in_air(false),
@@ -137,18 +141,22 @@ public:
struct tecs_state {
uint64_t timestamp;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
float altitude_filtered;
float altitude_sp;
float altitude_rate;
float altitude_rate_sp;
float airspeed_filtered;
float airspeed_sp;
float airspeed_rate;
float airspeed_rate_sp;
float energy_error_integ;
float energy_distribution_error_integ;
float total_energy_error;
float total_energy_rate_error;
float energy_distribution_error;
float energy_distribution_rate_error;
float throttle_integ;
float pitch_integ;
enum ECL_TECS_MODE mode;
};
@@ -376,6 +384,15 @@ private:
// Specific energy error quantities
float _STE_error;
// Energy error rate
float _STEdot_error;
// Specific energy balance error
float _SEB_error;
// Specific energy balance error rate
float _SEBdot_error;
// Time since last update of main TECS loop (seconds)
float _DT;
src/lib/mathlib/math/Matrix.hpp
+2 -2
View File
@@ -285,7 +285,7 @@ public:
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] / num;
res.data[i][j] = data[i][j] / num;
return res;
}
@@ -375,7 +375,7 @@ public:
printf("[ ");
for (unsigned int j = 0; j < N; j++)
printf("%.3f\t", data[i][j]);
printf("%.3f\t", (double)data[i][j]);
printf(" ]\n");
}
src/lib/px4_eigen.h
-2
View File
@@ -46,8 +46,6 @@
#pragma GCC diagnostic ignored "-Wfloat-equal"
#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1
#define EIGEN_MAX_STATIC_ALIGN_BYTES 16
#include <eigen/Eigen/Core>
#include <eigen/Eigen/Geometry>
#pragma GCC diagnostic pop
src/lib/uavcan
+1 -1
Submodule src/lib/uavcan updated: 988e404586...3ae5400aa5
src/modules/commander/commander.cpp
+44 -11
View File
@@ -108,6 +108,7 @@
#include <geo/geo.h>
#include <systemlib/state_table.h>
#include <dataman/dataman.h>
#include <navigator/navigation.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@@ -238,6 +239,8 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct ma
void set_control_mode();
bool stabilization_required();
void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
void print_reject_arm(const char *msg);
@@ -294,7 +297,7 @@ int commander_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 40,
3400,
commander_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
(char * const *)&argv[0]);
unsigned constexpr max_wait_us = 1000000;
unsigned constexpr max_wait_steps = 2000;
@@ -359,7 +362,7 @@ int commander_main(int argc, char *argv[])
if (calib_ret) {
warnx("calibration failed, exiting.");
return 0;
return 1;
} else {
return 0;
}
@@ -388,7 +391,7 @@ int commander_main(int argc, char *argv[])
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(false, mavlink_fd_local, "command line");
px4_close(mavlink_fd_local);
return 0;
return 0;
}
usage("unrecognized command");
@@ -874,6 +877,23 @@ int commander_thread_main(int argc, char *argv[])
bool arm_tune_played = false;
bool was_armed = false;
bool startup_in_hil = false;
#ifdef __PX4_NUTTX
/* NuttX indicates 3 arguments when only 2 are present */
argc -= 1;
#endif
if (argc > 2) {
if (!strcmp(argv[2],"-hil")) {
startup_in_hil = true;
} else {
PX4_ERR("Argument %s not supported.", argv[2]);
PX4_ERR("COMMANDER NOT STARTED");
thread_should_exit = true;
}
}
/* set parameters */
param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID");
@@ -955,7 +975,12 @@ int commander_thread_main(int argc, char *argv[])
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
if(startup_in_hil) {
status.hil_state = vehicle_status_s::HIL_STATE_ON;
} else {
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
}
status.failsafe = false;
/* neither manual nor offboard control commands have been received */
@@ -2219,7 +2244,6 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
//fflush(stdout);
counter++;
int blink_state = blink_msg_state();
@@ -2537,8 +2561,8 @@ set_control_mode()
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
control_mode.flag_control_rates_enabled = stabilization_required();
control_mode.flag_control_attitude_enabled = stabilization_required();
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
@@ -2579,8 +2603,8 @@ set_control_mode()
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_position_enabled = !status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
control_mode.flag_control_termination_enabled = false;
break;
@@ -2689,12 +2713,12 @@ set_control_mode()
!offboard_control_mode.ignore_acceleration_force;
control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode;
!offboard_control_mode.ignore_position) && !status.in_transition_mode;
control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode;
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode;
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
@@ -2706,6 +2730,15 @@ set_control_mode()
}
}
bool
stabilization_required()
{
return (status.is_rotary_wing || // is a rotary wing, or
status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or
(vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND
!status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode
}
void
print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
{
src/modules/commander/state_machine_helper_posix.cpp
-634
View File
@@ -1,634 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
#include "PreflightCheck.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/*
* Get sensing state if necessary
*/
int prearm_ret = OK;
/* only perform the check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
/*
* Perform an atomic state update
*/
//irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
// Perform power checks only if circuit breaker is not
// engaged for these checks
if (!status->circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
}
}
} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
warnx("HIL STATE ON -> STANDBY");
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
warnx("NOT ARMING: Sensors not operational.");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
} else {
mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
/* end of atomic state update */
//irqrestore(flags);
}
if (ret == TRANSITION_DENIED) {
const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
/* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
}
return ret;
}
bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
{
// System is safe if:
// 1) Not armed
// 2) Armed, but in software lockdown (HIL)
// 3) Safety switch is present AND engaged -> actuators locked
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
return false;
}
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
return ret;
}
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
case vehicle_status_s::HIL_STATE_ON:
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
break;
/* skip mavlink */
if (!strcmp(MAVLINK_LOG_DEVICE, devname)) {
continue;
}
int sensfd = px4_open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown HIL state");
break;
}
}
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
// XXX also set lockdown here
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case vehicle_status_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
* - depending on datalink, RC and if the mission is finished */
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* datalink loss disabled:
* check if both, RC and datalink are lost during the mission
* or RC is lost after the mission is finished: this should always trigger RCRECOVER */
} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
(status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}
default:
break;
}
return status->nav_state != nav_state_old;
}
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
/* at this point this equals the preflight check, but might add additional
* quantities later.
*/
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
checkAirspeed = true;
}
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
}
src/modules/dataman/dataman.h
+1
View File
@@ -39,6 +39,7 @@
#ifndef _DATAMANAGER_H
#define _DATAMANAGER_H
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -3
View File
@@ -947,9 +947,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
if (!_local_pos.xy_global ||
!_local_pos.v_xy_valid ||
_gps.timestamp_position == 0 ||
(dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
_global_pos.eph = EPH_LARGE_VALUE;
_global_pos.epv = EPV_LARGE_VALUE;
} else {
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
@@ -1129,8 +1134,8 @@ int AttitudePositionEstimatorEKF::start()
/* start the task */
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
7500,
SCHED_PRIORITY_MAX - 40,
4800,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
src/modules/ekf_att_pos_estimator/module.mk
+1 -1
View File
@@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
estimator_22states.cpp \
estimator_utilities.cpp
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400
src/modules/fw_att_control/fw_att_control_main.cpp
+40 -34
View File
@@ -42,6 +42,9 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -168,7 +171,6 @@ private:
float p_rmax_pos;
float p_rmax_neg;
float p_integrator_max;
float p_roll_feedforward;
float r_p;
float r_i;
float r_ff;
@@ -178,7 +180,6 @@ private:
float y_i;
float y_d;
float y_ff;
float y_roll_feedforward;
float y_integrator_max;
float y_coordinated_min_speed;
int32_t y_coordinated_method;
@@ -211,7 +212,6 @@ private:
param_t p_rmax_pos;
param_t p_rmax_neg;
param_t p_integrator_max;
param_t p_roll_feedforward;
param_t r_p;
param_t r_i;
param_t r_ff;
@@ -221,7 +221,6 @@ private:
param_t y_i;
param_t y_d;
param_t y_ff;
param_t y_roll_feedforward;
param_t y_integrator_max;
param_t y_coordinated_min_speed;
param_t y_coordinated_method;
@@ -374,7 +373,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_RR_P");
_parameter_handles.r_i = param_find("FW_RR_I");
@@ -426,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -450,7 +448,6 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
@@ -493,7 +490,6 @@ FixedwingAttitudeControl::parameters_update()
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.tconst);
@@ -671,8 +667,9 @@ FixedwingAttitudeControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@@ -807,7 +804,7 @@ FixedwingAttitudeControl::task_main()
float flaps_control = 0.0f;
/* map flaps by default to manual if valid */
if (isfinite(_manual.flaps)) {
if (PX4_ISFINITE(_manual.flaps)) {
flaps_control = _manual.flaps;
}
@@ -817,7 +814,7 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
@@ -997,7 +994,7 @@ FixedwingAttitudeControl::task_main()
control_input.lock_integrator = lock_integrator;
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
@@ -1009,8 +1006,8 @@ FixedwingAttitudeControl::task_main()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
@@ -1020,8 +1017,8 @@ FixedwingAttitudeControl::task_main()
}
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
@@ -1041,11 +1038,11 @@ FixedwingAttitudeControl::task_main()
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[2] += yaw_manual;
if (!isfinite(yaw_u)) {
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
@@ -1055,11 +1052,11 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
@@ -1139,7 +1136,6 @@ FixedwingAttitudeControl::task_main()
_control_task = -1;
_task_running = false;
_exit(0);
}
int
@@ -1152,7 +1148,7 @@ FixedwingAttitudeControl::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
(main_t)&FixedwingAttitudeControl::task_main_trampoline,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@@ -1166,23 +1162,29 @@ FixedwingAttitudeControl::start()
int fw_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: fw_att_control {start|stop|status}");
warnx("usage: fw_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (att_control::g_control != nullptr)
errx(1, "already running");
if (att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
att_control::g_control = new FixedwingAttitudeControl;
if (att_control::g_control == nullptr)
errx(1, "alloc failed");
if (att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;
err(1, "start failed");
warn("start failed");
return 1;
}
/* check if the waiting is necessary at all */
@@ -1196,24 +1198,28 @@ int fw_att_control_main(int argc, char *argv[])
}
printf("\n");
}
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr)
errx(1, "not running");
if (att_control::g_control == nullptr){
warnx("not running");
return 1;
}
delete att_control::g_control;
att_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (att_control::g_control) {
errx(0, "running");
warnx("running");
return 0;
} else {
errx(1, "not running");
warnx("not running");
return 1;
}
}
src/modules/fw_att_control/fw_att_control_params.c
-11
View File
@@ -128,17 +128,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
/**
* Roll to Pitch feedforward gain.
*
* This compensates during turns and ensures the nose stays level.
*
* @min 0.0
* @max 2.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f);
/**
* Roll rate proportional Gain
*
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+49 -31
View File
@@ -52,6 +52,9 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
if (!isfinite(global_pos.terrain_alt)) {
if (!PX4_ISFINITE(global_pos.terrain_alt)) {
return land_setpoint_alt;
}
@@ -1024,7 +1027,11 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
_althold_epv = _global_pos.epv;
_was_in_deadband = true;
}
if (_vehicle_status.is_vtol) {
if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) {
_hold_alt = _global_pos.alt;
}
}
return climbout_mode;
}
@@ -1728,8 +1735,9 @@ FixedwingPositionControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@@ -1787,11 +1795,11 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) {
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing) {
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
@@ -1821,7 +1829,6 @@ FixedwingPositionControl::task_main()
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}
void FixedwingPositionControl::reset_takeoff_state()
@@ -1920,22 +1927,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
break;
}
t.altitudeSp = s.hgt_dem;
t.altitude_filtered = s.hgt;
t.airspeedSp = s.spd_dem;
t.airspeed_filtered = s.spd;
t.altitudeSp = s.altitude_sp;
t.altitude_filtered = s.altitude_filtered;
t.airspeedSp = s.airspeed_sp;
t.airspeed_filtered = s.airspeed_filtered;
t.flightPathAngleSp = s.dhgt_dem;
t.flightPathAngle = s.dhgt;
t.flightPathAngleFiltered = s.dhgt;
t.flightPathAngleSp = s.altitude_rate_sp;
t.flightPathAngle = s.altitude_rate;
t.flightPathAngleFiltered = s.altitude_rate;
t.airspeedDerivativeSp = s.dspd_dem;
t.airspeedDerivative = s.dspd;
t.airspeedDerivativeSp = s.airspeed_rate_sp;
t.airspeedDerivative = s.airspeed_rate;
t.totalEnergyRateSp = s.thr;
t.totalEnergyRate = s.ithr;
t.energyDistributionRateSp = s.ptch;
t.energyDistributionRate = s.iptch;
t.totalEnergyError = s.total_energy_error;
t.totalEnergyRateError = s.total_energy_rate_error;
t.energyDistributionError = s.energy_distribution_error;
t.energyDistributionRateError = s.energy_distribution_rate_error;
t.throttle_integ = s.throttle_integ;
t.pitch_integ = s.pitch_integ;
if (_tecs_status_pub != nullptr) {
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
@@ -1955,7 +1965,7 @@ FixedwingPositionControl::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
(main_t)&FixedwingPositionControl::task_main_trampoline,
(px4_main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@@ -1969,16 +1979,20 @@ FixedwingPositionControl::start()
int fw_pos_control_l1_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
warnx("usage: fw_pos_control_l1 {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (l1_control::g_control != nullptr)
errx(1, "already running");
if (l1_control::g_control != nullptr) {
warnx("already running");
return 1;
}
if (OK != FixedwingPositionControl::start()) {
err(1, "start failed");
warn("start failed");
return 1;
}
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
@@ -1989,24 +2003,28 @@ int fw_pos_control_l1_main(int argc, char *argv[])
}
printf("\n");
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (l1_control::g_control == nullptr)
errx(1, "not running");
if (l1_control::g_control == nullptr){
warnx("not running");
return 1;
}
delete l1_control::g_control;
l1_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (l1_control::g_control) {
errx(0, "running");
warnx("running");
return 0;
} else {
errx(1, "not running");
warnx("not running");
return 1;
}
}
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+11 -11
View File
@@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.flightPathAngleFiltered = flightPathAngleFiltered;
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;
_status.totalEnergyRate = totalEnergyRate;
_status.energyDistributionRateSp = energyDistributionRateSp;
_status.energyDistributionRate = energyDistributionRate;
_status.mode = mode;
// /* Write part of the status message */
// _status.flightPathAngleSp = flightPathAngleSp;
// _status.flightPathAngle = flightPathAngle;
// _status.flightPathAngleFiltered = flightPathAngleFiltered;
// _status.airspeedDerivativeSp = airspeedDerivativeSp;
// _status.airspeedDerivative = airspeedDerivative;
// _status.totalEnergyRateSp = totalEnergyRateSp;
// _status.totalEnergyRate = totalEnergyRate;
// _status.energyDistributionRateSp = energyDistributionRateSp;
// _status.energyDistributionRate = energyDistributionRate;
// _status.mode = mode;
/** update control blocks **/
/* update total energy rate control block */
src/modules/mavlink/mavlink_main.cpp
+27 -3
View File
@@ -1411,7 +1411,7 @@ Mavlink::task_main(int argc, char *argv[])
_datarate = 0;
_mode = MAVLINK_MODE_NORMAL;
#ifndef __PX4_POSIX
#ifdef __PX4_NUTTX
/* the NuttX optarg handler does not
* ignore argv[0] like the POSIX handler
* does, nor does it deal with non-flag
@@ -1484,6 +1484,8 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "osd") == 0) {
_mode = MAVLINK_MODE_OSD;
} else if (strcmp(optarg, "config") == 0) {
_mode = MAVLINK_MODE_CONFIG;
}
break;
@@ -1600,8 +1602,6 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
@@ -1693,6 +1693,27 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VTOL_STATE", 0.5f);
break;
case MAVLINK_MODE_CONFIG:
// Enable a number of interesting streams we want via USB
configure_stream("PARAM_VALUE", 300.0f);
configure_stream("MISSION_ITEM", 50.0f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("VFR_HUD", 20.0f);
configure_stream("ATTITUDE", 100.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("RC_CHANNELS_RAW", 5.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
configure_stream("HIGHRES_IMU", 100.0f);
configure_stream("GPS_RAW_INT", 20.0f);
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("VTOL_STATE", 2.0f);
default:
break;
}
@@ -1877,6 +1898,9 @@ Mavlink::task_main(int argc, char *argv[])
}
perf_end(_loop_perf);
/* confirm task running only once fully initialized */
_task_running = true;
}
delete _subscribe_to_stream;
src/modules/mavlink/mavlink_main.h
+2 -1
View File
@@ -147,7 +147,8 @@ public:
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_ONBOARD,
MAVLINK_MODE_OSD
MAVLINK_MODE_OSD,
MAVLINK_MODE_CONFIG
};
void set_mode(enum MAVLINK_MODE);
src/modules/mavlink/mavlink_mission.cpp
+1
View File
@@ -49,6 +49,7 @@
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
src/modules/mavlink/mavlink_parameters.cpp
+2 -2
View File
@@ -159,9 +159,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -1050,7 +1050,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = RC_INPUT_RSSI_MAX;
/* channels */
@@ -1094,7 +1094,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = RC_INPUT_RSSI_MAX;
/* roll */
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -3
View File
@@ -993,9 +993,12 @@ MulticopterPositionControl::task_main()
reset_yaw_sp = true;
}
// XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition
if (_vehicle_status.in_transition_mode) {
reset_yaw_sp = true;
/* reset yaw and altitude setpoint for VTOL which are in fw mode */
if (_vehicle_status.is_vtol) {
if (!_vehicle_status.is_rotary_wing) {
reset_yaw_sp = true;
_reset_alt_sp = true;
}
}
//Update previous arming state
src/modules/navigator/datalinkloss.cpp
+1
View File
@@ -45,6 +45,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/enginefailure.cpp
+1
View File
@@ -44,6 +44,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/gpsfailure.cpp
+1
View File
@@ -45,6 +45,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/mission.cpp
+16 -12
View File
@@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -624,26 +625,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
{
/* select onboard/offboard mission */
int *mission_index_ptr;
struct mission_s *mission;
dm_item_t dm_item;
int mission_index_next;
struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
/* do not work on empty missions */
if (mission->count == 0) {
return false;
}
/* move to next item if there is one */
if (mission_index_next < ((int)mission->count - 1)) {
mission_index_next++;
}
if (onboard) {
/* onboard mission */
mission_index_next = _current_onboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
mission = &_onboard_mission;
dm_item = DM_KEY_WAYPOINTS_ONBOARD;
} else {
/* offboard mission */
mission_index_next = _current_offboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
mission = &_offboard_mission;
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
@@ -653,7 +657,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
return false;
}
@@ -665,7 +669,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
"ERROR waypoint could not be read");
return false;
}
src/modules/navigator/mission_block.h
+2
View File
@@ -43,6 +43,8 @@
#include <drivers/drv_hrt.h>
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
src/modules/uORB/topics/mission.h → src/modules/navigator/navigation.h
+4 -13
View File
@@ -32,19 +32,18 @@
****************************************************************************/
/**
* @file mission.h
* @file navigation.h
* Definition of a mission consisting of mission items.
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_H_
#define TOPIC_MISSION_H_
#ifndef NAVIGATION_H_
#define NAVIGATION_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#define NUM_MISSIONS_SUPPORTED 256
@@ -103,15 +102,7 @@ struct mission_item_s {
int actuator_value; /**< new value for selected actuator in ms 900...2000 */
};
/**
* This topic used to notify navigator about mission changes, mission itself and new mission state
* must be stored in dataman before publication.
*/
struct mission_s {
int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
unsigned count; /**< count of the missions stored in the dataman */
int current_seq; /**< default -1, start at the one changed latest */
};
#include <uORB/topics/mission.h>
/**
* @}
src/modules/navigator/navigator.h
+1
View File
@@ -45,6 +45,7 @@
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -524,7 +524,7 @@ Navigator::start()
/* start the task */
_navigator_task = px4_task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX -5,
SCHED_PRIORITY_DEFAULT - 5,
1500,
(px4_main_t)&Navigator::task_main_trampoline,
nullptr);
src/modules/navigator/rcloss.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
src/modules/navigator/rtl.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
src/modules/navigator/rtl.h
+1 -2
View File
@@ -44,8 +44,7 @@
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
src/modules/position_estimator_inav/position_estimator_inav_main.c
+3 -3
View File
@@ -116,10 +116,10 @@ static inline int max(int val1, int val2)
static void usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
PX4_INFO("%s\n", reason);
}
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
return;
}
@@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
if (isfinite(sensor.baro_alt_meter)) {
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
}
src/modules/sdlog2/sdlog2.c
+31 -13
View File
@@ -106,6 +106,7 @@
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/git_version.h>
#include <systemlib/printload.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
@@ -200,7 +201,8 @@ static unsigned long log_msgs_written = 0;
static unsigned long log_msgs_skipped = 0;
/* GPS time, used for log files naming */
static uint64_t gps_time = 0;
static uint64_t gps_time_sec = 0;
static bool has_gps_3d_fix = false;
/* current state of logging */
static bool logging_enabled = false;
@@ -412,13 +414,13 @@ bool get_log_time_utc_tt(struct tm *tt, bool boot_time) {
/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
time_t utc_time_sec;
if (_gpstime_only) {
utc_time_sec = gps_time / 1e6;
if (_gpstime_only && has_gps_3d_fix) {
utc_time_sec = gps_time_sec;
} else {
utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
}
if (utc_time_sec > PX4_EPOCH_SECS) {
if (utc_time_sec > PX4_EPOCH_SECS) {
/* strip the time elapsed since boot */
if (boot_time) {
utc_time_sec -= hrt_absolute_time() / 1e6;
@@ -737,9 +739,16 @@ void sdlog2_start_log()
}
/* write all performance counters */
hrt_abstime curr_time = hrt_absolute_time();
struct print_load_s load;
int perf_fd = open_perf_file("preflight");
init_print_load_s(curr_time, &load);
print_load(curr_time, perf_fd, &load);
dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
perf_print_all(perf_fd);
dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n");
usleep(500 * 1000);
print_load(hrt_absolute_time(), perf_fd, &load);
close(perf_fd);
/* reset performance counters to get in-flight min and max values in post flight log */
@@ -775,8 +784,15 @@ void sdlog2_stop_log()
/* write all performance counters */
int perf_fd = open_perf_file("postflight");
hrt_abstime curr_time = hrt_absolute_time();
dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
perf_print_all(perf_fd);
struct print_load_s load;
dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n");
init_print_load_s(curr_time, &load);
print_load(curr_time, perf_fd, &load);
sleep(1);
print_load(hrt_absolute_time(), perf_fd, &load);
close(perf_fd);
/* free log writer performance counter */
@@ -989,7 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[])
sdlog2_usage(NULL);
}
gps_time = 0;
gps_time_sec = 0;
/* interpret logging params */
@@ -1289,7 +1305,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_utc_usec / 1e6;
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
}
}
@@ -1320,7 +1336,8 @@ int sdlog2_thread_main(int argc, char *argv[])
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time = buf_gps_pos.time_utc_usec / 1e6;
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
has_gps_3d_fix = buf_gps_pos.fix_type == 3;
}
if (!logging_enabled) {
@@ -1900,15 +1917,16 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ;
log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ;
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
LOGBUFFER_WRITE_AND_COUNT(TECS);
}
@@ -1976,7 +1994,7 @@ int sdlog2_thread_main(int argc, char *argv[])
void sdlog2_status()
{
warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
warnx("time: gps: %u seconds", (unsigned)gps_time);
warnx("time: gps: %u seconds", (unsigned)gps_time_sec);
if (!logging_enabled) {
warnx("not logging");
} else {
src/modules/sdlog2/sdlog2_messages.h
+7 -7
View File
@@ -363,16 +363,16 @@ struct log_TECS_s {
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
float totalEnergyRateSp;
float totalEnergyRate;
float energyDistributionRateSp;
float energyDistributionRate;
float totalEnergyError;
float totalEnergyRateError;
float energyDistributionError;
float energyDistributionRateError;
float pitch_integ;
float throttle_integ;
uint8_t mode;
};
@@ -545,7 +545,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
src/modules/sensors/sensor_params.c
+4 -4
View File
@@ -1887,7 +1887,7 @@ PARAM_DEFINE_INT32(RC_TH_USER, 1);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_ROLL, 0);
/**
* Pitch control channel mapping.
@@ -1900,7 +1900,7 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 0);
/**
* Failsafe channel mapping.
@@ -1927,7 +1927,7 @@ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0);
/**
* Yaw control channel mapping.
@@ -1940,7 +1940,7 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
/**
* Mode switch channel mapping.
src/modules/sensors/sensors.cpp
+22 -23
View File
@@ -165,7 +165,7 @@ public:
int start();
private:
static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
@@ -239,7 +239,6 @@ private:
int32_t _mag2_prio; /**< mag2 sensor priority */
int32_t _baro_prio; /**< baro0 sensor priority */
int32_t _baro1_prio; /**< baro1 sensor priority */
int32_t _diff_pres_prio; /**< baro1 sensor priority */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -249,7 +248,7 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
struct rc_parameter_map_s _rc_parameter_map;
float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -294,7 +293,7 @@ private:
int rc_map_aux4;
int rc_map_aux5;
int rc_map_param[RC_PARAM_MAP_NCHAN];
int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
int32_t rc_fails_thr;
float rc_assist_th;
@@ -350,8 +349,8 @@ private:
param_t rc_map_aux4;
param_t rc_map_aux5;
param_t rc_map_param[RC_PARAM_MAP_NCHAN];
param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound
param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound
to a RC channel, equivalent float values in the
_parameters struct are not existing
because these parameters are never read. */
@@ -603,9 +602,9 @@ Sensors::Sensors() :
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
/* RC to parameter mapping for changing parameters with RC */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
char name[PARAM_ID_LEN];
snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
char name[rc_parameter_map_s::PARAM_ID_LEN];
snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
_parameter_handles.rc_map_param[i] = param_find(name);
}
@@ -783,7 +782,7 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
}
@@ -831,7 +830,7 @@ Sensors::parameters_update()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
@@ -1544,6 +1543,12 @@ Sensors::parameter_update_poll(bool forced)
/* run through all mag sensors */
for (unsigned s = 0; s < 3; s++) {
/* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced
* in the section below.
*/
_mag_rotation[s] = _board_rotation;
res = ERROR;
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
@@ -1554,12 +1559,6 @@ Sensors::parameter_update_poll(bool forced)
continue;
}
/* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced
* in the section below.
*/
_mag_rotation[s] = _board_rotation;
bool config_ok = false;
/* run through all stored calibrations */
@@ -1696,7 +1695,7 @@ Sensors::rc_parameter_map_poll(bool forced)
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
/* update paramter handles to which the RC channels are mapped */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
@@ -1709,17 +1708,17 @@ Sensors::rc_parameter_map_poll(bool forced)
_parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
} else {
_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
_parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]);
}
}
warnx("rc to parameter map updated");
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
i,
_rc_parameter_map.param_id[i],
&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)],
(double)_rc_parameter_map.scale[i],
(double)_rc_parameter_map.value0[i],
(double)_rc_parameter_map.value_min[i],
@@ -1911,7 +1910,7 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
void
Sensors::set_params_from_rc()
{
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
@@ -2142,7 +2141,7 @@ Sensors::task_main()
warnx("Sensor initialization failed");
_sensors_task = -1;
if (_fd_adc >=0) {
close(_fd_adc);
px4_close(_fd_adc);
_fd_adc = -1;
}
return;
src/modules/systemlib/circuit_breaker_params.c
+1 -1
View File
@@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
/**
* Circuit breaker for IO safety
*
* Setting this parameter to 894281 will disable IO safety.
* Setting this parameter to 22027 will disable IO safety.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
src/modules/systemlib/module.mk
+4 -1
View File
@@ -55,11 +55,14 @@ SRCS = \
ifeq ($(PX4_TARGET_OS),nuttx)
SRCS += err.c \
printload.c \
up_cxxinitialize.c
else
SRCS += print_load_posix.c
endif
ifneq ($(PX4_TARGET_OS),qurt)
SRCS += hx_stream.c
SRCS += hx_stream.c
endif
MAXOPTIMIZATION = -Os
src/modules/systemlib/perf_counter.c
+1 -1
View File
@@ -377,7 +377,7 @@ perf_reset(perf_counter_t handle)
void
perf_print_counter(perf_counter_t handle)
{
perf_print_counter_fd(0, handle);
perf_print_counter_fd(1, handle);
}
void
src/modules/systemlib/print_load_posix.c
+74
View File
@@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file print_load_posix.c
*
* Print the current system load.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <string.h>
#include <stdio.h>
#include <systemlib/cpuload.h>
#include <systemlib/printload.h>
#include <drivers/drv_hrt.h>
extern struct system_load_s system_load;
#define CL "\033[K" // clear line
void init_print_load_s(uint64_t t, struct print_load_s *s)
{
s->total_user_time = 0;
s->running_count = 0;
s->blocked_count = 0;
s->new_time = t;
s->interval_start_time = t;
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
s->last_times[i] = 0;
}
s->interval_time_ms_inv = 0.f;
}
void print_load(uint64_t t, int fd, struct print_load_s *print_state)
{
}
src/modules/systemlib/printload.c
+278
View File
@@ -0,0 +1,278 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file printload.c
*
* Print the current system load.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <string.h>
#include <stdio.h>
#include <systemlib/cpuload.h>
#include <systemlib/printload.h>
#include <drivers/drv_hrt.h>
extern struct system_load_s system_load;
#define CL "\033[K" // clear line
void init_print_load_s(uint64_t t, struct print_load_s *s)
{
s->total_user_time = 0;
s->running_count = 0;
s->blocked_count = 0;
s->new_time = t;
s->interval_start_time = t;
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
s->last_times[i] = 0;
}
s->interval_time_ms_inv = 0.f;
}
static const char *
tstate_name(const tstate_t s)
{
switch (s) {
case TSTATE_TASK_INVALID:
return "init";
case TSTATE_TASK_PENDING:
return "PEND";
case TSTATE_TASK_READYTORUN:
return "READY";
case TSTATE_TASK_RUNNING:
return "RUN";
case TSTATE_TASK_INACTIVE:
return "inact";
case TSTATE_WAIT_SEM:
return "w:sem";
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG:
return "w:sig";
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY:
return "w:mqe";
case TSTATE_WAIT_MQNOTFULL:
return "w:mqf";
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL:
return "w:pgf";
#endif
default:
return "ERROR";
}
}
void print_load(uint64_t t, int fd, struct print_load_s *print_state)
{
print_state->new_time = t;
int i;
uint64_t curr_time_us;
uint64_t idle_time_us;
char *clear_line = "";
/* print system information */
if (fd == 1) {
dprintf(fd, "\033[H"); /* move cursor home and clear screen */
clear_line = CL;
}
curr_time_us = t;
idle_time_us = system_load.tasks[0].total_runtime;
if (print_state->new_time > print_state->interval_start_time) {
print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000));
}
print_state->running_count = 0;
print_state->blocked_count = 0;
print_state->total_user_time = 0;
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
uint64_t interval_runtime;
if (system_load.tasks[i].valid) {
switch (system_load.tasks[i].tcb->task_state) {
case TSTATE_TASK_PENDING:
case TSTATE_TASK_READYTORUN:
case TSTATE_TASK_RUNNING:
print_state->running_count++;
break;
case TSTATE_TASK_INVALID:
case TSTATE_TASK_INACTIVE:
case TSTATE_WAIT_SEM:
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG:
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY:
case TSTATE_WAIT_MQNOTFULL:
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL:
#endif
print_state->blocked_count++;
break;
}
}
interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 &&
system_load.tasks[i].total_runtime > print_state->last_times[i])
? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000
: 0;
print_state->last_times[i] = system_load.tasks[i].total_runtime;
if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv;
if (i > 0) {
print_state->total_user_time += interval_runtime;
}
} else {
print_state->curr_loads[i] = 0;
}
}
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
if (system_load.tasks[i].tcb->pid == 0) {
float idle;
float task_load;
float sched_load;
idle = print_state->curr_loads[0];
task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv;
/* this can happen if one tasks total runtime was not computed
correctly by the scheduler instrumentation TODO */
if (task_load > (1.f - idle)) {
task_load = (1.f - idle);
}
sched_load = 1.f - idle - task_load;
dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n",
clear_line,
system_load.total_count,
print_state->running_count,
print_state->blocked_count);
dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
clear_line,
(double)(task_load * 100.f),
(double)(sched_load * 100.f),
(double)(idle * 100.f));
dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n",
clear_line,
(double)curr_time_us / 1000000.d,
(double)idle_time_us / 1000000.d,
clear_line);
/* header for task list */
dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n",
clear_line,
"PID",
CONFIG_TASK_NAME_SIZE, "COMMAND",
"CPU(ms)",
"CPU(%)",
"USED/STACK",
"PRIO(BASE)",
#if CONFIG_RR_INTERVAL > 0
"TSLICE"
#else
"STATE"
#endif
);
}
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff) {
break;
}
stack_free++;
}
dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
clear_line,
system_load.tasks[i].tcb->pid,
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
(system_load.tasks[i].total_runtime / 1000),
(int)(print_state->curr_loads[i] * 100.0f),
(int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000),
stack_size - stack_free,
stack_size,
system_load.tasks[i].tcb->sched_priority,
system_load.tasks[i].tcb->base_priority);
#if CONFIG_RR_INTERVAL > 0
/* print scheduling info with RR time slice */
dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice);
#else
// print task state instead
dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
#endif
}
}
print_state->interval_start_time = print_state->new_time;
}
src/modules/uORB/topics/rc_parameter_map.h → src/modules/systemlib/printload.h
+25 -31
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -20,7 +20,7 @@
* "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 ,
* 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
@@ -32,45 +32,39 @@
****************************************************************************/
/**
* @file rc_parameter_map.h
* Maps RC channels to parameters
* @file printload.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* Print the current system load.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#ifndef TOPIC_RC_PARAMETER_MAP_H
#define TOPIC_RC_PARAMETER_MAP_H
#pragma once
__BEGIN_DECLS
#include <px4_config.h>
#include <stdint.h>
#include "../uORB.h"
#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
#ifndef CONFIG_MAX_TASKS
#define CONFIG_MAX_TASKS 64
#endif
/**
* @addtogroup topics
* @{
*/
struct print_load_s {
uint64_t total_user_time;
struct rc_parameter_map_s {
uint64_t timestamp; /**< time at which the map was updated */
int running_count;
int blocked_count;
bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */
int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this
this field is ignored if set to -1, in this case param_id will
be used*/
char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */
float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */
float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */
float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */
float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */
uint64_t new_time;
uint64_t interval_start_time;
uint64_t last_times[CONFIG_MAX_TASKS];
float curr_loads[CONFIG_MAX_TASKS];
float interval_time_ms_inv;
};
/**
* @}
*/
__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s);
ORB_DECLARE(rc_parameter_map);
__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state);
#endif
__END_DECLS
src/modules/uORB/module.mk
+3 -6
View File
@@ -56,14 +56,11 @@ ifeq ($(PX4_TARGET_OS),posix-arm)
SRCS += uORBTest_UnitTest.cpp
endif
ifneq ($(PX4_TARGET_OS),qurt)
SRCS += Publication.cpp \
Subscription.cpp
endif
SRCS += objects_common.cpp \
uORBUtils.cpp \
uORB.cpp \
uORBMain.cpp
uORBMain.cpp \
Publication.cpp \
Subscription.cpp
MAXOPTIMIZATION = -Os
src/modules/uORB/objects_common.cpp
+16 -12
View File
@@ -45,23 +45,23 @@
#include <drivers/drv_orb_dev.h>
#include <drivers/drv_mag.h>
ORB_DEFINE(sensor_mag, struct mag_report);
#include "topics/sensor_mag.h"
ORB_DEFINE(sensor_mag, struct sensor_mag_s);
#include <drivers/drv_accel.h>
ORB_DEFINE(sensor_accel, struct accel_report);
#include "topics/sensor_accel.h"
ORB_DEFINE(sensor_accel, struct sensor_accel_s);
#include <drivers/drv_gyro.h>
ORB_DEFINE(sensor_gyro, struct gyro_report);
#include "topics/sensor_gyro.h"
ORB_DEFINE(sensor_gyro, struct sensor_gyro_s);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro, struct baro_report);
#include "topics/sensor_baro.h"
ORB_DEFINE(sensor_baro, struct sensor_baro_s);
#include <drivers/drv_pwm_output.h>
ORB_DEFINE(output_pwm, struct pwm_output_values);
#include "topics/output_pwm.h"
ORB_DEFINE(output_pwm, struct output_pwm_s);
#include <drivers/drv_rc_input.h>
ORB_DEFINE(input_rc, struct rc_input_values);
#include "topics/input_rc.h"
ORB_DEFINE(input_rc, struct input_rc_s);
#include "topics/pwm_input.h"
ORB_DEFINE(pwm_input, struct pwm_input_s);
@@ -140,6 +140,10 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);
// XXX onboard and offboard mission are still declared here until this is
// generator supported
#include <navigator/navigation.h>
ORB_DEFINE(offboard_mission, struct mission_s);
ORB_DEFINE(onboard_mission, struct mission_s);
src/modules/uORB/uORB.h
+1 -1
View File
@@ -338,7 +338,7 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
* priority, independent of the startup order of the associated publishers.
* @return OK on success, ERROR otherwise with errno set accordingly.
*/
extern int orb_priority(int handle, int *priority) __EXPORT;
extern int orb_priority(int handle, int32_t *priority) __EXPORT;
/**
* Set the minimum interval between which updates are seen for a subscription.
src/modules/uavcan/actuators/esc.cpp
+21 -13
View File
@@ -68,8 +68,8 @@ int UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("ESC status sub failed %i", res);
return res;
}
@@ -83,9 +83,9 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
* Rate limiting - we don't want to congest the bus
*/
const auto timestamp = _node.getMonotonicTime();
if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
return;
}
_prev_cmd_pub = timestamp;
/*
@@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
if (scaled < 0.0F) {
scaled = 0.0F;
}
}
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
@@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
} else {
msg.cmd.push_back(static_cast<unsigned>(0));
}
@@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm)
{
if (arm) {
_armed_mask = -1;
} else {
_armed_mask = 0;
}
@@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
} else {
_armed_mask = 0;
}
@@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
if (_esc_status_pub != nullptr) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
src/modules/uavcan/actuators/esc.hpp
+4 -4
View File
@@ -54,7 +54,7 @@
class UavcanEscController
{
public:
UavcanEscController(uavcan::INode& node);
UavcanEscController(uavcan::INode &node);
~UavcanEscController();
int init();
@@ -79,12 +79,12 @@ private:
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
typedef uavcan::MethodBinder<UavcanEscController*,
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
TimerCbBinder;
bool _armed = false;
esc_status_s _esc_status = {};
src/modules/uavcan/module.mk
+6 -1
View File
@@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400
# Main
SRCS += uavcan_main.cpp \
uavcan_servers.cpp \
uavcan_clock.cpp \
uavcan_params.c
@@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC))
INCLUDE_DIRS += $(LIBUAVCAN_INC)
# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
override EXTRADEFINES := $(EXTRADEFINES) \
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
-DUAVCAN_NO_ASSERTIONS \
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
#
# libuavcan drivers for STM32
src/modules/uavcan/sensors/baro.cpp
+3 -5
View File
@@ -45,9 +45,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
_sub_air_pressure_data(node),
_sub_air_temperature_data(node),
_reports(nullptr)
{
last_temperature = 0.0f;
}
{ }
int UavcanBarometerBridge::init()
{
@@ -153,7 +151,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
void UavcanBarometerBridge::air_temperature_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
{
last_temperature = msg.static_temperature;
last_temperature_kelvin = msg.static_temperature;
}
void UavcanBarometerBridge::air_pressure_sub_cb(const
@@ -162,7 +160,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
baro_report report;
report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = last_temperature;
report.temperature = last_temperature_kelvin - 273.15F;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0;
src/modules/uavcan/sensors/baro.hpp
+1 -1
View File
@@ -78,6 +78,6 @@ private:
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
unsigned _msl_pressure = 101325;
ringbuffer::RingBuffer *_reports;
float last_temperature;
float last_temperature_kelvin = 0.0f;
};
src/modules/uavcan/sensors/gnss.cpp
+17 -11
View File
@@ -46,20 +46,21 @@
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_sub_fix(node),
_report_pub(nullptr)
_node(node),
_sub_fix(node),
_report_pub(nullptr)
{
}
int UavcanGnssBridge::init()
{
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("GNSS fix sub failed %i", res);
return res;
}
return res;
}
@@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
void UavcanGnssBridge::print_status() const
{
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
if (_receiver_node_id < 0) {
printf("N/A\n");
} else {
printf("%d\n", _receiver_node_id);
}
@@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
@@ -114,14 +118,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
// Vertical position uncertainty
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
@@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
report.s_variance_m_s = -1.0F;
@@ -154,7 +159,8 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s *
report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
src/modules/uavcan/sensors/mag.cpp
+39 -26
View File
@@ -41,11 +41,11 @@
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
@@ -55,15 +55,18 @@ _sub_mag(node)
int UavcanMagnetometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
if (res < 0) {
DEVICE_LOG("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
@@ -74,6 +77,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
/* buffer must be large enough */
unsigned count = buflen / sizeof(struct mag_report);
if (count < 1) {
return -ENOSPC;
}
@@ -85,6 +89,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
last_read = _report.timestamp;
unlock();
return sizeof(struct mag_report);
} else {
/* no new data available, warn caller */
return -EAGAIN;
@@ -95,25 +100,31 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
{
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
return OK; // Pretend that this stuff is supported to keep APM happy
}
return OK; // Pretend that this stuff is supported to keep APM happy
}
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
}
std::memcpy(&_scale, reinterpret_cast<const void *>(arg), sizeof(_scale));
return 0;
}
case MAGIOCGSCALE: {
std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
return 0;
}
std::memcpy(reinterpret_cast<void *>(arg), &_scale, sizeof(_scale));
return 0;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
@@ -121,23 +132,25 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
&msg)
{
lock();
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
_report.timestamp = msg.getMonotonicTimestamp().toUSec();
_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
unlock();
publish(msg.getSrcNodeID().get(), &_report);
src/modules/uavcan/sensors/mag.hpp
+4 -4
View File
@@ -40,7 +40,7 @@
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
@@ -57,14 +57,14 @@ private:
ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &) >
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
mag_scale _scale = {};
mag_report _report = {};
};
src/modules/uavcan/sensors/sensor_bridge.cpp
+7 -1
View File
@@ -45,7 +45,7 @@
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
@@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _channels;
}
@@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
_out_of_channels = true;
DEVICE_LOG("out of class instances");
@@ -128,6 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
@@ -136,11 +139,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
out += 1;
}
}
return out;
}
@@ -152,6 +157,7 @@ void UavcanCDevSensorBridgeBase::print_status() const
if (_channels[i].node_id >= 0) {
printf("channel %d: node id %d --> class instance %d\n",
i, _channels[i].node_id, _channels[i].class_instance);
} else {
printf("channel %d: empty\n", i);
}
src/modules/uavcan/sensors/sensor_bridge.hpp
+10 -11
View File
@@ -45,7 +45,7 @@
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
@@ -77,7 +77,7 @@ public:
* Sensor bridge factory.
* Creates all known sensor bridges and puts them in the linked list.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
};
/**
@@ -86,8 +86,7 @@ public:
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
struct Channel {
int node_id = -1;
orb_advert_t orb_advert = nullptr;
int class_instance = -1;
@@ -104,13 +103,13 @@ protected:
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
{
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;
src/modules/uavcan/uavcan_main.cpp
+211 -113
View File
@@ -53,15 +53,12 @@
#include <drivers/drv_pwm_output.h>
#include "uavcan_main.hpp"
#include <uavcan/util/templates.hpp>
#if defined(USE_FW_NODE_SERVER)
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
#endif
/**
* @file uavcan_main.cpp
@@ -76,33 +73,28 @@
* UavcanNode
*/
UavcanNode *UavcanNode::_instance;
#if defined(USE_FW_NODE_SERVER)
uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
uavcan_posix::FirmwareVersionChecker fw_version_checker;
#endif
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_node_mutex(),
#if !defined(USE_FW_NODE_SERVER)
_esc_controller(_node)
#else
_esc_controller(_node),
_fileserver_backend(_node),
_node_info_retriever(_node),
_fw_upgrade_trigger(_node, fw_version_checker),
_fw_server(_node, _fileserver_backend)
#endif
{
_task_should_exit = false;
_fw_server_action = None;
_fw_server_status = -1;
_tx_injector = nullptr;
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
const int res = pthread_mutex_init(&_node_mutex, nullptr);
int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
res = sem_init(&_server_command_sem, 0 , 0);
if (res < 0) {
std::abort();
@@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
UavcanNode::~UavcanNode()
{
fw_server(Stop);
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
@@ -161,13 +157,127 @@ UavcanNode::~UavcanNode()
perf_free(_perfcnt_node_spin_elapsed);
perf_free(_perfcnt_esc_mixer_output_elapsed);
perf_free(_perfcnt_esc_mixer_total_elapsed);
#if defined(USE_FW_NODE_SERVER)
delete(_server_instance);
#endif
pthread_mutex_destroy(&_node_mutex);
sem_destroy(&_server_command_sem);
}
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
{
int rv = -1;
if (UavcanNode::instance()) {
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
rv = 0;
}
return rv;
}
int UavcanNode::start_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
rv = UavcanServers::start(2, _node);
if (rv >= 0) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().installRxFrameListener(driver.get());
* would require a dynamic cast and rtti is not enabled.
*/
UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
}
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::request_fw_check()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
_servers->requestCheckAllNodesFirmwareAndUpdate();
rv = 0;
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::stop_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
_tx_injector = nullptr;
rv = _servers->stop();
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::fw_server(eServerAction action)
{
int rv = -EAGAIN;
switch (action) {
case Start:
case Stop:
case CheckFW:
if (_fw_server_action == None) {
_fw_server_action = action;
sem_wait(&_server_command_sem);
rv = _fw_server_status;
}
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
@@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
return -1;
}
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
@@ -248,7 +363,7 @@ void UavcanNode::fill_node_info()
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
@@ -256,21 +371,7 @@ void UavcanNode::fill_node_info()
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
getHardwareVersion(hwver);
_node.setHardwareVersion(hwver);
}
@@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
br = br->getSibling();
}
#if defined(USE_FW_NODE_SERVER)
/* Initialize the fw version checker.
* giving it it's path
*/
ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
return ret;
}
/* Create dynamic node id server for the Firmware updates directory */
_server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer);
if (_server_instance == 0) {
return -ENOMEM;
}
/* Initialize the dynamic node id server */
ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id);
if (ret < 0) {
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath());
if (ret < 0) {
return ret;
}
#endif
/* Start the Node */
return _node.start();
@@ -398,6 +431,11 @@ void UavcanNode::node_spin_once()
warnx("node spin error %i", spin_res);
}
if (_tx_injector != nullptr) {
_tx_injector->injectTxFramesInto(_node);
}
perf_end(_perfcnt_node_spin_elapsed);
}
@@ -446,7 +484,7 @@ int UavcanNode::run()
* IO multiplexing shall be done here.
*/
_node.setStatusOk();
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
@@ -466,6 +504,24 @@ int UavcanNode::run()
while (!_task_should_exit) {
switch (_fw_server_action) {
case Start:
_fw_server_status = start_fw_server();
break;
case Stop:
_fw_server_status = stop_fw_server();
break;
case CheckFW:
_fw_server_status = request_fw_check();
break;
case None:
default:
break;
}
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
@@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
int
UavcanNode::teardown()
{
sem_post(&_server_command_sem);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
@@ -812,7 +870,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start|status|stop|arm|disarm}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[])
::exit(1);
}
bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (fw && UavcanServers::instance() == nullptr) {
int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
if (rv < 0) {
warnx("Firmware Server Failed to Start %d", rv);
::exit(rv);
}
::exit(0);
}
// Already running, no error
warnx("already started");
::exit(0);
@@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[])
errx(1, "application not running");
}
if (fw && !std::strcmp(argv[1], "update")) {
if (UavcanServers::instance() == nullptr) {
errx(1, "firmware server is not running");
}
int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
::exit(rv);
}
if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
::exit(0);
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
::exit(0);
@@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[])
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);
if (fw) {
int rv = inst->fw_server(UavcanNode::Stop);
if (rv < 0) {
warnx("Firmware Server Failed to Stop %d", rv);
::exit(rv);
}
::exit(0);
} else {
delete inst;
::exit(0);
}
}
print_usage();
src/modules/uavcan/uavcan_main.hpp
+14 -26
View File
@@ -48,13 +48,7 @@
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
#if defined(USE_FW_NODE_SERVER)
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
#endif
# include "uavcan_servers.hpp"
/**
* @file uavcan_main.hpp
@@ -65,10 +59,6 @@
*/
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH PX4_ROOTFSDIR"/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH PX4_ROOTFSDIR"/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
@@ -84,8 +74,7 @@ class UavcanNode : public device::CDev
static constexpr unsigned PollTimeoutMs = 10;
static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
/*
* This memory is reserved for uavcan to use for queuing CAN frames.
* At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -99,11 +88,12 @@ class UavcanNode : public device::CDev
*/
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
static constexpr unsigned StackSize = 3400;
static constexpr unsigned StackSize = 1600;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
enum eServerAction {None, Start, Stop, CheckFW , Busy};
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
@@ -126,6 +116,9 @@ public:
void print_info();
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
private:
void fill_node_info();
@@ -133,10 +126,14 @@ private:
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
volatile eServerAction _fw_server_action;
int _fw_server_status;
int _armed_sub = -1; ///< uORB subscription of the arming status
actuator_armed_s _armed = {}; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
@@ -151,22 +148,13 @@ private:
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
sem_t _server_command_sem;
UavcanEscController _esc_controller;
#if defined(USE_FW_NODE_SERVER)
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
#endif
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
ITxQueueInjector *_tx_injector;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
src/modules/uavcan/uavcan_params.c
+5 -2
View File
@@ -41,10 +41,13 @@
/**
* Enable UAVCAN.
*
* Enables support for UAVCAN-interfaced actuators and sensors.
* Allowed values:
* 0 - UAVCAN disabled.
* 1 - Enabled support for UAVCAN actuators and sensors.
* 2 - Enabled support for dynamic node ID allocation and firmware update.
*
* @min 0
* @max 1
* @max 2
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
src/modules/uavcan/uavcan_servers.cpp
+293
View File
@@ -0,0 +1,293 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <nuttx/config.h>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <memory>
#include <pthread.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/git_version.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include "uavcan_main.hpp"
#include "uavcan_servers.hpp"
#include "uavcan_virtual_can_driver.hpp"
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
/**
* @file uavcan_servers.cpp
*
* Implements basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* David Sidrane <david_s5@nscdg.com>
*/
/*
* UavcanNode
*/
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
_main_node(main_node),
_tracer(),
_storage_backend(),
_fw_version_checker(),
_server_instance(_subnode, _storage_backend, _tracer),
_fileserver_backend(_subnode),
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_mutex_inited(false),
_check_fw(false)
{
}
UavcanServers::~UavcanServers()
{
if (_mutex_inited) {
(void)Lock::deinit(_subnode_mutex);
}
_main_node.getDispatcher().removeRxFrameListener();
}
int UavcanServers::stop(void)
{
UavcanServers *server = instance();
if (server == nullptr) {
warnx("Already stopped");
return -1;
}
_instance = nullptr;
if (server->_subnode_thread != -1) {
pthread_cancel(server->_subnode_thread);
pthread_join(server->_subnode_thread, NULL);
}
server->_main_node.getDispatcher().removeRxFrameListener();
delete server;
return 0;
}
int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* Node init
*/
_instance = new UavcanServers(main_node);
if (_instance == nullptr) {
warnx("Out of memory");
return -2;
}
int rv = _instance->init(num_ifaces);
if (rv < 0) {
warnx("Node init failed: %d", rv);
delete _instance;
_instance = nullptr;
return rv;
}
/*
* Start the thread. Normally it should never exit.
*/
pthread_attr_t tattr;
struct sched_param param;
pthread_attr_init(&tattr);
tattr.stacksize = StackSize;
param.sched_priority = Priority;
pthread_attr_setschedparam(&tattr, &param);
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
if (rv < 0) {
warnx("pthread_create() failed: %d", errno);
rv = -errno;
delete _instance;
_instance = nullptr;
}
return rv;
}
int UavcanServers::init(unsigned num_ifaces)
{
errno = 0;
/*
* Initialize the mutex.
* giving it its path
*/
int ret = Lock::init(_subnode_mutex);
if (ret < 0) {
warnx("Lock init: %d", errno);
return ret;
}
_mutex_inited = true;
_subnode.setNodeID(_main_node.getNodeID());
_main_node.getDispatcher().installRxFrameListener(&_vdriver);
/*
* Initialize the fw version checker.
* giving it its path
*/
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
warnx("BasicFileServer init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
warnx("FileStorageBackend init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = _tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
warnx("FileEventTracer init: %d, errno: %d", ret, errno);
return ret;
}
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
UavcanNode::getHardwareVersion(hwver);
/* Initialize the dynamic node id server */
ret = _server_instance.init(hwver.unique_id);
if (ret < 0) {
warnx("CentralizedServer init: %d", ret);
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
warnx("NodeInfoRetriever init: %d", ret);
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
if (ret < 0) {
warnx("FirmwareUpdateTrigger init: %d", ret);
return ret;
}
/* Start the Node */
return OK;
}
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
Lock lock(_subnode_mutex);
while (1) {
if (_check_fw == true) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
}
warnx("exiting.");
return (pthread_addr_t) 0;
}
src/modules/uavcan/uavcan_servers.hpp
+145
View File
@@ -0,0 +1,145 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/device/device.h>
#include <systemlib/perf_counter.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
# include "uavcan_virtual_can_driver.hpp"
/**
* @file uavcan_servers.hpp
*
* Defines basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author David Sidrane <david_s5@nscdg.com>
*/
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
/**
* A UAVCAN Server Sub node.
*/
class UavcanServers
{
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
static constexpr unsigned MaxCanFramsPerTransfer = 63;
/**
* This number is based on the worst case max number of frames per interface. With
* MemPoolBlockSize set at 48 this is 6048 Bytes.
*
* The servers can be forced to use the primary interface only, this can be achieved simply by passing
* 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
*/
static constexpr unsigned QueuePoolSize =
(UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer);
static constexpr unsigned StackSize = 3500;
static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode;
public:
UavcanServers(uavcan::INode &main_node);
virtual ~UavcanServers();
static int start(unsigned num_ifaces, uavcan::INode &main_node);
static int stop(void);
SubNode &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
/*
* Set the main node's pointer to to the injector
* This is a work around as main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
int init(unsigned num_ifaces);
void deinit(void);
pthread_addr_t run(pthread_addr_t);
static UavcanServers *_instance; ///< singleton pointer
typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
vCanDriver _vdriver;
uavcan::SubNode<MemPoolSize> _subnode; ///< library instance
uavcan::INode &_main_node; ///< library instance
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
uavcan_posix::FirmwareVersionChecker _fw_version_checker;
uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
bool _mutex_inited;
volatile bool _check_fw;
};
src/modules/uavcan/uavcan_virtual_can_driver.hpp
+506
View File
@@ -0,0 +1,506 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Pavel Kirienko <pavel.kirienko@zubax.com>
* David Sidrane <david_s5@nscdg.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.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <cstdlib>
#include <cstdint>
#include <cstring>
#include <cstdio>
#include <fcntl.h>
#include <pthread.h>
#include <semaphore.h>
#include <debug.h>
#include <uavcan_stm32/clock.hpp>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
/*
* General purpose wrapper around os's mutual exclusion
* mechanism.
*
* It supports the
*
* Note the naming of thier_mutex_ implies that the underlying
* mutex is owned by class using the Lock.
* and this wrapper provides service to initialize and de initialize
* the mutex.
*/
class Lock
{
pthread_mutex_t &thier_mutex_;
public:
Lock(pthread_mutex_t &m) :
thier_mutex_(m)
{
(void)pthread_mutex_lock(&m);
}
~Lock()
{
(void)pthread_mutex_unlock(&thier_mutex_);
}
static int init(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_init(&thier_mutex_, NULL);
}
static int deinit(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_destroy(&thier_mutex_);
}
};
/**
* Generic queue based on the linked list class defined in libuavcan.
* This class does not use heap memory.
*/
template <typename T>
class Queue
{
struct Item : public uavcan::LinkedListNode<Item> {
T payload;
template <typename... Args>
Item(Args... args) : payload(args...) { }
};
uavcan::LimitedPoolAllocator allocator_;
uavcan::LinkedListRoot<Item> list_;
public:
Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
allocator_(arg_allocator, block_allocation_quota)
{
uavcan::IsDynamicallyAllocatable<Item>::check();
}
bool isEmpty() const { return list_.isEmpty(); }
/**
* Creates one item in-place at the end of the list.
* Returns true if the item was appended successfully, false if there's not enough memory.
* Complexity is O(N) where N is queue length.
*/
template <typename... Args>
bool tryEmplace(Args... args)
{
// Allocating memory
void *const ptr = allocator_.allocate(sizeof(Item));
if (ptr == nullptr) {
return false;
}
// Constructing the new item
Item *const item = new(ptr) Item(args...);
assert(item != nullptr);
// Inserting the new item at the end of the list
Item *p = list_.get();
if (p == nullptr) {
list_.insert(item);
} else {
while (p->getNextListNode() != nullptr) {
p = p->getNextListNode();
}
assert(p->getNextListNode() == nullptr);
p->setNextListNode(item);
assert(p->getNextListNode()->getNextListNode() == nullptr);
}
return true;
}
/**
* Accesses the first element.
* Nullptr will be returned if the queue is empty.
* Complexity is O(1).
*/
T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
/**
* Removes the first element.
* If the queue is empty, nothing will be done and assertion failure will be triggered.
* Complexity is O(1).
*/
void pop()
{
Item *const item = list_.get();
assert(item != nullptr);
if (item != nullptr) {
list_.remove(item);
item->~Item();
allocator_.deallocate(item);
}
}
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
*/
class VirtualCanIface : public uavcan::ICanIface,
uavcan::Noncopyable
{
/**
* This class re-defines uavcan::RxCanFrame with flags.
* Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
* rendering this class unusable with Queue<>.
*/
struct RxItem: public uavcan::CanFrame {
const uavcan::MonotonicTime ts_mono;
const uavcan::UtcTime ts_utc;
const uavcan::CanIOFlags flags;
const uint8_t iface_index;
RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
uavcan::CanFrame(arg_frame),
ts_mono(arg_frame.ts_mono),
ts_utc(arg_frame.ts_utc),
flags(arg_flags),
iface_index(arg_frame.iface_index)
{
// Making sure it will fit into a pool block with a pointer prefix
static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
}
};
pthread_mutex_t &common_driver_mutex_;
uavcan::CanTxQueue prioritized_tx_queue_;
Queue<RxItem> rx_queue_;
int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
{
Lock lock(common_driver_mutex_);
prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
return 1;
}
int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
{
Lock lock(common_driver_mutex_);
if (rx_queue_.isEmpty()) {
return 0;
}
const auto item = *rx_queue_.peek();
rx_queue_.pop();
out_frame = item;
out_ts_monotonic = item.ts_mono;
out_ts_utc = item.ts_utc;
out_flags = item.flags;
return 1;
}
int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
uint16_t getNumFilters() const override { return 0; }
uint64_t getErrorCount() const override { return 0; }
public:
VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
common_driver_mutex_(arg_mutex),
prioritized_tx_queue_(allocator, clock, quota_per_queue),
rx_queue_(allocator, quota_per_queue)
{
}
~VirtualCanIface()
{
}
/**
* Note that RX queue overwrites oldest items when overflowed.
* Call this from the main thread only.
* No additional locking is required.
*/
void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
{
Lock lock(common_driver_mutex_);
if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
rx_queue_.pop();
(void)rx_queue_.tryEmplace(frame, flags);
}
}
/**
* Call this from the main thread only.
* No additional locking is required.
*/
void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
{
Lock lock(common_driver_mutex_);
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
while (auto e = prioritized_tx_queue_.peek()) {
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
unsigned(iface_mask), e->toString().c_str());
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
uavcan::CanTxQueue::Qos(e->qos), e->flags);
prioritized_tx_queue_.remove(e);
if (res <= 0) {
break;
}
}
}
/**
* Call this from the sub-node thread only.
* No additional locking is required.
*/
bool hasDataInRxQueue()
{
Lock lock(common_driver_mutex_);
return !rx_queue_.isEmpty();
}
};
/**
* This interface defines one method that will be called by the main node thread periodically in order to
* transfer contents of TX queue of the sub-node into the TX queue of the main node.
*/
class ITxQueueInjector
{
public:
virtual ~ITxQueueInjector() { }
/**
* Flush contents of TX queues into the main node.
* @param main_node Reference to the main node.
*/
virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
* @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
* memory pool that will be shared across all interfaces for RX/TX queues.
* Typically this value should be no less than 4K per interface.
*/
template <unsigned SharedMemoryPoolSize>
class VirtualCanDriver : public uavcan::ICanDriver,
public uavcan::IRxFrameListener,
public ITxQueueInjector,
uavcan::Noncopyable
{
class Event
{
FAR sem_t sem;
public:
int init()
{
return sem_init(&sem, 0, 0);
}
int deinit()
{
return sem_destroy(&sem);
}
Event()
{
}
~Event()
{
}
/**
*/
void waitFor(uavcan::MonotonicDuration duration)
{
static const int NsPerSec = 1000000000;
if (duration.isPositive()) {
auto abstime = ::timespec();
if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
abstime.tv_nsec += duration.toUSec() * 1000;
if (abstime.tv_nsec >= NsPerSec) {
abstime.tv_sec++;
abstime.tv_nsec -= NsPerSec;
}
(void)sem_timedwait(&sem, &abstime);
}
}
}
void signal()
{
int count;
int rv = sem_getvalue(&sem, &count);
if (rv > 0 && count <= 0) {
sem_post(&sem);
}
}
};
Event event_; ///< Used to unblock the select() call when IO happens.
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
const unsigned num_ifaces_;
uavcan::ISystemClock &clock_;
uavcan::ICanIface *getIface(uint8_t iface_index) override
{
return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
}
uint8_t getNumIfaces() const override { return num_ifaces_; }
/**
* This and other methods of ICanDriver will be invoked by the sub-node thread.
*/
int16_t select(uavcan::CanSelectMasks &inout_masks,
const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override
{
bool need_block = (inout_masks.write == 0); // Write queue is infinite
for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
const bool need_read = inout_masks.read & (1U << i);
if (need_read && ifaces_[i]->hasDataInRxQueue()) {
need_block = false;
}
}
if (need_block) {
event_.waitFor(blocking_deadline - clock_.getMonotonic());
}
inout_masks = uavcan::CanSelectMasks();
for (unsigned i = 0; i < num_ifaces_; i++) {
const std::uint8_t iface_mask = 1U << i;
inout_masks.write |= iface_mask; // Always ready to write
if (ifaces_[i]->hasDataInRxQueue()) {
inout_masks.read |= iface_mask;
}
}
return num_ifaces_; // We're always ready to write, hence > 0.
}
/**
* This handler will be invoked by the main node thread.
*/
void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
{
UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
if (frame.iface_index < num_ifaces_) {
ifaces_[frame.iface_index]->addRxFrame(frame, flags);
event_.signal();
}
}
/**
* This method will be invoked by the main node thread.
*/
void injectTxFramesInto(uavcan::INode &main_node) override
{
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i]->flushTxQueueTo(main_node, i);
}
event_.signal();
}
public:
VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
num_ifaces_(arg_num_ifaces),
clock_(system_clock)
{
Lock::init(driver_mutex_);
event_.init();
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &,
pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue);
}
}
~VirtualCanDriver()
{
Lock::deinit(driver_mutex_);
event_.deinit();
}
};
src/modules/vtol_att_control/tiltrotor.cpp
+6 -2
View File
@@ -342,10 +342,14 @@ void Tiltrotor::fill_actuator_outputs()
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
}
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[4] = _tilt_control;
src/platforms/common/module.mk
+1 -2
View File
@@ -2,6 +2,5 @@
# Common OS porting APIs
#
SRCS = px4_getopt.c \
px4_log.c
SRCS = px4_getopt.c
src/platforms/common/px4_log.c
-5
View File
@@ -1,5 +0,0 @@
#include <px4_log.h>
__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" };
src/platforms/nuttx/px4_layer/module.mk
+2 -1
View File
@@ -35,6 +35,7 @@
# NuttX porting layer
#
SRCS = px4_nuttx_tasks.c
SRCS = px4_nuttx_tasks.c \
../../posix/px4_layer/px4_log.c
MAXOPTIMIZATION = -Os
src/platforms/posix/main.cpp
+5
View File
@@ -96,6 +96,11 @@ int main(int argc, char **argv)
if (argc == 2) {
ifstream infile(argv[1]);
if (!infile) {
cout << "failed opening script" << argv[1] << std::endl;
return 1;
}
for (string line; getline(infile, line, '\n'); ) {
process_line(line);
}
src/platforms/posix/px4_layer/module.mk
+1
View File
@@ -40,5 +40,6 @@ SRCS = \
px4_posix_tasks.cpp \
lib_crc32.c \
drv_hrt.c \
px4_log.c
MAXOPTIMIZATION = -Os
src/platforms/posix/px4_layer/px4_log.c
+5
View File
@@ -0,0 +1,5 @@
#include <px4_log.h>
__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };
src/platforms/px4_log.h
+80 -15
View File
@@ -38,6 +38,24 @@
#pragma once
#define _PX4_LOG_LEVEL_ALWAYS 0
#define _PX4_LOG_LEVEL_DEBUG 1
#define _PX4_LOG_LEVEL_WARN 2
#define _PX4_LOG_LEVEL_ERROR 3
#define _PX4_LOG_LEVEL_PANIC 4
// Used to silence unused variable warning
static inline void do_nothing(int level, ...)
{
(void)level;
}
/****************************************************************************
* __px4_log_omit:
* Compile out the message
****************************************************************************/
#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__)
#if defined(__PX4_ROS)
#include <ros/console.h>
@@ -47,6 +65,54 @@
#define PX4_INFO(...) ROS_WARN(__VA_ARGS__)
#define PX4_DEBUG(...)
#elif defined(__PX4_QURT)
#include "qurt_log.h"
/****************************************************************************
* Messages that should never be filtered or compiled out
****************************************************************************/
#define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#if defined(TRACE_BUILD)
/****************************************************************************
* Extremely Verbose settings for a Trace build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#elif defined(DEBUG_BUILD)
/****************************************************************************
* Verbose settings for a Debug build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#elif defined(RELEASE_BUILD)
/****************************************************************************
* Non-verbose settings for a Release build to minimize strings in build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
#else
/****************************************************************************
* Medium verbose settings for a default build
****************************************************************************/
#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
#endif
#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
#else
#define __STDC_FORMAT_MACROS
@@ -59,18 +125,6 @@
__BEGIN_DECLS
__EXPORT extern uint64_t hrt_absolute_time(void);
// Used to silence unused variable warning
static inline void do_nothing(int level, ...)
{
(void)level;
}
#define _PX4_LOG_LEVEL_ALWAYS 0
#define _PX4_LOG_LEVEL_PANIC 1
#define _PX4_LOG_LEVEL_ERROR 2
#define _PX4_LOG_LEVEL_WARN 3
#define _PX4_LOG_LEVEL_DEBUG 4
__EXPORT extern const char *__px4_log_level_str[5];
__EXPORT extern int __px4_log_level_current;
@@ -107,11 +161,22 @@ __EXPORT extern int __px4_log_level_current;
* Use these to implement the code level macros below
****************************************************************************/
/****************************************************************************
* __px4_log_omit:
* Compile out the message
* __px4_log_named_cond:
* Convert a message in the form:
* PX4_LOG_COND(__dbg_enabled, "val is %d", val);
* to
* printf("%-5s val is %d\n", "LOG", val);
* if the first arg/condition is true.
****************************************************************************/
#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__)
#define __px4_log_named_cond(name, cond, FMT, ...) \
__px4__log_startcond(cond)\
"%s " \
FMT\
__px4__log_end_fmt \
,name, ##__VA_ARGS__\
__px4__log_endline
/****************************************************************************
* __px4_log_named_cond:
src/platforms/px4_workqueue.h
+4
View File
@@ -47,6 +47,10 @@
#include <queue.h>
#include <px4_platform_types.h>
#ifdef __PX4_QURT
#include <dspal_types.h>
#endif
__BEGIN_DECLS
#define HPWORK 0
src/platforms/qurt/eagle_px4_firmware_patch.patch
+190
View File
@@ -0,0 +1,190 @@
diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index 4ba2f98..a7b664e 100644
--- a/src/include/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
-#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
- fprintf(stderr, "telem> "); \
+#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
+/* fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n");
+*/
/**
* Send a mavlink critical message and print to console.
@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
-#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
- fprintf(stderr, "telem> "); \
+#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__)
+/* fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n");
+*/
/**
* Send a mavlink emergency message and print to console.
@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
-#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
- fprintf(stderr, "telem> "); \
+#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
+/* fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n");
-
+*/
struct mavlink_logmessage {
char text[MAVLINK_LOG_MAXLEN + 1];
unsigned char severity;
diff --git a/src/lib/eigen b/src/lib/eigen
--- a/src/lib/eigen
+++ b/src/lib/eigen
@@ -1 +1 @@
-Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71
+Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index bbf5f8e..50c7d8b 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -378,6 +378,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
}
+
+#ifdef __PX4_QURT
+ // WARNING: Preflight checks are important and should be added back when
+ // all the sensors are supported
+ PX4_WARN("WARNING: Preflight checks PASS always.");
+ failed = false;
+#endif
+
/* Report status */
return !failed;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 971e086..c165322 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* RC input check */
+#ifndef __PX4_QURT
if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
- hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
+ hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
+#else
+ // HACK: remove old data check due to timestamp issue in QURT
+ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 ) {
+#endif
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@@ -2526,7 +2531,7 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
- control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
+ //control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
switch (status.nav_state) {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 892e50f..7ac6776 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
};
/* wait for initial baro value */
+#ifdef __PX4_QURT
+ // WARNING skipping
+ PX4_WARN("Hacked to skip baro initialization for testing.");
+ bool wait_baro = false;
+#else
bool wait_baro = true;
+#endif
thread_running = true;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 3fa4458..0ef1883 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.gyro_raw[1] = gyro_report.y_raw;
raw.gyro_raw[2] = gyro_report.z_raw;
+#ifdef __PX4_QURT
+// Temporary fix required due to lack of time sync between apps and dsp cores
+ raw.timestamp = hrt_absolute_time();
+#else
raw.timestamp = gyro_report.timestamp;
+#endif
raw.gyro_errcount = gyro_report.error_count;
raw.gyro_temp = gyro_report.temperature;
}
@@ -2068,6 +2073,7 @@ Sensors::task_main()
/* start individual sensors */
int ret = 0;
+#if 0
do { /* create a scope to handle exit with break */
ret = accel_init();
if (ret) break;
@@ -2091,7 +2097,7 @@ Sensors::task_main()
}
return;
}
-
+#endif
/*
* do subscriptions
*/
diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
index 83a9378..ec86faf 100644
--- a/src/systemcmds/mixer/mixer.cpp
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -118,6 +118,8 @@ load(const char *devname, const char *fname)
return 1;
}
+#ifndef __PX4_QURT
+
if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
warnx("can't load mixer: %s", fname);
return 1;
@@ -125,6 +127,24 @@ load(const char *devname, const char *fname)
/* XXX pass the buffer to the device */
int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
+#else
+ char newbuf[] =
+ "R: 4x 10000 10000 10000 0\n"
+ "M: 1\n"
+ "O: 10000 10000 0 -10000 10000\n"
+ "S: 0 4 10000 10000 0 -10000 10000\n"
+ "M: 1\n"
+ "O: 10000 10000 0 -10000 10000\n"
+ "S: 0 5 10000 10000 0 -10000 10000\n"
+ "M: 1\n"
+ "O: 10000 10000 0 -10000 10000\n"
+ "S: 0 6 10000 10000 0 -10000 10000\n"
+ "M: 1\n"
+ "O: 10000 10000 0 -10000 10000\n"
+ "S: 0 7 10000 10000 0 -10000 10000\n";
+ /* XXX pass the buffer to the device */
+ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf);
+#endif
if (ret < 0) {
warnx("error loading mixers from %s", fname);
src/platforms/qurt/include/qurt_log.h
+64
View File
@@ -0,0 +1,64 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef QURT_LOG_H
#define QURT_LOG_H
#include <stdarg.h>
#include <stdio.h>
#ifdef __cplusplus
extern "C" {
#endif
//void qurt_log(int level, const char *file, int line, const char *format, ...);
// declaration to make the compiler happy. This symbol is part of the adsp static image.
void HAP_debug(const char *msg, int level, const char *filename, int line);
static __inline void qurt_log(int level, const char *file, int line,
const char *format, ...)
{
char buf[256];
va_list args;
va_start(args, format);
vsnprintf(buf, sizeof(buf), format, args);
va_end(args);
HAP_debug(buf, level, file, line);
}
#ifdef __cplusplus
}
#endif
#endif // QURT_LOG_H
src/platforms/qurt/px4_layer/commands_hil.c
+1 -1
View File
@@ -48,7 +48,7 @@ const char *get_commands()
"param set CAL_MAG0_ID 196608\n"
// "rgbled start\n"
// "tone_alarm start\n"
"commander start\n"
"commander start -hil\n"
"sensors start\n"
//"ekf_att_pos_estimator start\n"
"attitude_estimator_q start\n"
src/platforms/qurt/px4_layer/drv_hrt.c
+4 -12
View File
@@ -46,8 +46,6 @@
static struct sq_queue_s callout_queue;
extern uint64_t get_abs_time_in_us();
/* latency histogram */
#define LATENCY_BUCKET_COUNT 8
__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
@@ -83,15 +81,9 @@ static void hrt_unlock(void)
*/
hrt_abstime hrt_absolute_time(void)
{
return get_abs_time_in_us();
/*
struct timespec ts;
// FIXME - clock_gettime unsupported in QuRT
//clock_gettime(CLOCK_MONOTONIC, &ts);
clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts);
*/
}
/*
@@ -252,7 +244,7 @@ hrt_call_reschedule()
hrt_abstime now = hrt_absolute_time();
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000);
uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX);
//printf("hrt_call_reschedule\n");
@@ -273,11 +265,11 @@ hrt_call_reschedule()
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
//lldbg("pre-expired\n");
/* set a minimal deadline so that we call ASAP */
ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
ticks = USEC2TICK(HRT_INTERVAL_MIN);
} else if (next->deadline < deadline) {
//lldbg("due soon\n");
ticks = USEC2TICK((next->deadline - now)*1000);
ticks = USEC2TICK((next->deadline - now));
}
}

Some files were not shown because too many files have changed in this diff Show More