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

Merge remote-tracking branch 'px4/master' into mavlink_broadcast

Browse Source
This commit is contained in:
Julian Oes 2014-04-07 21:45:45 +02:00
commit fc2bfb828f
35 changed files with 1059 additions and 747 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ROMFS/px4fmu_common/init.d/rcS
View File

@ -294,6 +294,7 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
then
echo "[init] Use FMU as primary output"
@ -317,6 +318,7 @@ then
fi
fi
fi
if [ $OUTPUT_MODE == mkblctrl ]
then
echo "[init] Use MKBLCTRL as primary output"
@ -338,7 +340,8 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
fi
if [ $OUTPUT_MODE == hil ]
then
echo "[init] Use HIL as primary output"
@ -396,34 +399,29 @@ then
#
# MAVLink
#
if [ $MAVLINK_FLAGS == default ]
then
if [ $HIL == yes ]
then
sleep 1
set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
usleep 5000
else
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
if [ $TTYS1_BUSY == yes ]
then
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
usleep 5000
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
# Start MAVLink on default port: ttyS1
set MAVLINK_FLAGS "-r 1000"
usleep 5000
fi
fi
fi
mavlink start $MAVLINK_FLAGS
usleep 5000
#
# Start the datamanager

28
Tools/px_uploader.py
View File

@ -389,18 +389,22 @@ class uploader(object):
self.otp_pid = self.otp[12:8:-1]
self.otp_coa = self.otp[32:160]
# show user:
print("type: " + self.otp_id.decode('Latin-1'))
print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
print("sn: ", end='')
for byte in range(0,12,4):
x = self.__getSN(byte)
x = x[::-1] # reverse the bytes
self.sn = self.sn + x
print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
print('')
try:
print("type: " + self.otp_id.decode('Latin-1'))
print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
print("sn: ", end='')
for byte in range(0,12,4):
x = self.__getSN(byte)
x = x[::-1] # reverse the bytes
self.sn = self.sn + x
print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
print('')
except Exception:
# ignore bad character encodings
pass
print("erase...")
self.__erase()

1
src/drivers/drv_tone_alarm.h
View File

@ -147,6 +147,7 @@ enum {
TONE_BATTERY_WARNING_SLOW_TUNE,
TONE_BATTERY_WARNING_FAST_TUNE,
TONE_GPS_WARNING_TUNE,
TONE_ARMING_FAILURE_TUNE,
TONE_NUMBER_OF_TUNES
};

36
src/drivers/hmc5883/hmc5883.cpp
View File

@ -169,6 +169,8 @@ private:
int _bus; /**< the bus the device is connected to */
struct mag_report _last_report; /**< used for info() */
/**
* Test whether the device supported by the driver is present at a
* specific address.
@ -870,6 +872,8 @@ HMC5883::collect()
}
}
_last_report = new_report;
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
@ -1042,32 +1046,29 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
goto out;
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
goto out;
}
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
goto out;
}
ret = OK;
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
}
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
@ -1221,6 +1222,7 @@ HMC5883::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,

128
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -87,6 +87,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <drivers/airspeed/airspeed.h>
@ -121,6 +122,14 @@ protected:
virtual int collect();
math::LowPassFilter2p _filter;
/**
* Correct for 5V rail voltage variations
*/
void voltage_correction(float &diff_pres_pa, float &temperature);
int _t_system_power;
struct system_power_s system_power;
};
/*
@ -129,10 +138,11 @@ 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),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1)
{
memset(&system_power, 0, sizeof(system_power));
}
int
@ -194,19 +204,48 @@ MEASAirspeed::collect()
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
float temperature = ((200.0f * dT_raw) / 2047) - 50;
/* calculate differential pressure. As its centered around 8000
* and can go positive or negative, enforce absolute value
*/
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
const float PSI_to_Pa = 6894.757f;
/*
this equation is an inversion of the equation in the
pressure transfer function figure on page 4 of the datasheet
if (diff_press_pa < 0.0f) {
diff_press_pa = 0.0f;
}
We negate the result so that positive differential pressures
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_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
@ -219,6 +258,13 @@ MEASAirspeed::collect()
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@ -287,6 +333,62 @@ MEASAirspeed::cycle()
USEC2TICK(CONVERSION_INTERVAL));
}
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
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;
}
const float slope = 65.0f;
/*
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;
/*
the temperature masurement varies as well
*/
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
}
/**
* Local functions in support of the shell command.
*/
@ -409,7 +511,7 @@ test()
}
warnx("single read");
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@ -437,7 +539,7 @@ test()
}
warnx("periodic read %u", i);
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}

24
src/drivers/px4io/px4io.cpp
View File

@ -944,8 +944,23 @@ PX4IO::task_main()
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
if (pret != OK) {
log("voltage scaling upload failed");
log("vscale upload failed");
}
/* send RC throttle failsafe value to IO */
int32_t failsafe_param_val;
param_t failsafe_param = param_find("RC_FAILS_THR");
if (failsafe_param > 0) {
param_get(failsafe_param, &failsafe_param_val);
uint16_t failsafe_thr = failsafe_param_val;
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
if (pret != OK) {
log("failsafe upload failed");
}
}
}
}
@ -1479,10 +1494,11 @@ PX4IO::io_publish_raw_rc()
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
/* we do not know the RC input, only publish if RC OK flag is set */
/* if no raw RC, just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
/* only keep publishing RC input if we ever got a valid input */
if (_rc_last_valid == 0) {
/* we have never seen valid RC signals, abort */
return OK;
}
}
/* lazily advertise on first publication */

139
src/drivers/sf0x/sf0x.cpp
View File

@ -124,6 +124,8 @@ private:
orb_advert_t _range_finder_topic;
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
_linebuf_index(0),
_last_read(0),
_range_finder_topic(-1),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@ -276,34 +279,11 @@ SF0X::init()
warnx("advert err");
}
/* attempt to get a measurement 5 times */
while (ret != OK && i < 5) {
if (measure()) {
ret = ERROR;
_sensor_ok = false;
}
usleep(100000);
if (collect()) {
ret = ERROR;
_sensor_ok = false;
} else {
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
}
i++;
}
/* close the fd */
::close(_fd);
_fd = -1;
out:
return ret;
return OK;
}
int
@ -376,6 +356,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@ -544,10 +525,16 @@ SF0X::collect()
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
} else if (_linebuf_index > 0) {
/* increment to next read position */
_linebuf_index++;
}
/* the buffer for read chars is buflen minus null termination */
unsigned readlen = sizeof(_linebuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
if (ret < 0) {
_linebuf[sizeof(_linebuf) - 1] = '\0';
@ -562,19 +549,30 @@ SF0X::collect()
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_linebuf_index += ret;
if (_linebuf_index >= sizeof(_linebuf)) {
_linebuf_index = 0;
}
/* we did increment the index to the next position already, so just add the additional fields */
_linebuf_index += (ret - 1);
_last_read = hrt_absolute_time();
if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
/* incomplete read, reschedule ourselves */
if (_linebuf_index < 1) {
/* we need at least the two end bytes to make sense of this string */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
if (_linebuf_index >= readlen - 1) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
return -ENOMEM;
} else {
/* incomplete read, reschedule ourselves */
return -EAGAIN;
}
}
char *end;
@ -582,22 +580,56 @@ SF0X::collect()
bool valid;
/* enforce line ending */
_linebuf[sizeof(_linebuf) - 1] = '\0';
unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
_linebuf[lend] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
valid = false;
} else {
si_units = strtod(_linebuf, &end);
valid = true;
/* we need to find a dot in the string, as we're missing the meters part else */
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (int i = 0; i < (lend - 2); i++) {
if (_linebuf[i] == '\n') {
char buf[sizeof(_linebuf)];
memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
}
if (_linebuf[i] == '.') {
valid = true;
}
}
if (valid) {
si_units = strtod(_linebuf, &end);
/* we require at least 3 characters for a valid number */
if (end > _linebuf + 3) {
valid = true;
} else {
si_units = -1.0f;
valid = false;
}
}
}
debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting */
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
/* if its invalid, there is no reason to forward the value */
if (!valid) {
perf_count(_comms_errors);
return -EINVAL;
}
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@ -689,10 +721,19 @@ SF0X::cycle()
}
if (OK != collect_ret) {
log("collection error");
/* we know the sensor needs about four seconds to initialize */
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
log("collection error #%u", _consecutive_fail_count);
}
_consecutive_fail_count++;
/* restart the measurement state machine */
start();
return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
}
/* next phase is measurement */
@ -848,10 +889,10 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("val: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
/* start the sensor polling at 2 Hz rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
@ -866,24 +907,26 @@ test()
int ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
warnx("timed out");
break;
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
break;
}
warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("read #%u", i);
warnx("val: %0.3f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
/* reset the sensor polling to default rate */
/* reset the sensor polling to the default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
errx(1, "ERR: DEF RATE");
}
errx(0, "PASS");

47
src/drivers/stm32/adc/adc.cpp
View File

@ -41,6 +41,7 @@
*/
#include <nuttx/config.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <sys/types.h>
@ -64,6 +65,8 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/system_power.h>
/*
* Register accessors.
* For now, no reason not to just use ADC1.
@ -119,6 +122,8 @@ private:
unsigned _channel_count;
adc_msg_s *_samples; /**< sample buffer */
orb_advert_t _to_system_power;
/** work trampoline */
static void _tick_trampoline(void *arg);
@ -134,13 +139,16 @@ private:
*/
uint16_t _sample(unsigned channel);
// update system_power ORB topic, only on FMUv2
void update_system_power(void);
};
ADC::ADC(uint32_t channels) :
CDev("adc", ADC_DEVICE_PATH),
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
_channel_count(0),
_samples(nullptr)
_samples(nullptr),
_to_system_power(0)
{
_debug_enabled = true;
@ -290,6 +298,43 @@ ADC::_tick()
/* scan the channel set and sample each */
for (unsigned i = 0; i < _channel_count; i++)
_samples[i].am_data = _sample(_samples[i].am_channel);
update_system_power();
}
void
ADC::update_system_power(void)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
system_power_s system_power;
system_power.timestamp = hrt_absolute_time();
system_power.voltage5V_v = 0;
for (unsigned i = 0; i < _channel_count; i++) {
if (_samples[i].am_channel == 4) {
// it is 2:1 scaled
system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
}
}
// these are not ADC related, but it is convenient to
// publish these to the same topic
system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
// note that the valid pins are active low
system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
// OC pins are active low
system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
/* lazily publish */
if (_to_system_power > 0) {
orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
} else {
_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
}
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
uint16_t

2
src/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow
_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast
_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow
_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@ -344,6 +345,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow
_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast
_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning
_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm
}
ToneAlarm::~ToneAlarm()

7
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
// don't allow bad values to propogate via the filter
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
return output;
}
float LowPassFilter2p::reset(float sample) {
_delay_element_1 = _delay_element_2 = sample;
return apply(sample);
}
} // namespace math

20
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -52,18 +52,30 @@ public:
_delay_element_1 = _delay_element_2 = 0;
}
// change parameters
/**
* Change filter parameters
*/
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// apply - Add a new raw value to the filter
// and retrieve the filtered result
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
float apply(float sample);
// return the cutoff frequency
/**
* Return the cutoff frequency
*/
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
/**
* Reset the filter state to this value
*/
float reset(float sample);
private:
float _cutoff_freq;
float _a1;

14
src/modules/commander/airspeed_calibration.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2500;
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)

13
src/modules/commander/commander.cpp
View File

@ -118,8 +118,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
#define RC_TIMEOUT 100000
#define RC_TIMEOUT_HIL 500000
#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[])
}
}
/*
* XXX workaround:
* Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz)
* which can trigger RC loss if the computer/simulator lags.
*/
uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT;
/* start RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) {
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;

18
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -660,18 +660,24 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s) ||
/* if airspeed is not updating, we assume the normal average speed */
if (!isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
} else {
airspeed = _airspeed.indicated_airspeed_m_s;
airspeed = _airspeed.true_airspeed_m_s;
}
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;

213
src/modules/fw_att_pos_estimator/estimator.cpp
View File

@ -2,85 +2,6 @@
#include <string.h>
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float posNED[3]; // North, East Down position (m)
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
// Baro input
float baroHgt;
bool statesInitialised = false;
bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode = true; ///< boolean true if no position feedback is fused
bool useAirspeed = true; ///< boolean true if airspeed data is being used
bool useCompass = true; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection = true;
static unsigned storeIndex = 0;
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
@ -185,7 +106,31 @@ void swap_var(float &d1, float &d2)
d2 = tmp;
}
void UpdateStrapdownEquationsNED()
AttPosEKF::AttPosEKF() :
fusionModeGPS(0),
covSkipCount(0),
EAS2TAS(1.0f),
statesInitialised(false),
fuseVelData(false),
fusePosData(false),
fuseHgtData(false),
fuseMagData(false),
fuseVtasData(false),
onGround(true),
staticMode(true),
useAirspeed(true),
useCompass(true),
numericalProtection(true),
storeIndex(0)
{
}
AttPosEKF::~AttPosEKF()
{
}
void AttPosEKF::UpdateStrapdownEquationsNED()
{
Vector3f delVelNav;
float q00;
@ -247,7 +192,7 @@ void UpdateStrapdownEquationsNED()
qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
// Normalise the quaternions and update the quaternion states
quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
if (quatMag > 1e-16f)
{
float quatMagInv = 1.0f/quatMag;
@ -312,7 +257,7 @@ void UpdateStrapdownEquationsNED()
ConstrainStates();
}
void CovariancePrediction(float dt)
void AttPosEKF::CovariancePrediction(float dt)
{
// scalars
float windVelSigma;
@ -953,7 +898,7 @@ void CovariancePrediction(float dt)
ConstrainVariances();
}
void FuseVelposNED()
void AttPosEKF::FuseVelposNED()
{
// declare variables used by fault isolation logic
@ -999,8 +944,8 @@ void FuseVelposNED()
observation[5] = -(hgtMea);
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
R_OBS[0] = 0.04f + sq(velErr);
R_OBS[1] = R_OBS[0];
R_OBS[2] = 0.08f + sq(velErr);
@ -1026,7 +971,7 @@ void FuseVelposNED()
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
}
// apply a 5-sigma threshold
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
{
@ -1175,7 +1120,7 @@ void FuseVelposNED()
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
}
void FuseMagnetometer()
void AttPosEKF::FuseMagnetometer()
{
uint8_t obsIndex;
uint8_t indexLimit;
@ -1482,7 +1427,7 @@ void FuseMagnetometer()
ConstrainVariances();
}
void FuseAirspeed()
void AttPosEKF::FuseAirspeed()
{
float vn;
float ve;
@ -1503,14 +1448,14 @@ void FuseAirspeed()
// Need to check that it is flying before fusing airspeed data
// Calculate the predicted airspeed
VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
float H_TAS[21];
for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
@ -1611,7 +1556,7 @@ void FuseAirspeed()
ConstrainVariances();
}
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
@ -1624,7 +1569,7 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
}
}
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
@ -1637,13 +1582,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
}
}
float sq(float valIn)
float AttPosEKF::sq(float valIn)
{
return valIn*valIn;
}
// Store states in a history array along with time stamp
void StoreStates(uint64_t timestamp_ms)
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{
for (unsigned i=0; i<n_states; i++)
storedStates[i][storeIndex] = states[i];
@ -1653,7 +1598,7 @@ void StoreStates(uint64_t timestamp_ms)
storeIndex = 0;
}
void ResetStoredStates()
void AttPosEKF::ResetStoredStates()
{
// reset all stored states
memset(&storedStates[0][0], 0, sizeof(storedStates));
@ -1674,7 +1619,7 @@ void ResetStoredStates()
}
// Output the state vector stored at the time that best matches that specified by msec
int RecallStates(float statesForFusion[n_states], uint64_t msec)
int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
{
int ret = 0;
@ -1723,7 +1668,7 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
return ret;
}
void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
{
// Calculate the nav to body cosine matrix
float q00 = sq(quat[0]);
@ -1748,7 +1693,7 @@ void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
Tnb.y.z = 2*(q23 + q01);
}
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
{
// Calculate the body to nav cosine matrix
float q00 = sq(quat[0]);
@ -1773,7 +1718,7 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
Tbn.z.y = 2*(q23 + q01);
}
void eul2quat(float (&quat)[4], const float (&eul)[3])
void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
{
float u1 = cos(0.5f*eul[0]);
float u2 = cos(0.5f*eul[1]);
@ -1787,35 +1732,35 @@ void eul2quat(float (&quat)[4], const float (&eul)[3])
quat[3] = u1*u2*u6-u4*u5*u3;
}
void quat2eul(float (&y)[3], const float (&u)[4])
void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
{
y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
}
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
{
velNED[0] = gpsGndSpd*cos(gpsCourse);
velNED[1] = gpsGndSpd*sin(gpsCourse);
velNED[0] = gpsGndSpd*cosf(gpsCourse);
velNED[1] = gpsGndSpd*sinf(gpsCourse);
velNED[2] = gpsVelD;
}
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
{
posNED[0] = earthRadius * (lat - latRef);
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
posNED[2] = -(hgt - hgtRef);
}
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
{
lat = latRef + posNED[0] * earthRadiusInv;
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNED[2];
}
void OnGroundCheck()
void AttPosEKF::OnGroundCheck()
{
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) {
@ -1823,7 +1768,7 @@ void OnGroundCheck()
}
}
void calcEarthRateNED(Vector3f &omega, float latitude)
void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
{
//Define Earth rotation vector in the NED navigation frame
omega.x = earthRate*cosf(latitude);
@ -1831,13 +1776,13 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
omega.z = -earthRate*sinf(latitude);
}
void CovarianceInit()
void AttPosEKF::CovarianceInit()
{
// Calculate the initial covariance matrix P
P[0][0] = 0.25f*sq(1.0f*deg2rad);
P[1][1] = 0.25f*sq(1.0f*deg2rad);
P[2][2] = 0.25f*sq(1.0f*deg2rad);
P[3][3] = 0.25f*sq(10.0f*deg2rad);
P[0][0] = 0.25f * sq(1.0f*deg2rad);
P[1][1] = 0.25f * sq(1.0f*deg2rad);
P[2][2] = 0.25f * sq(1.0f*deg2rad);
P[3][3] = 0.25f * sq(10.0f*deg2rad);
P[4][4] = sq(0.7);
P[5][5] = P[4][4];
P[6][6] = sq(0.7);
@ -1857,12 +1802,12 @@ void CovarianceInit()
P[20][20] = P[18][18];
}
float ConstrainFloat(float val, float min, float max)
float AttPosEKF::ConstrainFloat(float val, float min, float max)
{
return (val > max) ? max : ((val < min) ? min : val);
}
void ConstrainVariances()
void AttPosEKF::ConstrainVariances()
{
if (!numericalProtection) {
return;
@ -1914,7 +1859,7 @@ void ConstrainVariances()
}
void ConstrainStates()
void AttPosEKF::ConstrainStates()
{
if (!numericalProtection) {
return;
@ -1971,7 +1916,7 @@ void ConstrainStates()
}
void ForceSymmetry()
void AttPosEKF::ForceSymmetry()
{
if (!numericalProtection) {
return;
@ -1989,7 +1934,7 @@ void ForceSymmetry()
}
}
bool FilterHealthy()
bool AttPosEKF::FilterHealthy()
{
if (!statesInitialised) {
return false;
@ -2012,7 +1957,7 @@ bool FilterHealthy()
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition(void)
void AttPosEKF::ResetPosition(void)
{
if (staticMode) {
states[7] = 0;
@ -2030,7 +1975,7 @@ void ResetPosition(void)
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight(void)
void AttPosEKF::ResetHeight(void)
{
// write to the state vector
states[9] = -hgtMea;
@ -2039,7 +1984,7 @@ void ResetHeight(void)
/**
* Reset the velocity state.
*/
void ResetVelocity(void)
void AttPosEKF::ResetVelocity(void)
{
if (staticMode) {
states[4] = 0.0f;
@ -2054,7 +1999,7 @@ void ResetVelocity(void)
}
void FillErrorReport(struct ekf_status_report *err)
void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
{
for (int i = 0; i < n_states; i++)
{
@ -2069,7 +2014,7 @@ void FillErrorReport(struct ekf_status_report *err)
err->hgtTimeout = current_ekf_state.hgtTimeout;
}
bool StatesNaN(struct ekf_status_report *err_report) {
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
bool err = false;
// check all states and covariance matrices
@ -2122,7 +2067,7 @@ bool StatesNaN(struct ekf_status_report *err_report) {
* updated, but before any of the fusion steps are
* executed.
*/
int CheckAndBound()
int AttPosEKF::CheckAndBound()
{
// Store the old filter state
@ -2164,7 +2109,7 @@ int CheckAndBound()
return 0;
}
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
@ -2200,7 +2145,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
}
void InitializeDynamic(float (&initvelNED)[3])
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
{
// Clear the init flag
@ -2254,7 +2199,7 @@ void InitializeDynamic(float (&initvelNED)[3])
summedDelVel.z = 0.0f;
}
void InitialiseFilter(float (&initvelNED)[3])
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
{
//store initial lat,long and height
latRef = gpsLat;
@ -2266,7 +2211,7 @@ void InitialiseFilter(float (&initvelNED)[3])
InitializeDynamic(initvelNED);
}
void ZeroVariables()
void AttPosEKF::ZeroVariables()
{
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
@ -2292,12 +2237,12 @@ void ZeroVariables()
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
}
void GetFilterState(struct ekf_status_report *state)
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
{
memcpy(state, &current_ekf_state, sizeof(state));
}
void GetLastErrorState(struct ekf_status_report *last_error)
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
{
memcpy(last_error, &last_ekf_error, sizeof(last_error));
}

190
src/modules/fw_att_pos_estimator/estimator.h
View File

@ -48,85 +48,10 @@ void swap_var(float &d1, float &d2);
const unsigned int n_states = 21;
const unsigned int data_buffer_size = 50;
extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
extern Vector3f dVelIMU;
extern Vector3f dAngIMU;
extern float P[n_states][n_states]; // covariance matrix
extern float Kfusion[n_states]; // Kalman gains
extern float states[n_states]; // state matrix
extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
extern bool useAirspeed; // boolean true if airspeed data is being used
extern bool useCompass; // boolean true if magnetometer data is being used
extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
extern float innovVelPos[6]; // innovation output
extern float varInnovVelPos[6]; // innovation variance output
extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
extern bool fuseMagData; // boolean true when magnetometer data is to be fused
extern float velNED[3]; // North, East, Down velocity obs (m/s)
extern float posNE[2]; // North, East position obs (m)
extern float hgtMea; // measured height (m)
extern float posNED[3]; // North, East Down position (m)
extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
extern float innovMag[3]; // innovation output
extern float varInnovMag[3]; // innovation variance output
extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
extern float innovVtas; // innovation output
extern float varInnovVtas; // innovation variance output
extern bool fuseVtasData; // boolean true when airspeed data is to be fused
extern float VtasMeas; // true airspeed measurement (m/s)
extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
extern float latRef; // WGS-84 latitude of reference point (rad)
extern float lonRef; // WGS-84 longitude of reference point (rad)
extern float hgtRef; // WGS-84 height of reference point (m)
extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
extern float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
extern float gpsCourse;
extern float gpsVelD;
extern float gpsLat;
extern float gpsLon;
extern float gpsHgt;
extern uint8_t GPSstatus;
// Baro input
extern float baroHgt;
extern bool statesInitialised;
extern bool numericalProtection;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
extern bool staticMode;
// extern bool staticMode;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
@ -150,6 +75,93 @@ struct ekf_status_report {
bool kalmanGainsNaN;
};
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
@ -164,8 +176,6 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
float sq(float valIn);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
@ -190,15 +200,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
void OnGroundCheck();
@ -231,5 +245,15 @@ void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]);
protected:
bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
};
uint32_t millis();

384
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -124,6 +124,16 @@ public:
*/
int start();
/**
* Print the current status.
*/
void print_status();
/**
* Trip the filter by feeding it NaN values.
*/
int trip_nan();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
@ -199,6 +209,7 @@ private:
param_t tas_delay_ms;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
/**
* Update our local parameter cache.
@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_initialized(false),
_gps_initialized(false),
_mavlink_fd(-1)
_mavlink_fd(-1),
_ekf(nullptr)
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@ -384,6 +396,12 @@ void
FixedwingEstimator::task_main()
{
_ekf = new AttPosEKF();
if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!");
}
/*
* do subscriptions
*/
@ -414,23 +432,23 @@ FixedwingEstimator::task_main()
parameters_update();
/* set initial filter state */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
statesInitialised = false;
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
_ekf->fuseHgtData = false;
_ekf->fuseMagData = false;
_ekf->fuseVtasData = false;
_ekf->statesInitialised = false;
/* initialize measurement data */
VtasMeas = 0.0f;
_ekf->VtasMeas = 0.0f;
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
dVelIMU.x = 0.0f;
dVelIMU.y = 0.0f;
dVelIMU.z = 0.0f;
dAngIMU.x = 0.0f;
dAngIMU.y = 0.0f;
dAngIMU.z = 0.0f;
_ekf->dVelIMU.x = 0.0f;
_ekf->dVelIMU.y = 0.0f;
_ekf->dVelIMU.z = 0.0f;
_ekf->dAngIMU.x = 0.0f;
_ekf->dAngIMU.y = 0.0f;
_ekf->dAngIMU.z = 0.0f;
/* wakeup source(s) */
struct pollfd fds[2];
@ -509,7 +527,7 @@ FixedwingEstimator::task_main()
}
last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
last_run = _gyro.timestamp;
@ -521,20 +539,20 @@ FixedwingEstimator::task_main()
// Always store data, independent of init status
/* fill in last data set */
dtIMU = deltaT;
_ekf->dtIMU = deltaT;
angRate.x = _gyro.x;
angRate.y = _gyro.y;
angRate.z = _gyro.z;
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
accel.x = _accel.x;
accel.y = _accel.y;
accel.z = _accel.z;
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
lastAccel = accel;
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
_ekf->lastAccel = accel;
#else
@ -563,20 +581,20 @@ FixedwingEstimator::task_main()
// Always store data, independent of init status
/* fill in last data set */
dtIMU = deltaT;
_ekf->dtIMU = deltaT;
angRate.x = _sensor_combined.gyro_rad_s[0];
angRate.y = _sensor_combined.gyro_rad_s[1];
angRate.z = _sensor_combined.gyro_rad_s[2];
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
accel.x = _sensor_combined.accelerometer_m_s2[0];
accel.y = _sensor_combined.accelerometer_m_s2[1];
accel.z = _sensor_combined.accelerometer_m_s2[2];
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
lastAccel = accel;
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
mag_updated = true;
@ -597,7 +615,7 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
VtasMeas = _airspeed.true_airspeed_m_s;
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
newAdsData = true;
} else {
@ -622,24 +640,24 @@ FixedwingEstimator::task_main()
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
ResetPosition();
ResetVelocity();
ResetStoredStates();
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
/* fuse GPS updates */
//_gps.timestamp / 1e3;
GPSstatus = _gps.fix_type;
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
gpsLat = math::radians(_gps.lat / (double)1e7);
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
gpsHgt = _gps.alt / 1e3f;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
newDataGps = true;
}
@ -652,10 +670,10 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude - _baro_ref;
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
}
#ifndef SENSOR_COMBINED_SUB
@ -671,27 +689,27 @@ FixedwingEstimator::task_main()
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
magData.x = _mag.x;
magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.x = _mag.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
magData.y = _mag.y;
magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.y = _mag.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
magData.z = _mag.z;
magBias.z = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _mag.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#else
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
magData.x = _sensor_combined.magnetometer_ga[0];
magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
magData.y = _sensor_combined.magnetometer_ga[1];
magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
magData.z = _sensor_combined.magnetometer_ga[2];
magBias.z = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#endif
@ -705,7 +723,7 @@ FixedwingEstimator::task_main()
/**
* CHECK IF THE INPUT DATA IS SANE
*/
int check = CheckAndBound();
int check = _ekf->CheckAndBound();
switch (check) {
case 0:
@ -739,7 +757,7 @@ FixedwingEstimator::task_main()
struct ekf_status_report ekf_report;
GetLastErrorState(&ekf_report);
_ekf->GetLastErrorState(&ekf_report);
struct estimator_status_report rep;
memset(&rep, 0, sizeof(rep));
@ -779,16 +797,16 @@ FixedwingEstimator::task_main()
if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && (GPSstatus == 3)) {
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7;
double lon = _gps.lon * 1e-7;
float alt = _gps.alt * 1e-3;
InitialiseFilter(velNED);
_ekf->InitialiseFilter(_ekf->velNED);
// Initialize projection
_local_pos.ref_lat = _gps.lat;
@ -799,7 +817,7 @@ FixedwingEstimator::task_main()
// Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude;
baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude - _baro_ref;
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
// XXX this is not multithreading safe
@ -808,24 +826,24 @@ FixedwingEstimator::task_main()
_gps_initialized = true;
} else if (!statesInitialised) {
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
} else if (!_ekf->statesInitialised) {
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
InitialiseFilter(velNED);
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
}
}
// If valid IMU data and states initialised, predict states and covariances
if (statesInitialised) {
if (_ekf->statesInitialised) {
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
_ekf->UpdateStrapdownEquationsNED();
#if 0
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
@ -842,20 +860,20 @@ FixedwingEstimator::task_main()
#endif
// store the predicted states for subsequent use by measurement fusion
StoreStates(IMUmsec);
_ekf->StoreStates(IMUmsec);
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
_ekf->OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + dVelIMU;
dt += dtIMU;
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
dt += _ekf->dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction(dt);
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
_ekf->CovariancePrediction(dt);
_ekf->summedDelAng = _ekf->summedDelAng.zero();
_ekf->summedDelVel = _ekf->summedDelVel.zero();
dt = 0.0f;
}
@ -865,79 +883,79 @@ FixedwingEstimator::task_main()
// Fuse GPS Measurements
if (newDataGps && _gps_initialized) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else if (statesInitialised) {
} else if (_ekf->statesInitialised) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else {
fuseVelData = false;
fusePosData = false;
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
}
if (newAdsData && statesInitialised) {
if (newAdsData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
fuseHgtData = true;
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else {
fuseHgtData = false;
_ekf->fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised) {
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
if (newDataMag && _ekf->statesInitialised) {
_ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
} else {
fuseMagData = false;
_ekf->fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed();
} else {
fuseVtasData = false;
_ekf->fuseVtasData = false;
}
// Publish results
@ -954,7 +972,7 @@ FixedwingEstimator::task_main()
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
math::Quaternion q(states[0], states[1], states[2], states[3]);
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@ -962,10 +980,10 @@ FixedwingEstimator::task_main()
_att.R[i][j] = R(i, j);
_att.timestamp = last_sensor_timestamp;
_att.q[0] = states[0];
_att.q[1] = states[1];
_att.q[2] = states[2];
_att.q[3] = states[3];
_att.q[0] = _ekf->states[0];
_att.q[1] = _ekf->states[1];
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
@ -974,13 +992,13 @@ FixedwingEstimator::task_main()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = angRate.x - states[10];
_att.pitchspeed = angRate.y - states[11];
_att.yawspeed = angRate.z - states[12];
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
// gyro offsets
_att.rate_offsets[0] = states[10];
_att.rate_offsets[1] = states[11];
_att.rate_offsets[2] = states[12];
_att.rate_offsets[0] = _ekf->states[10];
_att.rate_offsets[1] = _ekf->states[11];
_att.rate_offsets[2] = _ekf->states[12];
/* lazily publish the attitude only once available */
if (_att_pub > 0) {
@ -993,20 +1011,15 @@ FixedwingEstimator::task_main()
}
}
if (!isfinite(states[0])) {
print_status();
_exit(0);
}
if (_gps_initialized) {
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = states[7];
_local_pos.y = states[8];
_local_pos.z = states[9];
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
_local_pos.z = _ekf->states[9];
_local_pos.vx = states[4];
_local_pos.vy = states[5];
_local_pos.vz = states[6];
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
_local_pos.vz = _ekf->states[6];
_local_pos.xy_valid = _gps_initialized;
_local_pos.z_valid = true;
@ -1107,9 +1120,10 @@ FixedwingEstimator::start()
return OK;
}
void print_status()
void
FixedwingEstimator::print_status()
{
math::Quaternion q(states[0], states[1], states[2], states[3]);
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@ -1125,30 +1139,30 @@ void print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(statesInitialised) ? "INITIALIZED" : "NON_INIT",
(onGround) ? "ON_GROUND" : "AIRBORNE",
(fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(fusePosData) ? "FUSE_POS" : "INH_POS",
(fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
}
int trip_nan() {
int FixedwingEstimator::trip_nan() {
int ret = 0;
@ -1166,7 +1180,7 @@ int trip_nan() {
float nan_val = 0.0f / 0.0f;
warnx("system not armed, tripping state vector with NaN values");
states[5] = nan_val;
_ekf->states[5] = nan_val;
usleep(100000);
// warnx("tripping covariance #1 with NaN values");
@ -1178,15 +1192,15 @@ int trip_nan() {
// usleep(100000);
warnx("tripping covariance #3 with NaN values");
P[3][3] = nan_val; // covariance matrix
_ekf->P[3][3] = nan_val; // covariance matrix
usleep(100000);
warnx("tripping Kalman gains with NaN values");
Kfusion[0] = nan_val; // Kalman gains
_ekf->Kfusion[0] = nan_val; // Kalman gains
usleep(100000);
warnx("tripping stored states[0] with NaN values");
storedStates[0][0] = nan_val;
_ekf->storedStates[0][0] = nan_val;
usleep(100000);
warnx("\nDONE - FILTER STATE:");
@ -1234,7 +1248,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
if (estimator::g_estimator) {
warnx("running");
print_status();
estimator::g_estimator->print_status();
exit(0);
@ -1245,7 +1259,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
if (!strcmp(argv[1], "trip")) {
if (estimator::g_estimator) {
int ret = trip_nan();
int ret = estimator::g_estimator->trip_nan();
exit(ret);

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -1937,6 +1937,8 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
break;
case MAVLINK_MODE_CAMERA:

31
src/modules/mavlink/mavlink_messages.cpp
View File

@ -262,22 +262,21 @@ protected:
void send(const hrt_abstime t)
{
if (status_sub->update(t)) {
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
};

1
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
if (_updated) {
orb_copy(_topic, _fd, _data);
_published = true;
return true;
}
}

60
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
if (_flow_pub <= 0) {
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (_vicon_position_pub <= 0) {
if (_vicon_position_pub < 0) {
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub <= 0) {
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub <= 0) {
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f;
if (_rc_pub == 0) {
if (_rc_pub < 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else {
@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
if (_manual_pub == 0) {
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.differential_pressure_timestamp = timestamp;
/* publish combined sensor topic */
if (_sensors_pub > 0) {
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
if (_sensors_pub < 0) {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
} else {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
}
}
@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
if (_gps_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
} else {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
}
}
@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
if (_attitude_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
if (_attitude_pub < 0) {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
} else {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
}
}
@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
if (_global_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
} else {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
}
}
@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed;
if (_local_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
if (_local_pos_pub < 0) {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
} else {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
}
}
@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -120,7 +120,6 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
int _manual_sub;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
orb_advert_t _attitude_pub;
@ -139,6 +138,7 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
int _manual_sub;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;

2
src/modules/navigator/navigator_main.cpp
View File

@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached()
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}

178
src/modules/px4iofirmware/controls.c
View File

@ -134,8 +134,6 @@ controls_tick() {
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
@ -201,94 +199,104 @@ controls_tick() {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* do not command anything in failsafe, kick in the RC loss counter */
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
scaled = -scaled;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) {
/* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
if (mapped == 3 && r_setup_rc_thr_failsafe) {
/* throttle failsafe detection */
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i))) {
r_rc_values[i] = 0;
}
}
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/*
@ -316,8 +324,13 @@ controls_tick() {
* Handle losing RC input
*/
/* this kicks in if the receiver is gone or the system went to failsafe */
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* if we are in failsafe, clear the override flag */
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
@ -326,27 +339,24 @@ controls_tick() {
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set raw channel count to zero */
r_raw_rc_count = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* must have R/C input (NO FAILSAFE!).
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false;
@ -369,10 +379,10 @@ controls_tick() {
mixer_tick();
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
}

28
src/modules/px4iofirmware/protocol.h
View File

@ -164,10 +164,10 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
@ -201,13 +201,15 @@ enum { /* DSM bind states */
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* 12 occupied by CRC */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
@ -217,10 +219,10 @@ enum { /* DSM bind states */
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_VALID 64
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 52

1
src/modules/px4iofirmware/px4io.h
View File

@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#endif
#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
#define r_control_values (&r_page_controls[0])

1
src/modules/px4iofirmware/registers.c
View File

@ -169,6 +169,7 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
};
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2

30
src/modules/sdlog2/sdlog2.c
View File

@ -84,6 +84,8 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
} buf;
memset(&buf, 0, sizeof(buf));
@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
int system_power_sub;
int servorail_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
thread_running = true;
@ -1184,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
@ -1226,6 +1236,24 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- SYSTEM POWER RAILS --- */
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
log_msg.msg_type = LOG_PWR_MSG;
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
/* copy servo rail status topic here too */
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
LOGBUFFER_WRITE_AND_COUNT(PWR);
}
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
@ -1258,7 +1286,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
LOGBUFFER_WRITE_AND_COUNT(DIST);
LOGBUFFER_WRITE_AND_COUNT(ESTM);
}
/* signal the other thread new data, but not yet unlock */

45
src/modules/sdlog2/sdlog2_messages.h
View File

@ -174,6 +174,7 @@ struct log_OUT0_s {
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
float air_temperature_celsius;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
@ -277,6 +278,29 @@ struct log_TELE_s {
uint8_t txbuf;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 23
struct log_ESTM_s {
float s[10];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
struct log_PWR_s {
float peripherals_5v;
float servo_rail_5v;
float servo_rssi;
uint8_t usb_ok;
uint8_t brick_ok;
uint8_t servo_ok;
uint8_t low_power_rail_overcurrent;
uint8_t high_power_rail_overcurrent;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@ -299,16 +323,6 @@ struct log_PARM_s {
float value;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 132
struct log_ESTM_s {
float s[32];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
#pragma pack(pop)
/* construct list of all message formats */
@ -325,7 +339,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
@ -335,15 +349,16 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
/* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
LOG_FORMAT(PARM, "Nf", "Name,Value"),
LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PARM, "Nf", "Name,Value")
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
#endif /* SDLOG2_MESSAGES_H_ */

28
src/modules/sensors/sensor_params.c
View File

@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
*/
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
/**
* Failsafe channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_CH, 0);
/**
* Failsafe channel mode.
*
* 0 = too low means signal loss,
* 1 = too high means signal loss
*
* @min 0
* @max 1
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_MODE, 0);
/**
* Failsafe channel PWM threshold.
*
* @min 0
* @max 1
* @min 800
* @max 2200
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
PARAM_DEFINE_INT32(RC_FAILS_THR, 0);

35
src/modules/sensors/sensors.cpp
View File

@ -263,9 +263,7 @@ private:
float rc_scale_yaw;
float rc_scale_flaps;
int rc_fs_ch;
int rc_fs_mode;
float rc_fs_thr;
int32_t rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
@ -313,8 +311,6 @@ private:
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t rc_fs_ch;
param_t rc_fs_mode;
param_t rc_fs_thr;
param_t battery_voltage_scaling;
@ -531,9 +527,7 @@ Sensors::Sensors() :
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* RC failsafe */
_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@ -689,8 +683,6 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
/* update RC function mappings */
@ -1033,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
@ -1312,19 +1305,15 @@ Sensors::rc_poll()
manual_control.aux5 = NAN;
/* require at least four channels to consider the signal valid */
if (rc_input.channel_count < 4)
if (rc_input.channel_count < 4) {
return;
}
/* failsafe check */
if (_parameters.rc_fs_ch != 0) {
if (_parameters.rc_fs_mode == 0) {
if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
return;
} else if (_parameters.rc_fs_mode == 1) {
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
return;
}
/* check for failsafe */
if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
/* do not publish manual control setpoints when there are none */
return;
}
unsigned channel_limit = rc_input.channel_count;

3
src/modules/uORB/objects_common.cpp
View File

@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
#include "topics/servorail_status.h"
ORB_DEFINE(servorail_status, struct servorail_status_s);
#include "topics/system_power.h"
ORB_DEFINE(system_power, struct system_power_s);
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);

5
src/modules/uORB/topics/airspeed.h
View File

@ -52,9 +52,10 @@
* Airspeed
*/
struct airspeed_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */
};
/**

9
src/modules/uORB/topics/differential_pressure.h
View File

@ -52,13 +52,14 @@
* Differential pressure.
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
};

71
src/modules/uORB/topics/system_power.h Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file system_power.h
*
* Definition of the system_power voltage and power status uORB topic.
*/
#ifndef SYSTEM_POWER_H_
#define SYSTEM_POWER_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* voltage and power supply status
*/
struct system_power_s {
uint64_t timestamp; /**< microseconds since system boot */
float voltage5V_v; /**< peripheral 5V rail voltage */
uint8_t usb_connected:1; /**< USB is connected when 1 */
uint8_t brick_valid:1; /**< brick power is good when 1 */
uint8_t servo_valid:1; /**< servo power is good when 1 */
uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */
uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(system_power);
#endif