forked from Archive/PX4-Autopilot
Merge branch 'ekf_voting_priority'
This commit is contained in:
Lorenz Meier
commit
b067a0c094
|
|
@ -79,7 +79,7 @@ MODULES += modules/land_detector
|
|||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
# Too high RAM usage due to static allocations
|
||||
# Removed from build due to large static allocations
|
||||
#MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_q
|
||||
MODULES += modules/ekf_att_pos_estimator
|
||||
|
|
|
|||
|
|
@ -1,8 +1,12 @@
|
|||
uint64 timestamp
|
||||
uint64 integral_dt # integration time
|
||||
uint64 error_count
|
||||
float32 x # acceleration in the NED X board axis in m/s^2
|
||||
float32 y # acceleration in the NED Y board axis in m/s^2
|
||||
float32 z # acceleration in the NED Z board axis in m/s^2
|
||||
float32 x_integral # velocity in the NED X board axis in m/s over the integration time frame
|
||||
float32 y_integral # velocity in the NED Y board axis in m/s over the integration time frame
|
||||
float32 z_integral # velocity in the NED Z board axis in m/s over the integration time frame
|
||||
float32 temperature # temperature in degrees celsius
|
||||
float32 range_m_s2 # range in m/s^2 (+- this value)
|
||||
float32 scaling
|
||||
|
|
|
|||
|
|
@ -26,96 +26,49 @@ uint32 SENSOR_PRIO_MAX = 255
|
|||
# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
|
||||
|
||||
uint64 timestamp # Timestamp in microseconds since boot, from gyro
|
||||
#
|
||||
int16[3] gyro_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro_rad_s # Angular velocity in radian per seconds
|
||||
uint32 gyro_priority # Sensor priority
|
||||
uint32 gyro_errcount # Error counter for gyro 0
|
||||
float32 gyro_temp # Temperature of gyro 0
|
||||
uint64[3] gyro_timestamp # Gyro timestamps
|
||||
int16[9] gyro_raw # Raw sensor values of angular velocity
|
||||
float32[9] gyro_rad_s # Angular velocity in radian per seconds
|
||||
uint32[3] gyro_priority # Sensor priority
|
||||
float32[9] gyro_integral_rad # delta angle in radians
|
||||
uint64[3] gyro_integral_dt # delta time for gyro integral in us
|
||||
uint32[3] gyro_errcount # Error counter for gyro 0
|
||||
float32[3] gyro_temp # Temperature of gyro 0
|
||||
|
||||
int16[3] accelerometer_raw # Raw acceleration in NED body frame
|
||||
float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
int16 accelerometer_mode # Accelerometer measurement mode
|
||||
float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2
|
||||
uint64 accelerometer_timestamp # Accelerometer timestamp
|
||||
uint32 accelerometer_priority # Sensor priority
|
||||
uint32 accelerometer_errcount # Error counter for accel 0
|
||||
float32 accelerometer_temp # Temperature of accel 0
|
||||
int16[9] accelerometer_raw # Raw acceleration in NED body frame
|
||||
float32[9] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
float32[9] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2
|
||||
uint64[3] accelerometer_integral_dt # delta time for accel integral in us
|
||||
int16[3] accelerometer_mode # Accelerometer measurement mode
|
||||
float32[3] accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2
|
||||
uint64[3] accelerometer_timestamp # Accelerometer timestamp
|
||||
uint32[3] accelerometer_priority # Sensor priority
|
||||
uint32[3] accelerometer_errcount # Error counter for accel 0
|
||||
float32[3] accelerometer_temp # Temperature of accel 0
|
||||
|
||||
int16[3] magnetometer_raw # Raw magnetic field in NED body frame
|
||||
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss
|
||||
int16 magnetometer_mode # Magnetometer measurement mode
|
||||
float32 magnetometer_range_ga # measurement range in Gauss
|
||||
float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor
|
||||
uint64 magnetometer_timestamp # Magnetometer timestamp
|
||||
uint32 magnetometer_priority # Sensor priority
|
||||
uint32 magnetometer_errcount # Error counter for mag 0
|
||||
float32 magnetometer_temp # Temperature of mag 0
|
||||
int16[9] magnetometer_raw # Raw magnetic field in NED body frame
|
||||
float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss
|
||||
int16[3] magnetometer_mode # Magnetometer measurement mode
|
||||
float32[3] magnetometer_range_ga # measurement range in Gauss
|
||||
float32[3] magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor
|
||||
uint64[3] magnetometer_timestamp # Magnetometer timestamp
|
||||
uint32[3] magnetometer_priority # Sensor priority
|
||||
uint32[3] magnetometer_errcount # Error counter for mag 0
|
||||
float32[3] magnetometer_temp # Temperature of mag 0
|
||||
|
||||
int16[3] gyro1_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro1_rad_s # Angular velocity in radian per seconds
|
||||
uint64 gyro1_timestamp # Gyro timestamp
|
||||
uint32 gyro1_priority # Sensor priority
|
||||
uint32 gyro1_errcount # Error counter for gyro 1
|
||||
float32 gyro1_temp # Temperature of gyro 1
|
||||
|
||||
int16[3] accelerometer1_raw # Raw acceleration in NED body frame
|
||||
float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
uint64 accelerometer1_timestamp # Accelerometer timestamp
|
||||
uint32 accelerometer1_priority # Sensor priority
|
||||
uint32 accelerometer1_errcount # Error counter for accel 1
|
||||
float32 accelerometer1_temp # Temperature of accel 1
|
||||
|
||||
int16[3] magnetometer1_raw # Raw magnetic field in NED body frame
|
||||
float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss
|
||||
uint64 magnetometer1_timestamp # Magnetometer timestamp
|
||||
uint32 magnetometer1_priority # Sensor priority
|
||||
uint32 magnetometer1_errcount # Error counter for mag 1
|
||||
float32 magnetometer1_temp # Temperature of mag 1
|
||||
|
||||
int16[3] gyro2_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro2_rad_s # Angular velocity in radian per seconds
|
||||
uint64 gyro2_timestamp # Gyro timestamp
|
||||
uint32 gyro2_priority # Sensor priority
|
||||
uint32 gyro2_errcount # Error counter for gyro 1
|
||||
float32 gyro2_temp # Temperature of gyro 1
|
||||
|
||||
int16[3] accelerometer2_raw # Raw acceleration in NED body frame
|
||||
float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2
|
||||
uint64 accelerometer2_timestamp # Accelerometer timestamp
|
||||
uint32 accelerometer2_priority # Sensor priority
|
||||
uint32 accelerometer2_errcount # Error counter for accel 2
|
||||
float32 accelerometer2_temp # Temperature of accel 2
|
||||
|
||||
int16[3] magnetometer2_raw # Raw magnetic field in NED body frame
|
||||
float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss
|
||||
uint64 magnetometer2_timestamp # Magnetometer timestamp
|
||||
uint32 magnetometer2_priority # Sensor priority
|
||||
uint32 magnetometer2_errcount # Error counter for mag 2
|
||||
float32 magnetometer2_temp # Temperature of mag 2
|
||||
|
||||
float32 baro_pres_mbar # Barometric pressure, already temp. comp.
|
||||
float32 baro_alt_meter # Altitude, already temp. comp.
|
||||
float32 baro_temp_celcius # Temperature in degrees celsius
|
||||
uint64 baro_timestamp # Barometer timestamp
|
||||
uint32 baro_priority # Sensor priority
|
||||
|
||||
float32 baro1_pres_mbar # Barometric pressure, already temp. comp.
|
||||
float32 baro1_alt_meter # Altitude, already temp. comp.
|
||||
float32 baro1_temp_celcius # Temperature in degrees celsius
|
||||
uint64 baro1_timestamp # Barometer timestamp
|
||||
uint32 baro1_priority # Sensor priority
|
||||
float32[3] baro_pres_mbar # Barometric pressure, already temp. comp.
|
||||
float32[3] baro_alt_meter # Altitude, already temp. comp.
|
||||
float32[3] baro_temp_celcius # Temperature in degrees celsius
|
||||
uint64[3] baro_timestamp # Barometer timestamp
|
||||
uint32[3] baro_priority # Sensor priority
|
||||
uint32[3] baro_errcount # Error count in communication
|
||||
|
||||
float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1
|
||||
uint16[10] adc_mapping # Channel indices of each of these values
|
||||
float32 mcu_temp_celcius # Internal temperature measurement of MCU
|
||||
|
||||
float32 differential_pressure_pa # Airspeed sensor differential pressure
|
||||
uint64 differential_pressure_timestamp # Last measurement timestamp
|
||||
float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading
|
||||
uint32 differential_pressure_priority # Sensor priority
|
||||
|
||||
float32 differential_pressure1_pa # Airspeed sensor differential pressure
|
||||
uint64 differential_pressure1_timestamp # Last measurement timestamp
|
||||
float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading
|
||||
uint32 differential_pressure1_priority # Sensor priority
|
||||
float32[3] differential_pressure_pa # Airspeed sensor differential pressure
|
||||
uint64[3] differential_pressure_timestamp # Last measurement timestamp
|
||||
float32[3] differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading
|
||||
uint32[3] differential_pressure_priority # Sensor priority
|
||||
uint32[3] differential_pressure_errcount # Error count in communication
|
||||
|
|
|
|||
|
|
@ -1,8 +1,12 @@
|
|||
uint64 timestamp
|
||||
uint64 integral_dt # integration time
|
||||
uint64 error_count
|
||||
float32 x # angular velocity in the NED X board axis in rad/s
|
||||
float32 y # angular velocity in the NED Y board axis in rad/s
|
||||
float32 z # angular velocity in the NED Z board axis in rad/s
|
||||
float32 x_integral # delta angle in the NED X board axis in rad/s in the integration time frame
|
||||
float32 y_integral # delta angle in the NED Y board axis in rad/s in the integration time frame
|
||||
float32 z_integral # delta angle in the NED Z board axis in rad/s in the integration time frame
|
||||
float32 temperature # temperature in degrees celcius
|
||||
float32 range_rad_s
|
||||
float32 scaling
|
||||
|
|
|
|||
|
|
@ -10,6 +10,9 @@ float32 yawspeed # Yaw body angular rate (rad/s, x forward/y right/z down)
|
|||
float32 rollacc # Roll angular accelration (rad/s^2, x forward/y right/z down)
|
||||
float32 pitchacc # Pitch angular acceleration (rad/s^2, x forward/y right/z down)
|
||||
float32 yawacc # Yaw angular acceleration (rad/s^2, x forward/y right/z down)
|
||||
float32 rate_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
|
||||
float32 accel_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
|
||||
float32 mag_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
|
||||
float32[3] rate_offsets # Offsets of the body angular rates from zero
|
||||
float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED)
|
||||
float32[4] q # Quaternion (NED)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,183 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file integrator.h
|
||||
*
|
||||
* A resettable integrator
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
class Integrator {
|
||||
public:
|
||||
Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
|
||||
virtual ~Integrator();
|
||||
|
||||
/**
|
||||
* Put an item into the integral.
|
||||
*
|
||||
* @param timestamp Timestamp of the current value
|
||||
* @param val Item to put
|
||||
* @param integral Current integral in case the integrator did reset, else the value will not be modified
|
||||
* @return true if putting the item triggered an integral reset
|
||||
* and the integral should be published
|
||||
*/
|
||||
bool put(hrt_abstime timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
|
||||
|
||||
/**
|
||||
* Get the current integral value
|
||||
*
|
||||
* @return the integral since the last auto-reset
|
||||
*/
|
||||
math::Vector<3> get() { return _integral_auto; }
|
||||
|
||||
/**
|
||||
* Read from the integral
|
||||
*
|
||||
* @param auto_reset Reset the integral to zero on read
|
||||
* @return the integral since the last read-reset
|
||||
*/
|
||||
math::Vector<3> read(bool auto_reset);
|
||||
|
||||
/**
|
||||
* Get current integral start time
|
||||
*/
|
||||
hrt_abstime current_integral_start() { return _last_auto; }
|
||||
|
||||
private:
|
||||
hrt_abstime _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset */
|
||||
hrt_abstime _last_integration; /**< timestamp of the last integration step */
|
||||
hrt_abstime _last_auto; /**< last auto-announcement of integral value */
|
||||
math::Vector<3> _integral_auto; /**< the integrated value which auto-resets after _auto_reset_interval */
|
||||
math::Vector<3> _integral_read; /**< the integrated value since the last read */
|
||||
math::Vector<3> _last_val; /**< previously integrated last value */
|
||||
math::Vector<3> _last_delta; /**< last local delta */
|
||||
void (*_auto_callback)(hrt_abstime, math::Vector<3>); /**< the function callback for auto-reset */
|
||||
bool _coning_comp_on; /**< coning compensation */
|
||||
|
||||
/* we don't want this class to be copied */
|
||||
Integrator(const Integrator&);
|
||||
Integrator operator=(const Integrator&);
|
||||
};
|
||||
|
||||
Integrator::Integrator(hrt_abstime auto_reset_interval, bool coning_compensation) :
|
||||
_auto_reset_interval(auto_reset_interval),
|
||||
_last_integration(0),
|
||||
_last_auto(0),
|
||||
_integral_auto(0.0f, 0.0f, 0.0f),
|
||||
_integral_read(0.0f, 0.0f, 0.0f),
|
||||
_last_val(0.0f, 0.0f, 0.0f),
|
||||
_last_delta(0.0f, 0.0f, 0.0f),
|
||||
_auto_callback(nullptr),
|
||||
_coning_comp_on(coning_compensation)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
Integrator::~Integrator()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool
|
||||
Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
|
||||
{
|
||||
bool auto_reset = false;
|
||||
|
||||
if (_last_integration == 0) {
|
||||
/* this is the first item in the integrator */
|
||||
_last_integration = timestamp;
|
||||
_last_auto = timestamp;
|
||||
_last_val = val;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Integrate
|
||||
double dt = (double)(timestamp - _last_integration) / 1000000.0;
|
||||
math::Vector<3> i = (val + _last_val) * dt * 0.5f;
|
||||
|
||||
// Apply coning compensation if required
|
||||
if (_coning_comp_on) {
|
||||
// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
|
||||
// following:
|
||||
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
|
||||
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
|
||||
|
||||
i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f;
|
||||
}
|
||||
|
||||
_integral_auto += i;
|
||||
_integral_read += i;
|
||||
|
||||
_last_integration = timestamp;
|
||||
_last_val = val;
|
||||
_last_delta = i;
|
||||
|
||||
if ((timestamp - _last_auto) > _auto_reset_interval) {
|
||||
if (_auto_callback) {
|
||||
/* call the callback */
|
||||
_auto_callback(timestamp, _integral_auto);
|
||||
}
|
||||
|
||||
integral = _integral_auto;
|
||||
integral_dt = (timestamp - _last_auto);
|
||||
|
||||
auto_reset = true;
|
||||
_last_auto = timestamp;
|
||||
_integral_auto(0) = 0.0f;
|
||||
_integral_auto(1) = 0.0f;
|
||||
_integral_auto(2) = 0.0f;
|
||||
}
|
||||
|
||||
return auto_reset;
|
||||
}
|
||||
|
||||
math::Vector<3>
|
||||
Integrator::read(bool auto_reset)
|
||||
{
|
||||
math::Vector<3> val = _integral_read;
|
||||
if (auto_reset) {
|
||||
_integral_read(0) = 0.0f;
|
||||
_integral_read(1) = 0.0f;
|
||||
_integral_read(2) = 0.0f;
|
||||
}
|
||||
|
||||
return val;
|
||||
}
|
||||
|
|
@ -178,12 +178,12 @@ void frsky_send_frame1(int uart)
|
|||
roundf(raw.accelerometer_m_s2[2] * 1000.0f));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
|
||||
raw.baro_alt_meter);
|
||||
raw.baro_alt_meter[0]);
|
||||
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
|
||||
roundf(frac(raw.baro_alt_meter) * 100.0f));
|
||||
roundf(frac(raw.baro_alt_meter[0]) * 100.0f));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_TEMP1,
|
||||
roundf(raw.baro_temp_celcius));
|
||||
roundf(raw.baro_temp_celcius[0]));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_VFAS,
|
||||
roundf(battery.voltage_v * 10.0f));
|
||||
|
|
|
|||
|
|
@ -148,12 +148,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
|
|||
msg.eam_sensor_id = EAM_SENSOR_ID;
|
||||
msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
|
||||
|
||||
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
|
||||
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20);
|
||||
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
|
||||
|
||||
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
|
||||
|
||||
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
|
||||
uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500);
|
||||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
|
|
|
|||
|
|
@ -66,6 +66,7 @@
|
|||
#include <drivers/device/spi.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
|
|
@ -175,6 +176,7 @@ static const int ERROR = -1;
|
|||
|
||||
#define L3GD20_DEFAULT_RATE 760
|
||||
#define L3G4200D_DEFAULT_RATE 800
|
||||
#define L3GD20_MAX_OUTPUT_RATE 280
|
||||
#define L3GD20_DEFAULT_RANGE_DPS 2000
|
||||
#define L3GD20_DEFAULT_FILTER_FREQ 30
|
||||
#define L3GD20_TEMP_OFFSET_CELSIUS 40
|
||||
|
|
@ -256,6 +258,8 @@ private:
|
|||
math::LowPassFilter2p _gyro_filter_y;
|
||||
math::LowPassFilter2p _gyro_filter_z;
|
||||
|
||||
Integrator _gyro_int;
|
||||
|
||||
/* true if an L3G4200D is detected */
|
||||
bool _is_l3g4200d;
|
||||
|
||||
|
|
@ -427,6 +431,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
|
|||
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true),
|
||||
_is_l3g4200d(false),
|
||||
_rotation(rotation),
|
||||
_checked_next(0)
|
||||
|
|
@ -1029,13 +1034,21 @@ L3GD20::measure()
|
|||
// apply user specified rotation
|
||||
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
|
||||
report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||
report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
||||
report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
||||
float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||
float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
||||
float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
||||
|
||||
report.x = _gyro_filter_x.apply(report.x);
|
||||
report.y = _gyro_filter_y.apply(report.y);
|
||||
report.z = _gyro_filter_z.apply(report.z);
|
||||
report.x = _gyro_filter_x.apply(xin);
|
||||
report.y = _gyro_filter_y.apply(yin);
|
||||
report.z = _gyro_filter_z.apply(zin);
|
||||
|
||||
math::Vector<3> gval(xin, yin, zin);
|
||||
math::Vector<3> gval_integrated;
|
||||
|
||||
bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
|
||||
report.x_integral = gval_integrated(0);
|
||||
report.y_integral = gval_integrated(1);
|
||||
report.z_integral = gval_integrated(2);
|
||||
|
||||
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
|
||||
|
||||
|
|
@ -1044,13 +1057,15 @@ L3GD20::measure()
|
|||
|
||||
_reports->force(&report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
if (gyro_notify) {
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
if (!(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
|
||||
/* publish for subscribers */
|
||||
if (!(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
|
||||
}
|
||||
}
|
||||
|
||||
_read++;
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@
|
|||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
|
@ -206,6 +207,7 @@ static const int ERROR = -1;
|
|||
#define LSM303D_ACCEL_DEFAULT_RATE 800
|
||||
#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
|
||||
#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
#define LSM303D_ACCEL_MAX_OUTPUT_RATE 280
|
||||
|
||||
#define LSM303D_MAG_DEFAULT_RANGE_GA 2
|
||||
#define LSM303D_MAG_DEFAULT_RATE 100
|
||||
|
|
@ -308,6 +310,8 @@ private:
|
|||
math::LowPassFilter2p _accel_filter_y;
|
||||
math::LowPassFilter2p _accel_filter_z;
|
||||
|
||||
Integrator _accel_int;
|
||||
|
||||
enum Rotation _rotation;
|
||||
|
||||
// values used to
|
||||
|
|
@ -577,6 +581,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
|
|||
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_accel_int(1000000 / LSM303D_ACCEL_MAX_OUTPUT_RATE, true),
|
||||
_rotation(rotation),
|
||||
_constant_accel_count(0),
|
||||
_last_temperature(0),
|
||||
|
|
@ -1411,6 +1416,13 @@ LSM303D::stop()
|
|||
{
|
||||
hrt_cancel(&_accel_call);
|
||||
hrt_cancel(&_mag_call);
|
||||
|
||||
/* reset internal states */
|
||||
memset(_last_accel, 0, sizeof(_last_accel));
|
||||
|
||||
/* discard unread data in the buffers */
|
||||
_accel_reports->flush();
|
||||
_mag_reports->flush();
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -1575,17 +1587,27 @@ LSM303D::measure()
|
|||
accel_report.y = _accel_filter_y.apply(y_in_new);
|
||||
accel_report.z = _accel_filter_z.apply(z_in_new);
|
||||
|
||||
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
|
||||
math::Vector<3> aval_integrated;
|
||||
|
||||
bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
|
||||
accel_report.x_integral = aval_integrated(0);
|
||||
accel_report.y_integral = aval_integrated(1);
|
||||
accel_report.z_integral = aval_integrated(2);
|
||||
|
||||
accel_report.scaling = _accel_range_scale;
|
||||
accel_report.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
_accel_reports->force(&accel_report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
if (accel_notify) {
|
||||
poll_notify(POLLIN);
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
if (!(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
}
|
||||
|
||||
_accel_read++;
|
||||
|
|
@ -1841,7 +1863,7 @@ namespace lsm303d
|
|||
|
||||
LSM303D *g_dev;
|
||||
|
||||
void start(bool external_bus, enum Rotation rotation);
|
||||
void start(bool external_bus, enum Rotation rotation, unsigned range);
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
|
|
@ -1856,11 +1878,12 @@ void test_error();
|
|||
* up and running or failed to detect the sensor.
|
||||
*/
|
||||
void
|
||||
start(bool external_bus, enum Rotation rotation)
|
||||
start(bool external_bus, enum Rotation rotation, unsigned range)
|
||||
{
|
||||
int fd, fd_mag;
|
||||
if (g_dev != nullptr)
|
||||
if (g_dev != nullptr) {
|
||||
errx(0, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
if (external_bus) {
|
||||
|
|
@ -1884,11 +1907,17 @@ start(bool external_bus, enum Rotation rotation)
|
|||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
|
||||
|
||||
|
|
@ -1980,7 +2009,10 @@ test()
|
|||
warnx("mag z: \t%d\traw", (int)m_report.z_raw);
|
||||
warnx("mag range: %8.4f ga", (double)m_report.range_ga);
|
||||
|
||||
/* XXX add poll-rate tests here too */
|
||||
/* reset to default polling */
|
||||
if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
err(1, "reset to default polling");
|
||||
}
|
||||
|
||||
close(fd_accel);
|
||||
close(fd_mag);
|
||||
|
|
@ -2084,9 +2116,10 @@ lsm303d_main(int argc, char *argv[])
|
|||
bool external_bus = false;
|
||||
int ch;
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int accel_range = 8;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
|
||||
while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
external_bus = true;
|
||||
|
|
@ -2094,6 +2127,9 @@ lsm303d_main(int argc, char *argv[])
|
|||
case 'R':
|
||||
rotation = (enum Rotation)atoi(optarg);
|
||||
break;
|
||||
case 'a':
|
||||
accel_range = atoi(optarg);
|
||||
break;
|
||||
default:
|
||||
lsm303d::usage();
|
||||
exit(0);
|
||||
|
|
@ -2107,7 +2143,7 @@ lsm303d_main(int argc, char *argv[])
|
|||
|
||||
*/
|
||||
if (!strcmp(verb, "start"))
|
||||
lsm303d::start(external_bus, rotation);
|
||||
lsm303d::start(external_bus, rotation, accel_range);
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
|
|
|
|||
|
|
@ -69,6 +69,7 @@
|
|||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
|
|
@ -167,10 +168,13 @@
|
|||
|
||||
#define MPU6000_ACCEL_DEFAULT_RANGE_G 8
|
||||
#define MPU6000_ACCEL_DEFAULT_RATE 1000
|
||||
#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280
|
||||
#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
|
||||
#define MPU6000_GYRO_DEFAULT_RANGE_G 8
|
||||
#define MPU6000_GYRO_DEFAULT_RATE 1000
|
||||
/* rates need to be the same between accel and gyro */
|
||||
#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE
|
||||
#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
|
||||
#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
|
||||
|
|
@ -281,6 +285,9 @@ private:
|
|||
math::LowPassFilter2p _gyro_filter_y;
|
||||
math::LowPassFilter2p _gyro_filter_z;
|
||||
|
||||
Integrator _accel_int;
|
||||
Integrator _gyro_int;
|
||||
|
||||
enum Rotation _rotation;
|
||||
|
||||
// this is used to support runtime checking of key
|
||||
|
|
@ -535,6 +542,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
|
|||
_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_accel_int(1000000 / MPU6000_ACCEL_MAX_OUTPUT_RATE),
|
||||
_gyro_int(1000000 / MPU6000_GYRO_MAX_OUTPUT_RATE, true),
|
||||
_rotation(rotation),
|
||||
_checked_next(0),
|
||||
_in_factory_test(false),
|
||||
|
|
@ -1525,6 +1534,13 @@ void
|
|||
MPU6000::stop()
|
||||
{
|
||||
hrt_cancel(&_call);
|
||||
|
||||
/* reset internal states */
|
||||
memset(_last_accel, 0, sizeof(_last_accel));
|
||||
|
||||
/* discard unread data in the buffers */
|
||||
_accel_reports->flush();
|
||||
_gyro_reports->flush();
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -1759,6 +1775,14 @@ MPU6000::measure()
|
|||
arb.y = _accel_filter_y.apply(y_in_new);
|
||||
arb.z = _accel_filter_z.apply(z_in_new);
|
||||
|
||||
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
|
||||
math::Vector<3> aval_integrated;
|
||||
|
||||
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
|
||||
arb.x_integral = aval_integrated(0);
|
||||
arb.y_integral = aval_integrated(1);
|
||||
arb.z_integral = aval_integrated(2);
|
||||
|
||||
arb.scaling = _accel_range_scale;
|
||||
arb.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
|
|
@ -1786,6 +1810,14 @@ MPU6000::measure()
|
|||
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
|
||||
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
|
||||
|
||||
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
|
||||
math::Vector<3> gval_integrated;
|
||||
|
||||
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
|
||||
grb.x_integral = gval_integrated(0);
|
||||
grb.y_integral = gval_integrated(1);
|
||||
grb.z_integral = gval_integrated(2);
|
||||
|
||||
grb.scaling = _gyro_range_scale;
|
||||
grb.range_rad_s = _gyro_range_rad_s;
|
||||
|
||||
|
|
@ -1796,10 +1828,15 @@ MPU6000::measure()
|
|||
_gyro_reports->force(&grb);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
_gyro->parent_poll_notify();
|
||||
if (accel_notify) {
|
||||
poll_notify(POLLIN);
|
||||
}
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
if (gyro_notify) {
|
||||
_gyro->parent_poll_notify();
|
||||
}
|
||||
|
||||
if (accel_notify && !(_pub_blocked)) {
|
||||
/* log the time of this report */
|
||||
perf_begin(_controller_latency_perf);
|
||||
perf_begin(_system_latency_perf);
|
||||
|
|
@ -1807,7 +1844,7 @@ MPU6000::measure()
|
|||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
|
||||
}
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
if (gyro_notify && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
|
||||
}
|
||||
|
|
@ -1925,7 +1962,7 @@ namespace mpu6000
|
|||
MPU6000 *g_dev_int; // on internal bus
|
||||
MPU6000 *g_dev_ext; // on external bus
|
||||
|
||||
void start(bool, enum Rotation);
|
||||
void start(bool, enum Rotation, int range);
|
||||
void stop(bool);
|
||||
void test(bool);
|
||||
void reset(bool);
|
||||
|
|
@ -1942,7 +1979,7 @@ void usage();
|
|||
* or failed to detect the sensor.
|
||||
*/
|
||||
void
|
||||
start(bool external_bus, enum Rotation rotation)
|
||||
start(bool external_bus, enum Rotation rotation, int range)
|
||||
{
|
||||
int fd;
|
||||
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
|
||||
|
|
@ -1979,6 +2016,9 @@ start(bool external_bus, enum Rotation rotation)
|
|||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
goto fail;
|
||||
|
||||
if (ioctl(fd, ACCELIOCSRANGE, range) < 0)
|
||||
goto fail;
|
||||
|
||||
close(fd);
|
||||
|
||||
exit(0);
|
||||
|
|
@ -2076,6 +2116,12 @@ test(bool external_bus)
|
|||
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
|
||||
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
|
||||
|
||||
/* reset to default polling */
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
err(1, "reset to default polling");
|
||||
|
||||
close(fd);
|
||||
close(fd_gyro);
|
||||
|
||||
/* XXX add poll-rate tests here too */
|
||||
|
||||
|
|
@ -2175,6 +2221,7 @@ usage()
|
|||
warnx("options:");
|
||||
warnx(" -X (external bus)");
|
||||
warnx(" -R rotation");
|
||||
warnx(" -a accel range (in g)");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
|
@ -2185,9 +2232,10 @@ mpu6000_main(int argc, char *argv[])
|
|||
bool external_bus = false;
|
||||
int ch;
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int accel_range = 8;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
|
||||
while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
external_bus = true;
|
||||
|
|
@ -2195,6 +2243,9 @@ mpu6000_main(int argc, char *argv[])
|
|||
case 'R':
|
||||
rotation = (enum Rotation)atoi(optarg);
|
||||
break;
|
||||
case 'a':
|
||||
accel_range = atoi(optarg);
|
||||
break;
|
||||
default:
|
||||
mpu6000::usage();
|
||||
exit(0);
|
||||
|
|
@ -2208,7 +2259,7 @@ mpu6000_main(int argc, char *argv[])
|
|||
|
||||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
mpu6000::start(external_bus, rotation);
|
||||
mpu6000::start(external_bus, rotation, accel_range);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
|
|
|
|||
|
|
@ -274,7 +274,7 @@ MS5611::init()
|
|||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
|
||||
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
DEVICE_DEBUG("can't get memory for reports");
|
||||
|
|
|
|||
|
|
@ -38,6 +38,9 @@
|
|||
*/
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_log.h>
|
||||
|
||||
#define ecl_absolute_time hrt_absolute_time
|
||||
#define ecl_elapsed_time hrt_elapsed_time
|
||||
#define ECL_WARN PX4_WARN
|
||||
#define ECL_INFO PX4_INFO
|
||||
|
|
|
|||
|
|
@ -39,6 +39,8 @@ SRCS = attitude_fw/ecl_controller.cpp \
|
|||
attitude_fw/ecl_pitch_controller.cpp \
|
||||
attitude_fw/ecl_roll_controller.cpp \
|
||||
attitude_fw/ecl_yaw_controller.cpp \
|
||||
l1/ecl_l1_pos_controller.cpp
|
||||
l1/ecl_l1_pos_controller.cpp \
|
||||
validation/data_validator.cpp \
|
||||
validation/data_validator_group.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
|
|
@ -0,0 +1,147 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file data_validator.c
|
||||
*
|
||||
* A data validation class to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "data_validator.h"
|
||||
#include <ecl/ecl.h>
|
||||
|
||||
DataValidator::DataValidator(DataValidator *prev_sibling) :
|
||||
_time_last(0),
|
||||
_timeout_interval(70000),
|
||||
_event_count(0),
|
||||
_error_count(0),
|
||||
_priority(0),
|
||||
_mean{0.0f},
|
||||
_lp{0.0f},
|
||||
_M2{0.0f},
|
||||
_rms{0.0f},
|
||||
_value{0.0f},
|
||||
_value_equal_count(0),
|
||||
_sibling(prev_sibling)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
DataValidator::~DataValidator()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
|
||||
{
|
||||
_event_count++;
|
||||
_error_count = error_count_in;
|
||||
_priority = priority_in;
|
||||
|
||||
for (unsigned i = 0; i < _dimensions; i++) {
|
||||
if (_time_last == 0) {
|
||||
_mean[i] = 0;
|
||||
_lp[i] = val[i];
|
||||
_M2[i] = 0;
|
||||
} else {
|
||||
float lp_val = val[i] - _lp[i];
|
||||
|
||||
float delta_val = lp_val - _mean[i];
|
||||
_mean[i] += delta_val / _event_count;
|
||||
_M2[i] += delta_val * (lp_val - _mean[i]);
|
||||
_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
|
||||
|
||||
if (fabsf(_value[i] - val[i]) < 0.000001f) {
|
||||
_value_equal_count++;
|
||||
} else {
|
||||
_value_equal_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// XXX replace with better filter, make it auto-tune to update rate
|
||||
_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
|
||||
|
||||
_value[i] = val[i];
|
||||
}
|
||||
|
||||
_time_last = timestamp;
|
||||
}
|
||||
|
||||
float
|
||||
DataValidator::confidence(uint64_t timestamp)
|
||||
{
|
||||
/* check if we have any data */
|
||||
if (_time_last == 0) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* check error count limit */
|
||||
if (_error_count > NORETURN_ERRCOUNT) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* we got the exact same sensor value N times in a row */
|
||||
if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* timed out - that's it */
|
||||
if (timestamp - _time_last > _timeout_interval) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
return 1.0f;
|
||||
}
|
||||
|
||||
int
|
||||
DataValidator::priority()
|
||||
{
|
||||
return _priority;
|
||||
}
|
||||
|
||||
void
|
||||
DataValidator::print()
|
||||
{
|
||||
if (_time_last == 0) {
|
||||
ECL_INFO("\tno data\n");
|
||||
return;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < _dimensions; i++) {
|
||||
ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
|
||||
(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file data_validator.h
|
||||
*
|
||||
* A data validation class to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <stdint.h>
|
||||
|
||||
class __EXPORT DataValidator {
|
||||
public:
|
||||
DataValidator(DataValidator *prev_sibling = nullptr);
|
||||
virtual ~DataValidator();
|
||||
|
||||
/**
|
||||
* Put an item into the validator.
|
||||
*
|
||||
* @param val Item to put
|
||||
*/
|
||||
void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority);
|
||||
|
||||
/**
|
||||
* Get the next sibling in the group
|
||||
*
|
||||
* @return the next sibling
|
||||
*/
|
||||
DataValidator* sibling() { return _sibling; }
|
||||
|
||||
/**
|
||||
* Get the confidence of this validator
|
||||
* @return the confidence between 0 and 1
|
||||
*/
|
||||
float confidence(uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Get the error count of this validator
|
||||
* @return the error count
|
||||
*/
|
||||
uint64_t error_count() { return _error_count; }
|
||||
|
||||
/**
|
||||
* Get the values of this validator
|
||||
* @return the stored value
|
||||
*/
|
||||
float* value() { return _value; }
|
||||
|
||||
/**
|
||||
* Get the priority of this validator
|
||||
* @return the stored priority
|
||||
*/
|
||||
int priority();
|
||||
|
||||
/**
|
||||
* Get the RMS values of this validator
|
||||
* @return the stored RMS
|
||||
*/
|
||||
float* rms() { return _rms; }
|
||||
|
||||
/**
|
||||
* Print the validator value
|
||||
*
|
||||
*/
|
||||
void print();
|
||||
|
||||
/**
|
||||
* Set the timeout value
|
||||
*
|
||||
* @param timeout_interval_us The timeout interval in microseconds
|
||||
*/
|
||||
void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
|
||||
|
||||
private:
|
||||
static const unsigned _dimensions = 3;
|
||||
uint64_t _time_last; /**< last timestamp */
|
||||
uint64_t _timeout_interval; /**< interval in which the datastream times out in us */
|
||||
uint64_t _event_count; /**< total data counter */
|
||||
uint64_t _error_count; /**< error count */
|
||||
int _priority; /**< sensor nominal priority */
|
||||
float _mean[_dimensions]; /**< mean of value */
|
||||
float _lp[3]; /**< low pass value */
|
||||
float _M2[3]; /**< RMS component value */
|
||||
float _rms[3]; /**< root mean square error */
|
||||
float _value[3]; /**< last value */
|
||||
float _value_equal_count; /**< equal values in a row */
|
||||
DataValidator *_sibling; /**< sibling in the group */
|
||||
const unsigned NORETURN_ERRCOUNT = 100; /**< if the error count reaches this value, return sensor as invalid */
|
||||
const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
|
||||
|
||||
/* we don't want this class to be copied */
|
||||
DataValidator(const DataValidator&);
|
||||
DataValidator operator=(const DataValidator&);
|
||||
};
|
||||
|
|
@ -0,0 +1,199 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file data_validator_group.cpp
|
||||
*
|
||||
* A data validation group to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "data_validator_group.h"
|
||||
#include <ecl/ecl.h>
|
||||
|
||||
DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
|
||||
_first(nullptr),
|
||||
_curr_best(-1),
|
||||
_prev_best(-1),
|
||||
_first_failover_time(0),
|
||||
_toggle_count(0)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
|
||||
for (unsigned i = 0; i < siblings; i++) {
|
||||
next = new DataValidator(next);
|
||||
}
|
||||
|
||||
_first = next;
|
||||
}
|
||||
|
||||
DataValidatorGroup::~DataValidatorGroup()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
|
||||
while (next != nullptr) {
|
||||
next->set_timeout(timeout_interval_us);
|
||||
next = next->sibling();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (i == index) {
|
||||
next->put(timestamp, val, error_count, priority);
|
||||
break;
|
||||
}
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
float*
|
||||
DataValidatorGroup::get_best(uint64_t timestamp, int *index)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
|
||||
// XXX This should eventually also include voting
|
||||
int pre_check_best = _curr_best;
|
||||
float max_confidence = -1.0f;
|
||||
int max_priority = -1000;
|
||||
int max_index = -1;
|
||||
uint64_t min_error_count = 30000;
|
||||
DataValidator *best = nullptr;
|
||||
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
float confidence = next->confidence(timestamp);
|
||||
if (confidence > max_confidence ||
|
||||
(fabsf(confidence - max_confidence) < 0.01f &&
|
||||
((next->error_count() < min_error_count) &&
|
||||
(next->priority() >= max_priority)))) {
|
||||
max_index = i;
|
||||
max_confidence = confidence;
|
||||
max_priority = next->priority();
|
||||
min_error_count = next->error_count();
|
||||
best = next;
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
|
||||
/* the current best sensor is not matching the previous best sensor */
|
||||
if (max_index != _curr_best) {
|
||||
|
||||
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
|
||||
if (_curr_best < 0) {
|
||||
_prev_best = max_index;
|
||||
} else {
|
||||
/* we were initialized before, this is a real failsafe */
|
||||
_prev_best = pre_check_best;
|
||||
_toggle_count++;
|
||||
|
||||
/* if this is the first time, log when we failed */
|
||||
if (_first_failover_time == 0) {
|
||||
_first_failover_time = timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
/* for all cases we want to keep a record of the best index */
|
||||
_curr_best = max_index;
|
||||
}
|
||||
*index = max_index;
|
||||
return (best) ? best->value() : nullptr;
|
||||
}
|
||||
|
||||
float
|
||||
DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
|
||||
float vibe = 0.0f;
|
||||
|
||||
/* find the best RMS value of a non-timed out sensor */
|
||||
while (next != nullptr) {
|
||||
|
||||
if (next->confidence(timestamp) > 0.5f) {
|
||||
float* rms = next->rms();
|
||||
|
||||
for (unsigned j = 0; j < 3; j++) {
|
||||
if (rms[j] > vibe) {
|
||||
vibe = rms[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
}
|
||||
|
||||
return vibe;
|
||||
}
|
||||
|
||||
void
|
||||
DataValidatorGroup::print()
|
||||
{
|
||||
/* print the group's state */
|
||||
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
|
||||
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
|
||||
_toggle_count);
|
||||
|
||||
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
ECL_INFO("sensor #%u:\n", i);
|
||||
next->print();
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned
|
||||
DataValidatorGroup::failover_count()
|
||||
{
|
||||
return _toggle_count;
|
||||
}
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file data_validator_group.h
|
||||
*
|
||||
* A data validation group to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "data_validator.h"
|
||||
|
||||
class __EXPORT DataValidatorGroup {
|
||||
public:
|
||||
DataValidatorGroup(unsigned siblings);
|
||||
virtual ~DataValidatorGroup();
|
||||
|
||||
/**
|
||||
* Put an item into the validator group.
|
||||
*
|
||||
* @param index Sensor index
|
||||
* @param timestamp The timestamp of the measurement
|
||||
* @param val The 3D vector
|
||||
* @param error_count The current error count of the sensor
|
||||
* @param priority The priority of the sensor
|
||||
*/
|
||||
void put(unsigned index, uint64_t timestamp,
|
||||
float val[3], uint64_t error_count, int priority);
|
||||
|
||||
/**
|
||||
* Get the best data triplet of the group
|
||||
*
|
||||
* @return pointer to the array of best values
|
||||
*/
|
||||
float* get_best(uint64_t timestamp, int *index);
|
||||
|
||||
/**
|
||||
* Get the RMS / vibration factor
|
||||
*
|
||||
* @return float value representing the RMS, which a valid indicator for vibration
|
||||
*/
|
||||
float get_vibration_factor(uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Get the number of failover events
|
||||
*
|
||||
* @return the number of failovers
|
||||
*/
|
||||
unsigned failover_count();
|
||||
|
||||
/**
|
||||
* Print the validator value
|
||||
*
|
||||
*/
|
||||
void print();
|
||||
|
||||
/**
|
||||
* Set the timeout value
|
||||
*
|
||||
* @param timeout_interval_us The timeout interval in microseconds
|
||||
*/
|
||||
void set_timeout(uint64_t timeout_interval_us);
|
||||
|
||||
private:
|
||||
DataValidator *_first; /**< sibling in the group */
|
||||
int _curr_best; /**< currently best index */
|
||||
int _prev_best; /**< the previous best index */
|
||||
uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */
|
||||
unsigned _toggle_count; /**< number of back and forth switches between two sensors */
|
||||
|
||||
/* we don't want this class to be copied */
|
||||
DataValidatorGroup(const DataValidatorGroup&);
|
||||
DataValidatorGroup operator=(const DataValidatorGroup&);
|
||||
};
|
||||
|
|
@ -383,10 +383,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
|||
uint8_t update_vect[3] = {0, 0, 0};
|
||||
|
||||
/* Fill in gyro measurements */
|
||||
if (sensor_last_timestamp[0] != raw.timestamp) {
|
||||
if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
|
||||
update_vect[0] = 1;
|
||||
// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
|
||||
sensor_last_timestamp[0] = raw.timestamp;
|
||||
sensor_last_timestamp[0] = raw.gyro_timestamp[0];
|
||||
}
|
||||
|
||||
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
|
||||
|
|
@ -394,10 +394,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
|||
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
|
||||
|
||||
/* update accelerometer measurements */
|
||||
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
|
||||
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
|
||||
update_vect[1] = 1;
|
||||
// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
|
||||
sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
|
||||
}
|
||||
|
||||
hrt_abstime vel_t = 0;
|
||||
|
|
@ -437,14 +437,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
|||
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
|
||||
|
||||
/* update magnetometer measurements */
|
||||
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
|
||||
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
|
||||
/* check that the mag vector is > 0 */
|
||||
fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
|
||||
raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
|
||||
raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
|
||||
update_vect[2] = 1;
|
||||
// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
|
||||
sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
|
||||
}
|
||||
|
||||
bool vision_updated = false;
|
||||
|
|
|
|||
|
|
@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200
|
|||
EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
|
||||
|
||||
ifeq ($(PX4_TARGET_OS),nuttx)
|
||||
EXTRACXXFLAGS = -Wframe-larger-than=2400
|
||||
EXTRACXXFLAGS = -Wframe-larger-than=2600
|
||||
endif
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@
|
|||
#include <limits.h>
|
||||
#include <math.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
|
@ -59,8 +58,11 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/ecl/validation/data_validator_group.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
|
@ -103,6 +105,8 @@ public:
|
|||
|
||||
void task_main();
|
||||
|
||||
void print();
|
||||
|
||||
private:
|
||||
static constexpr float _dt_max = 0.02;
|
||||
bool _task_should_exit = false; /**< if true, task should exit */
|
||||
|
|
@ -121,6 +125,7 @@ private:
|
|||
param_t mag_decl_auto;
|
||||
param_t acc_comp;
|
||||
param_t bias_max;
|
||||
param_t vibe_thresh;
|
||||
} _params_handles; /**< handles for interesting parameters */
|
||||
|
||||
float _w_accel = 0.0f;
|
||||
|
|
@ -130,6 +135,8 @@ private:
|
|||
bool _mag_decl_auto = false;
|
||||
bool _acc_comp = false;
|
||||
float _bias_max = 0.0f;
|
||||
float _vibration_warning_threshold = 1.0f;
|
||||
hrt_abstime _vibration_warning_timestamp = 0;
|
||||
|
||||
Vector<3> _gyro;
|
||||
Vector<3> _accel;
|
||||
|
|
@ -142,10 +149,23 @@ private:
|
|||
vehicle_global_position_s _gpos = {};
|
||||
Vector<3> _vel_prev;
|
||||
Vector<3> _pos_acc;
|
||||
|
||||
DataValidatorGroup _voter_gyro;
|
||||
DataValidatorGroup _voter_accel;
|
||||
DataValidatorGroup _voter_mag;
|
||||
|
||||
/* Low pass filter for attitude rates */
|
||||
math::LowPassFilter2p _lp_roll_rate;
|
||||
math::LowPassFilter2p _lp_pitch_rate;
|
||||
|
||||
hrt_abstime _vel_prev_t = 0;
|
||||
|
||||
bool _inited = false;
|
||||
bool _data_good = false;
|
||||
bool _failsafe = false;
|
||||
bool _vibration_warning = false;
|
||||
|
||||
int _mavlink_fd = -1;
|
||||
|
||||
perf_counter_t _update_perf;
|
||||
perf_counter_t _loop_perf;
|
||||
|
|
@ -160,7 +180,15 @@ private:
|
|||
};
|
||||
|
||||
|
||||
AttitudeEstimatorQ::AttitudeEstimatorQ() {
|
||||
AttitudeEstimatorQ::AttitudeEstimatorQ() :
|
||||
_voter_gyro(3),
|
||||
_voter_accel(3),
|
||||
_voter_mag(3),
|
||||
_lp_roll_rate(250.0f, 20.0f),
|
||||
_lp_pitch_rate(250.0f, 20.0f)
|
||||
{
|
||||
_voter_mag.set_timeout(200000);
|
||||
|
||||
_params_handles.w_acc = param_find("ATT_W_ACC");
|
||||
_params_handles.w_mag = param_find("ATT_W_MAG");
|
||||
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
|
||||
|
|
@ -168,12 +196,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() {
|
|||
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
|
||||
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
|
||||
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
|
||||
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
|
||||
}
|
||||
|
||||
/**
|
||||
* Destructor, also kills task.
|
||||
*/
|
||||
AttitudeEstimatorQ::~AttitudeEstimatorQ() {
|
||||
AttitudeEstimatorQ::~AttitudeEstimatorQ()
|
||||
{
|
||||
if (_control_task != -1) {
|
||||
/* task wakes up every 100ms or so at the longest */
|
||||
_task_should_exit = true;
|
||||
|
|
@ -196,14 +226,15 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() {
|
|||
attitude_estimator_q::instance = nullptr;
|
||||
}
|
||||
|
||||
int AttitudeEstimatorQ::start() {
|
||||
int AttitudeEstimatorQ::start()
|
||||
{
|
||||
ASSERT(_control_task == -1);
|
||||
|
||||
/* start the task */
|
||||
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
2100,
|
||||
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
|
@ -215,12 +246,23 @@ int AttitudeEstimatorQ::start() {
|
|||
return OK;
|
||||
}
|
||||
|
||||
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
|
||||
void AttitudeEstimatorQ::print()
|
||||
{
|
||||
warnx("gyro status:");
|
||||
_voter_gyro.print();
|
||||
warnx("accel status:");
|
||||
_voter_accel.print();
|
||||
warnx("mag status:");
|
||||
_voter_mag.print();
|
||||
}
|
||||
|
||||
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
attitude_estimator_q::instance->task_main();
|
||||
}
|
||||
|
||||
void AttitudeEstimatorQ::task_main() {
|
||||
|
||||
void AttitudeEstimatorQ::task_main()
|
||||
{
|
||||
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
|
|
@ -236,6 +278,10 @@ void AttitudeEstimatorQ::task_main() {
|
|||
while (!_task_should_exit) {
|
||||
int ret = px4_poll(fds, 1, 1000);
|
||||
|
||||
if (_mavlink_fd < 0) {
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
|
||||
if (ret < 0) {
|
||||
// Poll error, sleep and try again
|
||||
usleep(10000);
|
||||
|
|
@ -250,11 +296,71 @@ void AttitudeEstimatorQ::task_main() {
|
|||
// Update sensors
|
||||
sensor_combined_s sensors;
|
||||
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
|
||||
_gyro.set(sensors.gyro_rad_s);
|
||||
_accel.set(sensors.accelerometer_m_s2);
|
||||
_mag.set(sensors.magnetometer_ga);
|
||||
// Feed validator with recent sensor data
|
||||
|
||||
for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
|
||||
|
||||
/* ignore empty fields */
|
||||
if (sensors.gyro_timestamp[i] > 0) {
|
||||
|
||||
float gyro[3];
|
||||
|
||||
for (unsigned j = 0; j < 3; j++) {
|
||||
if (sensors.gyro_integral_dt[i] > 0) {
|
||||
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
|
||||
} else {
|
||||
/* fall back to angular rate */
|
||||
gyro[j] = sensors.gyro_rad_s[i * 3 + j];
|
||||
}
|
||||
}
|
||||
|
||||
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
|
||||
}
|
||||
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
|
||||
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
|
||||
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
|
||||
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
|
||||
}
|
||||
|
||||
int best_gyro, best_accel, best_mag;
|
||||
|
||||
// Get best measurement values
|
||||
hrt_abstime curr_time = hrt_absolute_time();
|
||||
_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
|
||||
_accel.set(_voter_accel.get_best(curr_time, &best_accel));
|
||||
_mag.set(_voter_mag.get_best(curr_time, &best_mag));
|
||||
|
||||
if (_accel.length() < 0.01f || _mag.length() < 0.01f) {
|
||||
warnx("WARNING: degenerate accel / mag!");
|
||||
continue;
|
||||
}
|
||||
|
||||
_data_good = true;
|
||||
|
||||
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
|
||||
_voter_accel.failover_count() > 0 ||
|
||||
_voter_mag.failover_count() > 0)) {
|
||||
|
||||
_failsafe = true;
|
||||
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
|
||||
}
|
||||
|
||||
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
|
||||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
|
||||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
|
||||
|
||||
if (_vibration_warning_timestamp == 0) {
|
||||
_vibration_warning_timestamp = curr_time;
|
||||
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
|
||||
_vibration_warning = true;
|
||||
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
|
||||
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
|
||||
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
|
||||
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
|
||||
}
|
||||
} else {
|
||||
_vibration_warning_timestamp = 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool gpos_updated;
|
||||
|
|
@ -311,8 +417,11 @@ void AttitudeEstimatorQ::task_main() {
|
|||
att.pitch = euler(1);
|
||||
att.yaw = euler(2);
|
||||
|
||||
att.rollspeed = _rates(0);
|
||||
att.pitchspeed = _rates(1);
|
||||
/* the complimentary filter should reflect the true system
|
||||
* state, but we need smoother outputs for the control system
|
||||
*/
|
||||
att.rollspeed = _lp_roll_rate.apply(_rates(0));
|
||||
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
|
||||
att.yawspeed = _rates(2);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
|
|
@ -328,6 +437,10 @@ void AttitudeEstimatorQ::task_main() {
|
|||
memcpy(&att.R[0], R.data, sizeof(att.R));
|
||||
att.R_valid = true;
|
||||
|
||||
att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
|
||||
att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
|
||||
att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
|
||||
|
||||
if (_att_pub == nullptr) {
|
||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
} else {
|
||||
|
|
@ -358,6 +471,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) {
|
|||
param_get(_params_handles.acc_comp, &acc_comp_int);
|
||||
_acc_comp = acc_comp_int != 0;
|
||||
param_get(_params_handles.bias_max, &_bias_max);
|
||||
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -501,6 +615,7 @@ int attitude_estimator_q_main(int argc, char *argv[]) {
|
|||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (attitude_estimator_q::instance) {
|
||||
attitude_estimator_q::instance->print();
|
||||
warnx("running");
|
||||
return 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -108,3 +108,12 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
|
|||
* @unit rad/s
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
|
||||
|
||||
/**
|
||||
* Threshold (of RMS) to warn about high vibration levels
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @min 0.001
|
||||
* @max 100
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f);
|
||||
|
|
|
|||
|
|
@ -1438,7 +1438,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
* vertical separation from other airtraffic the operator has to know when the
|
||||
* barometer is inoperational.
|
||||
* */
|
||||
if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
|
||||
if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
|
||||
/* handle the case where baro was regained */
|
||||
if (status.barometer_failure) {
|
||||
status.barometer_failure = false;
|
||||
|
|
|
|||
|
|
@ -55,6 +55,6 @@ MODULE_STACKSIZE = 5000
|
|||
MAXOPTIMIZATION = -Os
|
||||
|
||||
ifeq ($(PX4_TARGET_OS),nuttx)
|
||||
EXTRACXXFLAGS = -Wframe-larger-than=2200
|
||||
EXTRACXXFLAGS = -Wframe-larger-than=2400
|
||||
endif
|
||||
|
||||
|
|
|
|||
|
|
@ -97,6 +97,11 @@ void BlockParam<T>::update() {
|
|||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
void BlockParam<T>::commit() {
|
||||
if (_handle != PARAM_INVALID) param_set(_handle, &_val);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
BlockParam<T>::~BlockParam() {};
|
||||
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@ public:
|
|||
BlockParam(Block *block, const char *name,
|
||||
bool parent_prefix = true);
|
||||
T get();
|
||||
void commit();
|
||||
void set(T val);
|
||||
void update();
|
||||
virtual ~BlockParam();
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@
|
|||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
|
@ -70,11 +69,16 @@
|
|||
|
||||
#include <geo/geo.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <lib/ecl/validation/data_validator_group.h>
|
||||
#include "estimator_22states.h"
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
//Forward declaration
|
||||
class AttPosEKF;
|
||||
|
||||
class AttitudePositionEstimatorEKF
|
||||
class AttitudePositionEstimatorEKF : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
|
@ -168,9 +172,8 @@ private:
|
|||
struct vehicle_land_detected_s _landDetector;
|
||||
struct actuator_armed_s _armed;
|
||||
|
||||
struct gyro_scale _gyro_offsets[3];
|
||||
struct accel_scale _accel_offsets[3];
|
||||
struct mag_scale _mag_offsets[3];
|
||||
hrt_abstime _last_accel;
|
||||
hrt_abstime _last_mag;
|
||||
|
||||
struct sensor_combined_s _sensor_combined;
|
||||
|
||||
|
|
@ -179,6 +182,8 @@ private:
|
|||
float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
|
||||
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
|
||||
hrt_abstime _last_debug_print = 0;
|
||||
float _vibration_warning_threshold = 1.0f;
|
||||
hrt_abstime _vibration_warning_timestamp = 0;
|
||||
|
||||
perf_counter_t _loop_perf; ///< loop performance counter
|
||||
perf_counter_t _loop_intvl; ///< loop rate counter
|
||||
|
|
@ -198,14 +203,16 @@ private:
|
|||
bool _gps_initialized;
|
||||
hrt_abstime _filter_start_time;
|
||||
hrt_abstime _last_sensor_timestamp;
|
||||
hrt_abstime _last_run;
|
||||
hrt_abstime _distance_last_valid;
|
||||
bool _gyro_valid;
|
||||
bool _accel_valid;
|
||||
bool _mag_valid;
|
||||
DataValidatorGroup _voter_gyro;
|
||||
DataValidatorGroup _voter_accel;
|
||||
DataValidatorGroup _voter_mag;
|
||||
int _gyro_main; ///< index of the main gyroscope
|
||||
int _accel_main; ///< index of the main accelerometer
|
||||
int _mag_main; ///< index of the main magnetometer
|
||||
bool _data_good; ///< all required filter data is ok
|
||||
bool _failsafe; ///< failsafe on one of the sensors
|
||||
bool _vibration_warning; ///< high vibration levels detected
|
||||
bool _ekf_logging; ///< log EKF state
|
||||
unsigned _debug; ///< debug level - default 0
|
||||
|
||||
|
|
@ -216,6 +223,10 @@ private:
|
|||
|
||||
int _mavlink_fd;
|
||||
|
||||
control::BlockParamFloat _mag_offset_x;
|
||||
control::BlockParamFloat _mag_offset_y;
|
||||
control::BlockParamFloat _mag_offset_z;
|
||||
|
||||
struct {
|
||||
int32_t vel_delay_ms;
|
||||
int32_t pos_delay_ms;
|
||||
|
|
|
|||
|
|
@ -68,7 +68,6 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
#include <platforms/px4_defines.h>
|
||||
|
||||
static uint64_t IMUmsec = 0;
|
||||
static uint64_t IMUusec = 0;
|
||||
|
||||
//Constants
|
||||
|
|
@ -88,13 +87,18 @@ static constexpr float EPV_LARGE_VALUE = 1000.0f;
|
|||
*/
|
||||
extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
|
||||
|
||||
__EXPORT uint32_t millis();
|
||||
|
||||
__EXPORT uint64_t getMicros();
|
||||
uint32_t millis();
|
||||
uint64_t getMicros();
|
||||
uint32_t getMillis();
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
return IMUmsec;
|
||||
return getMillis();
|
||||
}
|
||||
|
||||
uint32_t getMillis()
|
||||
{
|
||||
return getMicros() / 1000;
|
||||
}
|
||||
|
||||
uint64_t getMicros()
|
||||
|
|
@ -115,6 +119,7 @@ AttitudePositionEstimatorEKF *g_estimator = nullptr;
|
|||
}
|
||||
|
||||
AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
||||
SuperBlock(NULL, "EKF"),
|
||||
_task_should_exit(false),
|
||||
_task_running(false),
|
||||
_estimator_task(-1),
|
||||
|
|
@ -133,84 +138,88 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||
_landDetectorSub(-1),
|
||||
_armedSub(-1),
|
||||
|
||||
/* publications */
|
||||
/* publications */
|
||||
_att_pub(nullptr),
|
||||
_global_pos_pub(nullptr),
|
||||
_local_pos_pub(nullptr),
|
||||
_estimator_status_pub(nullptr),
|
||||
_wind_pub(nullptr),
|
||||
|
||||
_att({}),
|
||||
_gyro({}),
|
||||
_accel({}),
|
||||
_mag({}),
|
||||
_airspeed({}),
|
||||
_baro({}),
|
||||
_vstatus({}),
|
||||
_global_pos({}),
|
||||
_local_pos({}),
|
||||
_gps({}),
|
||||
_wind({}),
|
||||
_distance {},
|
||||
_landDetector {},
|
||||
_armed {},
|
||||
_att{},
|
||||
_gyro{},
|
||||
_accel{},
|
||||
_mag{},
|
||||
_airspeed{},
|
||||
_baro{},
|
||||
_vstatus{},
|
||||
_global_pos{},
|
||||
_local_pos{},
|
||||
_gps{},
|
||||
_wind{},
|
||||
_distance{},
|
||||
_landDetector{},
|
||||
_armed{},
|
||||
|
||||
_gyro_offsets({}),
|
||||
_accel_offsets({}),
|
||||
_mag_offsets({}),
|
||||
_last_accel(0),
|
||||
_last_mag(0),
|
||||
|
||||
_sensor_combined {},
|
||||
_sensor_combined{},
|
||||
|
||||
_pos_ref{},
|
||||
_filter_ref_offset(0.0f),
|
||||
_baro_gps_offset(0.0f),
|
||||
_pos_ref{},
|
||||
_filter_ref_offset(0.0f),
|
||||
_baro_gps_offset(0.0f),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
|
||||
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
|
||||
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
|
||||
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
|
||||
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
|
||||
|
||||
/* states */
|
||||
_gps_alt_filt(0.0f),
|
||||
_baro_alt_filt(0.0f),
|
||||
_covariancePredictionDt(0.0f),
|
||||
_gpsIsGood(false),
|
||||
_previousGPSTimestamp(0),
|
||||
_baro_init(false),
|
||||
_gps_initialized(false),
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
_gyro_main(0),
|
||||
_accel_main(0),
|
||||
_mag_main(0),
|
||||
_ekf_logging(true),
|
||||
_debug(0),
|
||||
/* states */
|
||||
_gps_alt_filt(0.0f),
|
||||
_baro_alt_filt(0.0f),
|
||||
_covariancePredictionDt(0.0f),
|
||||
_gpsIsGood(false),
|
||||
_previousGPSTimestamp(0),
|
||||
_baro_init(false),
|
||||
_gps_initialized(false),
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(hrt_absolute_time()),
|
||||
_distance_last_valid(0),
|
||||
_voter_gyro(3),
|
||||
_voter_accel(3),
|
||||
_voter_mag(3),
|
||||
_gyro_main(-1),
|
||||
_accel_main(-1),
|
||||
_mag_main(-1),
|
||||
_data_good(false),
|
||||
_failsafe(false),
|
||||
_vibration_warning(false),
|
||||
_ekf_logging(true),
|
||||
_debug(0),
|
||||
|
||||
_newHgtData(false),
|
||||
_newAdsData(false),
|
||||
_newDataMag(false),
|
||||
_newRangeData(false),
|
||||
_newHgtData(false),
|
||||
_newAdsData(false),
|
||||
_newDataMag(false),
|
||||
_newRangeData(false),
|
||||
|
||||
_mavlink_fd(-1),
|
||||
_parameters{},
|
||||
_parameter_handles{},
|
||||
_ekf(nullptr),
|
||||
_mavlink_fd(-1),
|
||||
_mag_offset_x(this, "PE_MAGB_X"),
|
||||
_mag_offset_y(this, "PE_MAGB_Y"),
|
||||
_mag_offset_z(this, "PE_MAGB_Z"),
|
||||
_parameters{},
|
||||
_parameter_handles{},
|
||||
_ekf(nullptr),
|
||||
|
||||
_LP_att_P(100.0f, 10.0f),
|
||||
_LP_att_Q(100.0f, 10.0f),
|
||||
_LP_att_R(100.0f, 10.0f)
|
||||
_LP_att_P(250.0f, 20.0f),
|
||||
_LP_att_Q(250.0f, 20.0f),
|
||||
_LP_att_R(250.0f, 20.0f)
|
||||
{
|
||||
_last_run = hrt_absolute_time();
|
||||
_voter_mag.set_timeout(200000);
|
||||
|
||||
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
|
||||
_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
|
||||
|
|
@ -237,48 +246,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
||||
/* get offsets */
|
||||
int fd, res;
|
||||
|
||||
for (unsigned s = 0; s < 3; s++) {
|
||||
char str[30];
|
||||
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
fd = px4_open(str, O_RDONLY);
|
||||
|
||||
if (fd >= 0) {
|
||||
res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]);
|
||||
px4_close(fd);
|
||||
|
||||
if (res) {
|
||||
PX4_WARN("G%u SCALE FAIL", s);
|
||||
}
|
||||
}
|
||||
|
||||
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
|
||||
fd = px4_open(str, O_RDONLY);
|
||||
|
||||
if (fd >= 0) {
|
||||
res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]);
|
||||
px4_close(fd);
|
||||
|
||||
if (res) {
|
||||
PX4_WARN("A%u SCALE FAIL", s);
|
||||
}
|
||||
}
|
||||
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
|
||||
fd = px4_open(str, O_RDONLY);
|
||||
|
||||
if (fd >= 0) {
|
||||
res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]);
|
||||
px4_close(fd);
|
||||
|
||||
if (res) {
|
||||
PX4_WARN("M%u SCALE FAIL", s);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
|
||||
|
|
@ -317,6 +284,8 @@ int AttitudePositionEstimatorEKF::enable_logging(bool logging)
|
|||
|
||||
int AttitudePositionEstimatorEKF::parameters_update()
|
||||
{
|
||||
/* update C++ param system */
|
||||
updateParams();
|
||||
|
||||
param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
|
||||
param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
|
||||
|
|
@ -354,6 +323,13 @@ int AttitudePositionEstimatorEKF::parameters_update()
|
|||
_ekf->accelProcessNoise = _parameters.acc_pnoise;
|
||||
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
|
||||
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
|
||||
#if 0
|
||||
// Initially disable loading until
|
||||
// convergence is flight-test proven
|
||||
_ekf->magBias.x = _mag_offset_x.get();
|
||||
_ekf->magBias.y = _mag_offset_y.get();
|
||||
_ekf->magBias.z = _mag_offset_z.get();
|
||||
#endif
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
|
@ -366,12 +342,24 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll()
|
|||
/* Check HIL state if vehicle status has changed */
|
||||
orb_check(_vstatus_sub, &vstatus_updated);
|
||||
|
||||
bool landed = _vstatus.condition_landed;
|
||||
|
||||
if (vstatus_updated) {
|
||||
|
||||
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
|
||||
|
||||
//Tell EKF that the vehicle is a fixed wing or multi-rotor
|
||||
// Tell EKF that the vehicle is a fixed wing or multi-rotor
|
||||
_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
|
||||
|
||||
// Save params on landed
|
||||
if (!landed && _vstatus.condition_landed) {
|
||||
_mag_offset_x.set(_ekf->magBias.x);
|
||||
_mag_offset_x.commit();
|
||||
_mag_offset_y.set(_ekf->magBias.y);
|
||||
_mag_offset_y.commit();
|
||||
_mag_offset_z.set(_ekf->magBias.z);
|
||||
_mag_offset_z.commit();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -541,8 +529,6 @@ void AttitudePositionEstimatorEKF::task_main()
|
|||
orb_set_interval(_vstatus_sub, 200);
|
||||
|
||||
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
/* XXX remove this!, BUT increase the data buffer size! */
|
||||
orb_set_interval(_sensor_combined_sub, 9);
|
||||
|
||||
/* sets also parameters in the EKF object */
|
||||
parameters_update();
|
||||
|
|
@ -610,15 +596,14 @@ void AttitudePositionEstimatorEKF::task_main()
|
|||
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
|
||||
|
||||
/* set sensors to de-initialized state */
|
||||
_gyro_valid = false;
|
||||
_accel_valid = false;
|
||||
_mag_valid = false;
|
||||
_gyro_main = -1;
|
||||
_accel_main = -1;
|
||||
_mag_main = -1;
|
||||
|
||||
_baro_init = false;
|
||||
_gps_initialized = false;
|
||||
|
||||
_last_sensor_timestamp = hrt_absolute_time();
|
||||
_last_run = _last_sensor_timestamp;
|
||||
|
||||
_ekf->ZeroVariables();
|
||||
_ekf->dtIMU = 0.01f;
|
||||
|
|
@ -646,7 +631,7 @@ void AttitudePositionEstimatorEKF::task_main()
|
|||
* We run the filter only once all data has been fetched
|
||||
**/
|
||||
|
||||
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
|
||||
if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) {
|
||||
|
||||
// maintain filtered baro and gps altitudes to calculate weather offset
|
||||
// baro sample rate is ~70Hz and measurement bandwidth is high
|
||||
|
|
@ -836,9 +821,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
|
|||
_att.pitch = euler(1);
|
||||
_att.yaw = euler(2);
|
||||
|
||||
_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
|
||||
_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
|
||||
_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
|
||||
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
|
||||
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
|
||||
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
|
||||
|
||||
// gyro offsets
|
||||
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
|
||||
|
|
@ -1009,7 +994,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
_ekf->UpdateStrapdownEquationsNED();
|
||||
|
||||
// store the predicted states for subsequent use by measurement fusion
|
||||
_ekf->StoreStates(IMUmsec);
|
||||
_ekf->StoreStates(getMillis());
|
||||
|
||||
// sum delta angles and time used by covariance prediction
|
||||
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
|
||||
|
|
@ -1035,8 +1020,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
_ekf->fusePosData = true;
|
||||
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
|
||||
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
|
@ -1059,8 +1044,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
_ekf->fusePosData = true;
|
||||
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
|
||||
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
|
@ -1076,7 +1061,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
_ekf->fuseHgtData = true;
|
||||
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms));
|
||||
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
|
@ -1089,7 +1074,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
if (fuseMag) {
|
||||
_ekf->fuseMagData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtMagMeasTime,
|
||||
(IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
(getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
|
||||
_ekf->magstate.obsIndex = 0;
|
||||
_ekf->FuseMagnetometer();
|
||||
|
|
@ -1104,7 +1089,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
|
||||
_ekf->fuseVtasData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
|
||||
(IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
(getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
_ekf->FuseAirspeed();
|
||||
|
||||
} else {
|
||||
|
|
@ -1117,7 +1102,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
|||
// _ekf->rngMea is set in sensor readout already
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->fuseOptFlowData = false;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f));
|
||||
_ekf->OpticalFlowEKF();
|
||||
_ekf->fuseRngData = false;
|
||||
}
|
||||
|
|
@ -1131,7 +1116,7 @@ int AttitudePositionEstimatorEKF::start()
|
|||
/* start the task */
|
||||
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
4800,
|
||||
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
|
||||
nullptr);
|
||||
|
|
@ -1163,7 +1148,7 @@ void AttitudePositionEstimatorEKF::print_status()
|
|||
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
|
||||
|
||||
PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
|
||||
PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis());
|
||||
PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt);
|
||||
PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt);
|
||||
PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset,
|
||||
|
|
@ -1210,6 +1195,13 @@ void AttitudePositionEstimatorEKF::print_status()
|
|||
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
|
||||
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
|
||||
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
|
||||
|
||||
PX4_INFO("gyro status:");
|
||||
_voter_gyro.print();
|
||||
PX4_INFO("accel status:");
|
||||
_voter_accel.print();
|
||||
PX4_INFO("mag status:");
|
||||
_voter_mag.print();
|
||||
}
|
||||
|
||||
void AttitudePositionEstimatorEKF::pollData()
|
||||
|
|
@ -1222,125 +1214,153 @@ void AttitudePositionEstimatorEKF::pollData()
|
|||
orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
|
||||
}
|
||||
|
||||
//Update Gyro and Accelerometer
|
||||
static Vector3f lastAngRate;
|
||||
static Vector3f lastAccel;
|
||||
bool accel_updated = false;
|
||||
|
||||
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
|
||||
|
||||
static hrt_abstime last_accel = 0;
|
||||
static hrt_abstime last_mag = 0;
|
||||
// Feed validator with recent sensor data
|
||||
|
||||
if (last_accel != _sensor_combined.accelerometer_timestamp) {
|
||||
accel_updated = true;
|
||||
|
||||
} else {
|
||||
accel_updated = false;
|
||||
for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
|
||||
_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3],
|
||||
_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
|
||||
_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3],
|
||||
_sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
|
||||
_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3],
|
||||
_sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
|
||||
}
|
||||
|
||||
last_accel = _sensor_combined.accelerometer_timestamp;
|
||||
// Get best measurement values
|
||||
hrt_abstime curr_time = hrt_absolute_time();
|
||||
(void)_voter_gyro.get_best(curr_time, &_gyro_main);
|
||||
if (_gyro_main >= 0) {
|
||||
|
||||
// Use pre-integrated values if possible
|
||||
if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) {
|
||||
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
|
||||
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
|
||||
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
|
||||
} else {
|
||||
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]);
|
||||
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]);
|
||||
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]);
|
||||
}
|
||||
|
||||
_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
|
||||
_ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1];
|
||||
_ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2];
|
||||
perf_count(_perf_gyro);
|
||||
}
|
||||
|
||||
(void)_voter_accel.get_best(curr_time, &_accel_main);
|
||||
if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
|
||||
|
||||
// Use pre-integrated values if possible
|
||||
if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) {
|
||||
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
|
||||
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
|
||||
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
|
||||
} else {
|
||||
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
|
||||
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
|
||||
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
|
||||
}
|
||||
|
||||
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
|
||||
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1];
|
||||
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2];
|
||||
_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
|
||||
}
|
||||
|
||||
(void)_voter_mag.get_best(curr_time, &_mag_main);
|
||||
if (_mag_main >= 0) {
|
||||
Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1],
|
||||
_sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
|
||||
|
||||
/* fail over to the 2nd mag if we know the first is down */
|
||||
if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
|
||||
_ekf->magData.x = mag.x;
|
||||
_ekf->magData.y = mag.y;
|
||||
_ekf->magData.z = mag.z;
|
||||
_newDataMag = true;
|
||||
_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
|
||||
perf_count(_perf_mag);
|
||||
}
|
||||
}
|
||||
|
||||
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
|
||||
_voter_accel.failover_count() > 0 ||
|
||||
_voter_mag.failover_count() > 0)) {
|
||||
|
||||
_failsafe = true;
|
||||
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
|
||||
}
|
||||
|
||||
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
|
||||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
|
||||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
|
||||
|
||||
if (_vibration_warning_timestamp == 0) {
|
||||
_vibration_warning_timestamp = curr_time;
|
||||
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
|
||||
_vibration_warning = true;
|
||||
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
|
||||
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
|
||||
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
|
||||
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
|
||||
}
|
||||
} else {
|
||||
_vibration_warning_timestamp = 0;
|
||||
}
|
||||
|
||||
// Copy gyro and accel
|
||||
_last_sensor_timestamp = _sensor_combined.timestamp;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3;
|
||||
IMUusec = _sensor_combined.timestamp;
|
||||
|
||||
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
|
||||
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
|
||||
_last_sensor_timestamp = _sensor_combined.timestamp;
|
||||
|
||||
/* guard against too large deltaT's */
|
||||
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
|
||||
deltaT = 0.01f;
|
||||
}
|
||||
|
||||
_last_run = _sensor_combined.timestamp;
|
||||
|
||||
// Always store data, independent of init status
|
||||
/* fill in last data set */
|
||||
_ekf->dtIMU = deltaT;
|
||||
|
||||
int last_gyro_main = _gyro_main;
|
||||
// XXX this is for assessing the filter performance
|
||||
// leave this in as long as larger improvements are still being made.
|
||||
#if 0
|
||||
|
||||
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
|
||||
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
|
||||
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
|
||||
(_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
|
||||
float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
|
||||
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
|
||||
|
||||
_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];
|
||||
_gyro_main = 0;
|
||||
_gyro_valid = true;
|
||||
static unsigned dtoverflow5 = 0;
|
||||
static unsigned dtoverflow10 = 0;
|
||||
static hrt_abstime lastprint = 0;
|
||||
|
||||
} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
|
||||
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
|
||||
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
|
||||
if (hrt_elapsed_time(&lastprint) > 1000000) {
|
||||
warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
|
||||
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
|
||||
dtoverflow5, dtoverflow10);
|
||||
|
||||
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
|
||||
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
|
||||
_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
|
||||
_gyro_main = 1;
|
||||
_gyro_valid = true;
|
||||
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
|
||||
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
|
||||
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
|
||||
|
||||
} else {
|
||||
_gyro_valid = false;
|
||||
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
|
||||
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
|
||||
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
|
||||
|
||||
lastprint = hrt_absolute_time();
|
||||
}
|
||||
|
||||
if (last_gyro_main != _gyro_main) {
|
||||
mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
|
||||
if (deltaT > 0.005f) {
|
||||
dtoverflow5++;
|
||||
}
|
||||
|
||||
if (!_gyro_valid) {
|
||||
/* keep last value if he hit an out of band value */
|
||||
lastAngRate = _ekf->angRate;
|
||||
|
||||
} else {
|
||||
perf_count(_perf_gyro);
|
||||
if (deltaT > 0.01f) {
|
||||
dtoverflow10++;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (accel_updated) {
|
||||
|
||||
int last_accel_main = _accel_main;
|
||||
|
||||
/* fail over to the 2nd accel if we know the first is down */
|
||||
if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
|
||||
_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];
|
||||
_accel_main = 0;
|
||||
|
||||
} else {
|
||||
_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
|
||||
_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
|
||||
_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
|
||||
_accel_main = 1;
|
||||
}
|
||||
|
||||
if (!_accel_valid) {
|
||||
lastAccel = _ekf->accel;
|
||||
}
|
||||
|
||||
if (last_accel_main != _accel_main) {
|
||||
mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
|
||||
}
|
||||
|
||||
_accel_valid = true;
|
||||
}
|
||||
|
||||
_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) {
|
||||
_newDataMag = true;
|
||||
|
||||
} else {
|
||||
_newDataMag = false;
|
||||
}
|
||||
|
||||
last_mag = _sensor_combined.magnetometer_timestamp;
|
||||
_data_good = true;
|
||||
|
||||
//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
|
||||
|
||||
|
|
@ -1362,7 +1382,6 @@ void AttitudePositionEstimatorEKF::pollData()
|
|||
_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
|
||||
}
|
||||
|
||||
|
||||
bool gps_update;
|
||||
orb_check(_gps_sub, &gps_update);
|
||||
|
||||
|
|
@ -1516,57 +1535,6 @@ void AttitudePositionEstimatorEKF::pollData()
|
|||
perf_count(_perf_baro);
|
||||
}
|
||||
|
||||
//Update Magnetometer
|
||||
if (_newDataMag) {
|
||||
|
||||
_mag_valid = true;
|
||||
|
||||
perf_count(_perf_mag);
|
||||
|
||||
int last_mag_main = _mag_main;
|
||||
|
||||
Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
|
||||
_sensor_combined.magnetometer_ga[2]);
|
||||
|
||||
Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
|
||||
_sensor_combined.magnetometer1_ga[2]);
|
||||
|
||||
const unsigned mag_timeout_us = 200000;
|
||||
|
||||
/* fail over to the 2nd mag if we know the first is down */
|
||||
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
|
||||
_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
|
||||
mag0.length() > 0.1f) {
|
||||
_ekf->magData.x = mag0.x;
|
||||
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
|
||||
|
||||
_ekf->magData.y = mag0.y;
|
||||
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
|
||||
|
||||
_ekf->magData.z = mag0.z;
|
||||
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
|
||||
_mag_main = 0;
|
||||
|
||||
} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
|
||||
mag1.length() > 0.1f) {
|
||||
_ekf->magData.x = mag1.x;
|
||||
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
|
||||
|
||||
_ekf->magData.y = mag1.y;
|
||||
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
|
||||
|
||||
_ekf->magData.z = mag1.z;
|
||||
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
|
||||
_mag_main = 1;
|
||||
} else {
|
||||
_mag_valid = false;
|
||||
}
|
||||
|
||||
if (last_mag_main != _mag_main) {
|
||||
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
|
||||
}
|
||||
}
|
||||
|
||||
//Update range data
|
||||
orb_check(_distance_sub, &_newRangeData);
|
||||
|
||||
|
|
|
|||
|
|
@ -258,6 +258,42 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
|
||||
|
||||
/**
|
||||
* Magnetometer X bias
|
||||
*
|
||||
* The magnetometer bias. This bias is learnt by the filter over time and
|
||||
* persists between boots.
|
||||
*
|
||||
* @min -0.6
|
||||
* @max 0.6
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
|
||||
|
||||
/**
|
||||
* Magnetometer Y bias
|
||||
*
|
||||
* The magnetometer bias. This bias is learnt by the filter over time and
|
||||
* persists between boots.
|
||||
*
|
||||
* @min -0.6
|
||||
* @max 0.6
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
|
||||
|
||||
/**
|
||||
* Magnetometer Z bias
|
||||
*
|
||||
* The magnetometer bias. This bias is learnt by the filter over time and
|
||||
* persists between boots.
|
||||
*
|
||||
* @min -0.6
|
||||
* @max 0.6
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
|
||||
|
||||
/**
|
||||
* Threshold for filter initialization.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -643,28 +643,28 @@ protected:
|
|||
if (_sensor_sub->update(&_sensor_time, &sensor)) {
|
||||
uint16_t fields_updated = 0;
|
||||
|
||||
if (_accel_timestamp != sensor.accelerometer_timestamp) {
|
||||
if (_accel_timestamp != sensor.accelerometer_timestamp[0]) {
|
||||
/* mark first three dimensions as changed */
|
||||
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
|
||||
_accel_timestamp = sensor.accelerometer_timestamp;
|
||||
_accel_timestamp = sensor.accelerometer_timestamp[0];
|
||||
}
|
||||
|
||||
if (_gyro_timestamp != sensor.timestamp) {
|
||||
if (_gyro_timestamp != sensor.gyro_timestamp[0]) {
|
||||
/* mark second group dimensions as changed */
|
||||
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
|
||||
_gyro_timestamp = sensor.timestamp;
|
||||
_gyro_timestamp = sensor.gyro_timestamp[0];
|
||||
}
|
||||
|
||||
if (_mag_timestamp != sensor.magnetometer_timestamp) {
|
||||
if (_mag_timestamp != sensor.magnetometer_timestamp[0]) {
|
||||
/* mark third group dimensions as changed */
|
||||
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
|
||||
_mag_timestamp = sensor.magnetometer_timestamp;
|
||||
_mag_timestamp = sensor.magnetometer_timestamp[0];
|
||||
}
|
||||
|
||||
if (_baro_timestamp != sensor.baro_timestamp) {
|
||||
if (_baro_timestamp != sensor.baro_timestamp[0]) {
|
||||
/* mark last group dimensions as changed */
|
||||
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
|
||||
_baro_timestamp = sensor.baro_timestamp;
|
||||
_baro_timestamp = sensor.baro_timestamp[0];
|
||||
}
|
||||
|
||||
mavlink_highres_imu_t msg;
|
||||
|
|
@ -679,10 +679,10 @@ protected:
|
|||
msg.xmag = sensor.magnetometer_ga[0];
|
||||
msg.ymag = sensor.magnetometer_ga[1];
|
||||
msg.zmag = sensor.magnetometer_ga[2];
|
||||
msg.abs_pressure = sensor.baro_pres_mbar;
|
||||
msg.diff_pressure = sensor.differential_pressure_pa;
|
||||
msg.pressure_alt = sensor.baro_alt_meter;
|
||||
msg.temperature = sensor.baro_temp_celcius;
|
||||
msg.abs_pressure = sensor.baro_pres_mbar[0];
|
||||
msg.diff_pressure = sensor.differential_pressure_pa[0];
|
||||
msg.pressure_alt = sensor.baro_alt_meter[0];
|
||||
msg.temperature = sensor.baro_temp_celcius[0];
|
||||
msg.fields_updated = fields_updated;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
|
||||
|
|
|
|||
|
|
@ -1410,6 +1410,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
hil_sensors.gyro_rad_s[0] = imu.xgyro;
|
||||
hil_sensors.gyro_rad_s[1] = imu.ygyro;
|
||||
hil_sensors.gyro_rad_s[2] = imu.zgyro;
|
||||
hil_sensors.gyro_timestamp[0] = timestamp;
|
||||
|
||||
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
|
||||
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
|
||||
|
|
@ -1417,9 +1418,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
|
||||
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
|
||||
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
|
||||
hil_sensors.accelerometer_mode = 0; // TODO what is this?
|
||||
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
|
||||
hil_sensors.accelerometer_timestamp = timestamp;
|
||||
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
|
||||
hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
|
||||
hil_sensors.accelerometer_timestamp[0] = timestamp;
|
||||
|
||||
hil_sensors.adc_voltage_v[0] = 0.0f;
|
||||
hil_sensors.adc_voltage_v[1] = 0.0f;
|
||||
|
|
@ -1431,19 +1432,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
hil_sensors.magnetometer_ga[0] = imu.xmag;
|
||||
hil_sensors.magnetometer_ga[1] = imu.ymag;
|
||||
hil_sensors.magnetometer_ga[2] = imu.zmag;
|
||||
hil_sensors.magnetometer_range_ga = 32.7f; // int16
|
||||
hil_sensors.magnetometer_mode = 0; // TODO what is this
|
||||
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
|
||||
hil_sensors.magnetometer_timestamp = timestamp;
|
||||
hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16
|
||||
hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
|
||||
hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
|
||||
hil_sensors.magnetometer_timestamp[0] = timestamp;
|
||||
|
||||
hil_sensors.baro_pres_mbar = imu.abs_pressure;
|
||||
hil_sensors.baro_alt_meter = imu.pressure_alt;
|
||||
hil_sensors.baro_temp_celcius = imu.temperature;
|
||||
hil_sensors.baro_timestamp = timestamp;
|
||||
hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
|
||||
hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
|
||||
hil_sensors.baro_temp_celcius[0] = imu.temperature;
|
||||
hil_sensors.baro_timestamp[0] = timestamp;
|
||||
|
||||
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
|
||||
hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa;
|
||||
hil_sensors.differential_pressure_timestamp = timestamp;
|
||||
hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa
|
||||
hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0];
|
||||
hil_sensors.differential_pressure_timestamp[0] = timestamp;
|
||||
|
||||
/* publish combined sensor topic */
|
||||
if (_sensors_pub == nullptr) {
|
||||
|
|
|
|||
|
|
@ -406,7 +406,7 @@ Navigator::task_main()
|
|||
/* Check geofence violation */
|
||||
static hrt_abstime last_geofence_check = 0;
|
||||
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
|
||||
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set);
|
||||
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
|
||||
last_geofence_check = hrt_absolute_time();
|
||||
have_geofence_position_data = false;
|
||||
if (!inside) {
|
||||
|
|
|
|||
|
|
@ -399,13 +399,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
if (fds_init[0].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
|
||||
if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
|
||||
baro_timestamp = sensor.baro_timestamp;
|
||||
if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
|
||||
baro_timestamp = sensor.baro_timestamp[0];
|
||||
|
||||
/* mean calculation over several measurements */
|
||||
if (baro_init_cnt < baro_init_num) {
|
||||
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
|
||||
baro_offset += sensor.baro_alt_meter;
|
||||
if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
|
||||
baro_offset += sensor.baro_alt_meter[0];
|
||||
baro_init_cnt++;
|
||||
}
|
||||
|
||||
|
|
@ -474,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
if (updated) {
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
|
||||
if (sensor.accelerometer_timestamp != accel_timestamp) {
|
||||
if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
|
||||
if (att.R_valid) {
|
||||
/* correct accel bias */
|
||||
sensor.accelerometer_m_s2[0] -= acc_bias[0];
|
||||
|
|
@ -496,13 +496,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
memset(acc, 0, sizeof(acc));
|
||||
}
|
||||
|
||||
accel_timestamp = sensor.accelerometer_timestamp;
|
||||
accel_timestamp = sensor.accelerometer_timestamp[0];
|
||||
accel_updates++;
|
||||
}
|
||||
|
||||
if (sensor.baro_timestamp != baro_timestamp) {
|
||||
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
|
||||
baro_timestamp = sensor.baro_timestamp;
|
||||
if (sensor.baro_timestamp[0] != baro_timestamp) {
|
||||
corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
|
||||
baro_timestamp = sensor.baro_timestamp[0];
|
||||
baro_updates++;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200
|
|||
MAXOPTIMIZATION = -Os
|
||||
|
||||
ifeq ($(PX4_TARGET_OS),nuttx)
|
||||
EXTRACFLAGS = -Wframe-larger-than=1400
|
||||
EXTRACFLAGS = -Wframe-larger-than=1600
|
||||
endif
|
||||
|
|
|
|||
|
|
@ -1287,18 +1287,11 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
pthread_cond_init(&logbuffer_cond, NULL);
|
||||
|
||||
/* track changes in sensor_combined topic */
|
||||
hrt_abstime gyro_timestamp = 0;
|
||||
hrt_abstime accelerometer_timestamp = 0;
|
||||
hrt_abstime magnetometer_timestamp = 0;
|
||||
hrt_abstime barometer_timestamp = 0;
|
||||
hrt_abstime differential_pressure_timestamp = 0;
|
||||
hrt_abstime barometer1_timestamp = 0;
|
||||
hrt_abstime gyro1_timestamp = 0;
|
||||
hrt_abstime accelerometer1_timestamp = 0;
|
||||
hrt_abstime magnetometer1_timestamp = 0;
|
||||
hrt_abstime gyro2_timestamp = 0;
|
||||
hrt_abstime accelerometer2_timestamp = 0;
|
||||
hrt_abstime magnetometer2_timestamp = 0;
|
||||
hrt_abstime gyro_timestamp[3] = {0, 0, 0};
|
||||
hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
|
||||
hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
|
||||
hrt_abstime barometer_timestamp[3] = {0, 0, 0};
|
||||
hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0};
|
||||
|
||||
/* initialize calculated mean SNR */
|
||||
float snr_mean = 0.0f;
|
||||
|
|
@ -1446,144 +1439,86 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
|
||||
/* --- SENSOR COMBINED --- */
|
||||
if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
|
||||
bool write_IMU = false;
|
||||
bool write_IMU1 = false;
|
||||
bool write_IMU2 = false;
|
||||
bool write_SENS = false;
|
||||
bool write_SENS1 = false;
|
||||
|
||||
|
||||
if (buf.sensor.timestamp != gyro_timestamp) {
|
||||
gyro_timestamp = buf.sensor.timestamp;
|
||||
write_IMU = true;
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
bool write_IMU = false;
|
||||
bool write_SENS = false;
|
||||
|
||||
if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) {
|
||||
gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
|
||||
accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
|
||||
magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
|
||||
barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
|
||||
write_SENS = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) {
|
||||
differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i];
|
||||
write_SENS = true;
|
||||
}
|
||||
|
||||
if (write_IMU) {
|
||||
switch (i) {
|
||||
case 0:
|
||||
log_msg.msg_type = LOG_IMU_MSG;
|
||||
break;
|
||||
case 1:
|
||||
log_msg.msg_type = LOG_IMU1_MSG;
|
||||
break;
|
||||
case 2:
|
||||
log_msg.msg_type = LOG_IMU2_MSG;
|
||||
break;
|
||||
}
|
||||
|
||||
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0];
|
||||
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1];
|
||||
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2];
|
||||
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0];
|
||||
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1];
|
||||
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2];
|
||||
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
|
||||
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
|
||||
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
|
||||
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0];
|
||||
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0];
|
||||
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0];
|
||||
LOGBUFFER_WRITE_AND_COUNT(IMU);
|
||||
}
|
||||
|
||||
if (write_SENS) {
|
||||
switch (i) {
|
||||
case 0:
|
||||
log_msg.msg_type = LOG_SENS_MSG;
|
||||
break;
|
||||
case 1:
|
||||
log_msg.msg_type = LOG_AIR1_MSG;
|
||||
break;
|
||||
case 2:
|
||||
continue;
|
||||
break;
|
||||
}
|
||||
|
||||
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
|
||||
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
|
||||
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
|
||||
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i];
|
||||
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i];
|
||||
LOGBUFFER_WRITE_AND_COUNT(SENS);
|
||||
}
|
||||
}
|
||||
|
||||
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
|
||||
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
|
||||
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.baro_timestamp != barometer_timestamp) {
|
||||
barometer_timestamp = buf.sensor.baro_timestamp;
|
||||
write_SENS = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
|
||||
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
|
||||
write_SENS = true;
|
||||
}
|
||||
|
||||
if (write_IMU) {
|
||||
log_msg.msg_type = LOG_IMU_MSG;
|
||||
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
|
||||
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
|
||||
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
|
||||
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
|
||||
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
|
||||
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
|
||||
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
|
||||
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
|
||||
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
|
||||
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp;
|
||||
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp;
|
||||
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp;
|
||||
LOGBUFFER_WRITE_AND_COUNT(IMU);
|
||||
}
|
||||
|
||||
if (write_SENS) {
|
||||
log_msg.msg_type = LOG_SENS_MSG;
|
||||
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
|
||||
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
|
||||
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
|
||||
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
|
||||
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
|
||||
LOGBUFFER_WRITE_AND_COUNT(SENS);
|
||||
}
|
||||
|
||||
if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
|
||||
barometer1_timestamp = buf.sensor.baro1_timestamp;
|
||||
write_SENS1 = true;
|
||||
}
|
||||
|
||||
if (write_SENS1) {
|
||||
log_msg.msg_type = LOG_AIR1_MSG;
|
||||
log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
|
||||
log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
|
||||
log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
|
||||
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
|
||||
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
|
||||
// XXX moving to AIR0-AIR2 instead of SENS
|
||||
LOGBUFFER_WRITE_AND_COUNT(SENS);
|
||||
}
|
||||
|
||||
if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
|
||||
accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
|
||||
write_IMU1 = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
|
||||
gyro1_timestamp = buf.sensor.gyro1_timestamp;
|
||||
write_IMU1 = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
|
||||
magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
|
||||
write_IMU1 = true;
|
||||
}
|
||||
|
||||
if (write_IMU1) {
|
||||
log_msg.msg_type = LOG_IMU1_MSG;
|
||||
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
|
||||
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
|
||||
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
|
||||
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
|
||||
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
|
||||
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
|
||||
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
|
||||
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
|
||||
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
|
||||
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp;
|
||||
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp;
|
||||
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp;
|
||||
LOGBUFFER_WRITE_AND_COUNT(IMU);
|
||||
}
|
||||
|
||||
if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
|
||||
accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
|
||||
write_IMU2 = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
|
||||
gyro2_timestamp = buf.sensor.gyro2_timestamp;
|
||||
write_IMU2 = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
|
||||
magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
|
||||
write_IMU2 = true;
|
||||
}
|
||||
|
||||
if (write_IMU2) {
|
||||
log_msg.msg_type = LOG_IMU2_MSG;
|
||||
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
|
||||
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
|
||||
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
|
||||
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
|
||||
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
|
||||
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
|
||||
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
|
||||
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
|
||||
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
|
||||
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp;
|
||||
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp;
|
||||
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp;
|
||||
LOGBUFFER_WRITE_AND_COUNT(IMU);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* --- ATTITUDE --- */
|
||||
|
|
|
|||
|
|
@ -129,6 +129,8 @@
|
|||
#define STICK_ON_OFF_LIMIT 0.75f
|
||||
#define MAG_ROT_VAL_INTERNAL -1
|
||||
|
||||
#define SENSOR_COUNT_MAX 3
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
|
|
@ -200,25 +202,23 @@ private:
|
|||
|
||||
bool _hil_enabled; /**< if true, HIL is active */
|
||||
bool _publishing; /**< if true, we are publishing sensor data */
|
||||
bool _armed; /**< arming status of the vehicle */
|
||||
|
||||
int _gyro_sub; /**< raw gyro0 data subscription */
|
||||
int _accel_sub; /**< raw accel0 data subscription */
|
||||
int _mag_sub; /**< raw mag0 data subscription */
|
||||
int _gyro1_sub; /**< raw gyro1 data subscription */
|
||||
int _accel1_sub; /**< raw accel1 data subscription */
|
||||
int _mag1_sub; /**< raw mag1 data subscription */
|
||||
int _gyro2_sub; /**< raw gyro2 data subscription */
|
||||
int _accel2_sub; /**< raw accel2 data subscription */
|
||||
int _mag2_sub; /**< raw mag2 data subscription */
|
||||
int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */
|
||||
int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */
|
||||
int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */
|
||||
int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */
|
||||
unsigned _gyro_count; /**< raw gyro data count */
|
||||
unsigned _accel_count; /**< raw accel data count */
|
||||
unsigned _mag_count; /**< raw mag data count */
|
||||
unsigned _baro_count; /**< raw baro data count */
|
||||
|
||||
int _rc_sub; /**< raw rc channels data subscription */
|
||||
int _baro_sub; /**< raw baro0 data subscription */
|
||||
int _baro1_sub; /**< raw baro1 data subscription */
|
||||
//int _airspeed_sub; /**< airspeed subscription */
|
||||
int _diff_pres_sub; /**< raw differential pressure subscription */
|
||||
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
|
||||
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _rc_parameter_map_sub; /**< rc parameter map subscription */
|
||||
int _manual_control_sub; /**< notification of manual control updates */
|
||||
int _rc_parameter_map_sub; /**< rc parameter map subscription */
|
||||
int _manual_control_sub; /**< notification of manual control updates */
|
||||
|
||||
orb_advert_t _sensor_pub; /**< combined sensor data topic */
|
||||
orb_advert_t _manual_control_pub; /**< manual control signal topic */
|
||||
|
|
@ -228,18 +228,6 @@ private:
|
|||
orb_advert_t _airspeed_pub; /**< airspeed */
|
||||
orb_advert_t _diff_pres_pub; /**< differential_pressure */
|
||||
|
||||
int32_t _gyro_prio; /**< gyro0 sensor priority */
|
||||
int32_t _accel_prio; /**< accel0 sensor priority */
|
||||
int32_t _mag_prio; /**< mag0 sensor priority */
|
||||
int32_t _gyro1_prio; /**< gyro1 sensor priority */
|
||||
int32_t _accel1_prio; /**<accel1 sensor priority */
|
||||
int32_t _mag1_prio; /**< mag1 sensor priority */
|
||||
int32_t _gyro2_prio; /**< gyro2 sensor priority */
|
||||
int32_t _accel2_prio; /**< accel2 sensor priority */
|
||||
int32_t _mag2_prio; /**< mag2 sensor priority */
|
||||
int32_t _baro_prio; /**< baro0 sensor priority */
|
||||
int32_t _baro1_prio; /**< baro1 sensor priority */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
struct rc_channels_s _rc; /**< r/c channel data */
|
||||
|
|
@ -251,7 +239,7 @@ private:
|
|||
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
|
||||
|
||||
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
|
||||
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
|
||||
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
|
||||
|
||||
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
|
||||
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
|
||||
|
|
@ -377,6 +365,9 @@ private:
|
|||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
int init_sensor_class(const struct orb_metadata *meta, int *subs,
|
||||
unsigned *priorities, unsigned *errcount);
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
*/
|
||||
|
|
@ -495,20 +486,18 @@ Sensors::Sensors() :
|
|||
_sensors_task(-1),
|
||||
_hil_enabled(false),
|
||||
_publishing(true),
|
||||
_armed(false),
|
||||
|
||||
/* subscriptions */
|
||||
_gyro_sub(-1),
|
||||
_accel_sub(-1),
|
||||
_mag_sub(-1),
|
||||
_gyro1_sub(-1),
|
||||
_accel1_sub(-1),
|
||||
_mag1_sub(-1),
|
||||
_gyro2_sub(-1),
|
||||
_accel2_sub(-1),
|
||||
_mag2_sub(-1),
|
||||
_gyro_sub{-1, -1, -1},
|
||||
_accel_sub{-1, -1, -1},
|
||||
_mag_sub{-1, -1, -1},
|
||||
_baro_sub{-1, -1, -1},
|
||||
_gyro_count(0),
|
||||
_accel_count(0),
|
||||
_mag_count(0),
|
||||
_baro_count(0),
|
||||
_rc_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_baro1_sub(-1),
|
||||
_vcontrol_mode_sub(-1),
|
||||
_params_sub(-1),
|
||||
_rc_parameter_map_sub(-1),
|
||||
|
|
@ -523,19 +512,6 @@ Sensors::Sensors() :
|
|||
_airspeed_pub(nullptr),
|
||||
_diff_pres_pub(nullptr),
|
||||
|
||||
/* sensor priorities */
|
||||
_gyro_prio(-1),
|
||||
_accel_prio(-1),
|
||||
_mag_prio(-1),
|
||||
_gyro1_prio(-1),
|
||||
_accel1_prio(-1),
|
||||
_mag1_prio(-1),
|
||||
_gyro2_prio(-1),
|
||||
_accel2_prio(-1),
|
||||
_mag2_prio(-1),
|
||||
_baro_prio(-1),
|
||||
_baro1_prio(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||
|
||||
|
|
@ -546,6 +522,14 @@ Sensors::Sensors() :
|
|||
_battery_discharged(0),
|
||||
_battery_current_timestamp(0)
|
||||
{
|
||||
/* initialize subscriptions */
|
||||
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
|
||||
_gyro_sub[i] = -1;
|
||||
_accel_sub[i] = -1;
|
||||
_mag_sub[i] = -1;
|
||||
_baro_sub[i] = -1;
|
||||
}
|
||||
|
||||
memset(&_rc, 0, sizeof(_rc));
|
||||
memset(&_diff_pres, 0, sizeof(_diff_pres));
|
||||
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
|
||||
|
|
@ -1032,269 +1016,132 @@ Sensors::adc_init()
|
|||
void
|
||||
Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool accel_updated;
|
||||
orb_check(_accel_sub, &accel_updated);
|
||||
for (unsigned i = 0; i < _accel_count; i++) {
|
||||
bool accel_updated;
|
||||
orb_check(_accel_sub[i], &accel_updated);
|
||||
|
||||
if (accel_updated) {
|
||||
struct accel_report accel_report;
|
||||
if (accel_updated) {
|
||||
struct accel_report accel_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
|
||||
|
||||
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.accelerometer_m_s2[0] = vect(0);
|
||||
raw.accelerometer_m_s2[1] = vect(1);
|
||||
raw.accelerometer_m_s2[2] = vect(2);
|
||||
raw.accelerometer_m_s2[i * 3 + 0] = vect(0);
|
||||
raw.accelerometer_m_s2[i * 3 + 1] = vect(1);
|
||||
raw.accelerometer_m_s2[i * 3 + 2] = vect(2);
|
||||
|
||||
raw.accelerometer_raw[0] = accel_report.x_raw;
|
||||
raw.accelerometer_raw[1] = accel_report.y_raw;
|
||||
raw.accelerometer_raw[2] = accel_report.z_raw;
|
||||
math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
|
||||
vect_int = _board_rotation * vect_int;
|
||||
|
||||
raw.accelerometer_timestamp = accel_report.timestamp;
|
||||
raw.accelerometer_priority = _accel_prio;
|
||||
raw.accelerometer_errcount = accel_report.error_count;
|
||||
raw.accelerometer_temp = accel_report.temperature;
|
||||
}
|
||||
raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
|
||||
raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
|
||||
raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
|
||||
|
||||
orb_check(_accel1_sub, &accel_updated);
|
||||
raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
|
||||
|
||||
if (accel_updated) {
|
||||
struct accel_report accel_report;
|
||||
raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw;
|
||||
raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw;
|
||||
raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw;
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report);
|
||||
|
||||
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.accelerometer1_m_s2[0] = vect(0);
|
||||
raw.accelerometer1_m_s2[1] = vect(1);
|
||||
raw.accelerometer1_m_s2[2] = vect(2);
|
||||
|
||||
raw.accelerometer1_raw[0] = accel_report.x_raw;
|
||||
raw.accelerometer1_raw[1] = accel_report.y_raw;
|
||||
raw.accelerometer1_raw[2] = accel_report.z_raw;
|
||||
|
||||
raw.accelerometer1_timestamp = accel_report.timestamp;
|
||||
raw.accelerometer1_priority = _accel1_prio;
|
||||
raw.accelerometer1_errcount = accel_report.error_count;
|
||||
raw.accelerometer1_temp = accel_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_accel2_sub, &accel_updated);
|
||||
|
||||
if (accel_updated) {
|
||||
struct accel_report accel_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
|
||||
|
||||
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.accelerometer2_m_s2[0] = vect(0);
|
||||
raw.accelerometer2_m_s2[1] = vect(1);
|
||||
raw.accelerometer2_m_s2[2] = vect(2);
|
||||
|
||||
raw.accelerometer2_raw[0] = accel_report.x_raw;
|
||||
raw.accelerometer2_raw[1] = accel_report.y_raw;
|
||||
raw.accelerometer2_raw[2] = accel_report.z_raw;
|
||||
|
||||
raw.accelerometer2_timestamp = accel_report.timestamp;
|
||||
raw.accelerometer2_priority = _accel2_prio;
|
||||
raw.accelerometer2_errcount = accel_report.error_count;
|
||||
raw.accelerometer2_temp = accel_report.temperature;
|
||||
raw.accelerometer_timestamp[i] = accel_report.timestamp;
|
||||
raw.accelerometer_errcount[i] = accel_report.error_count;
|
||||
raw.accelerometer_temp[i] = accel_report.temperature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool gyro_updated;
|
||||
orb_check(_gyro_sub, &gyro_updated);
|
||||
for (unsigned i = 0; i < _gyro_count; i++) {
|
||||
bool gyro_updated;
|
||||
orb_check(_gyro_sub[i], &gyro_updated);
|
||||
|
||||
if (gyro_updated) {
|
||||
struct gyro_report gyro_report;
|
||||
if (gyro_updated) {
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
|
||||
|
||||
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.gyro_rad_s[0] = vect(0);
|
||||
raw.gyro_rad_s[1] = vect(1);
|
||||
raw.gyro_rad_s[2] = vect(2);
|
||||
raw.gyro_rad_s[i * 3 + 0] = vect(0);
|
||||
raw.gyro_rad_s[i * 3 + 1] = vect(1);
|
||||
raw.gyro_rad_s[i * 3 + 2] = vect(2);
|
||||
|
||||
raw.gyro_raw[0] = gyro_report.x_raw;
|
||||
raw.gyro_raw[1] = gyro_report.y_raw;
|
||||
raw.gyro_raw[2] = gyro_report.z_raw;
|
||||
math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
|
||||
vect_int = _board_rotation * vect_int;
|
||||
|
||||
raw.timestamp = gyro_report.timestamp;
|
||||
raw.gyro_priority = _gyro_prio;
|
||||
raw.gyro_errcount = gyro_report.error_count;
|
||||
raw.gyro_temp = gyro_report.temperature;
|
||||
}
|
||||
raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
|
||||
raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
|
||||
raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
|
||||
|
||||
orb_check(_gyro1_sub, &gyro_updated);
|
||||
raw.gyro_integral_dt[i] = gyro_report.integral_dt;
|
||||
|
||||
if (gyro_updated) {
|
||||
struct gyro_report gyro_report;
|
||||
raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw;
|
||||
raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw;
|
||||
raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
|
||||
|
||||
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.gyro1_rad_s[0] = vect(0);
|
||||
raw.gyro1_rad_s[1] = vect(1);
|
||||
raw.gyro1_rad_s[2] = vect(2);
|
||||
|
||||
raw.gyro1_raw[0] = gyro_report.x_raw;
|
||||
raw.gyro1_raw[1] = gyro_report.y_raw;
|
||||
raw.gyro1_raw[2] = gyro_report.z_raw;
|
||||
|
||||
raw.gyro1_timestamp = gyro_report.timestamp;
|
||||
raw.gyro1_priority = _gyro1_prio;
|
||||
raw.gyro1_errcount = gyro_report.error_count;
|
||||
raw.gyro1_temp = gyro_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_gyro2_sub, &gyro_updated);
|
||||
|
||||
if (gyro_updated) {
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report);
|
||||
|
||||
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.gyro2_rad_s[0] = vect(0);
|
||||
raw.gyro2_rad_s[1] = vect(1);
|
||||
raw.gyro2_rad_s[2] = vect(2);
|
||||
|
||||
raw.gyro2_raw[0] = gyro_report.x_raw;
|
||||
raw.gyro2_raw[1] = gyro_report.y_raw;
|
||||
raw.gyro2_raw[2] = gyro_report.z_raw;
|
||||
|
||||
raw.gyro2_timestamp = gyro_report.timestamp;
|
||||
raw.gyro2_priority = _gyro2_prio;
|
||||
raw.gyro2_errcount = gyro_report.error_count;
|
||||
raw.gyro2_temp = gyro_report.temperature;
|
||||
raw.gyro_timestamp[i] = gyro_report.timestamp;
|
||||
if (i == 0) {
|
||||
raw.timestamp = gyro_report.timestamp;
|
||||
}
|
||||
raw.gyro_errcount[i] = gyro_report.error_count;
|
||||
raw.gyro_temp[i] = gyro_report.temperature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool mag_updated;
|
||||
orb_check(_mag_sub, &mag_updated);
|
||||
for (unsigned i = 0; i < _mag_count; i++) {
|
||||
bool mag_updated;
|
||||
orb_check(_mag_sub[i], &mag_updated);
|
||||
|
||||
if (mag_updated) {
|
||||
struct mag_report mag_report;
|
||||
if (mag_updated) {
|
||||
struct mag_report mag_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
|
||||
|
||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||
|
||||
vect = _mag_rotation[0] * vect;
|
||||
vect = _mag_rotation[i] * vect;
|
||||
|
||||
raw.magnetometer_ga[0] = vect(0);
|
||||
raw.magnetometer_ga[1] = vect(1);
|
||||
raw.magnetometer_ga[2] = vect(2);
|
||||
raw.magnetometer_ga[i * 3 + 0] = vect(0);
|
||||
raw.magnetometer_ga[i * 3 + 1] = vect(1);
|
||||
raw.magnetometer_ga[i * 3 + 2] = vect(2);
|
||||
|
||||
raw.magnetometer_raw[0] = mag_report.x_raw;
|
||||
raw.magnetometer_raw[1] = mag_report.y_raw;
|
||||
raw.magnetometer_raw[2] = mag_report.z_raw;
|
||||
raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw;
|
||||
raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw;
|
||||
raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw;
|
||||
|
||||
raw.magnetometer_timestamp = mag_report.timestamp;
|
||||
raw.magnetometer_priority = _mag_prio;
|
||||
raw.magnetometer_errcount = mag_report.error_count;
|
||||
raw.magnetometer_temp = mag_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_mag1_sub, &mag_updated);
|
||||
|
||||
if (mag_updated) {
|
||||
struct mag_report mag_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
|
||||
|
||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||
|
||||
vect = _mag_rotation[1] * vect;
|
||||
|
||||
raw.magnetometer1_ga[0] = vect(0);
|
||||
raw.magnetometer1_ga[1] = vect(1);
|
||||
raw.magnetometer1_ga[2] = vect(2);
|
||||
|
||||
raw.magnetometer1_raw[0] = mag_report.x_raw;
|
||||
raw.magnetometer1_raw[1] = mag_report.y_raw;
|
||||
raw.magnetometer1_raw[2] = mag_report.z_raw;
|
||||
|
||||
raw.magnetometer1_timestamp = mag_report.timestamp;
|
||||
raw.magnetometer1_priority = _mag1_prio;
|
||||
raw.magnetometer1_errcount = mag_report.error_count;
|
||||
raw.magnetometer1_temp = mag_report.temperature;
|
||||
}
|
||||
|
||||
orb_check(_mag2_sub, &mag_updated);
|
||||
|
||||
if (mag_updated) {
|
||||
struct mag_report mag_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report);
|
||||
|
||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||
|
||||
vect = _mag_rotation[2] * vect;
|
||||
|
||||
raw.magnetometer2_ga[0] = vect(0);
|
||||
raw.magnetometer2_ga[1] = vect(1);
|
||||
raw.magnetometer2_ga[2] = vect(2);
|
||||
|
||||
raw.magnetometer2_raw[0] = mag_report.x_raw;
|
||||
raw.magnetometer2_raw[1] = mag_report.y_raw;
|
||||
raw.magnetometer2_raw[2] = mag_report.z_raw;
|
||||
|
||||
raw.magnetometer2_timestamp = mag_report.timestamp;
|
||||
raw.magnetometer2_priority = _mag2_prio;
|
||||
raw.magnetometer2_errcount = mag_report.error_count;
|
||||
raw.magnetometer2_temp = mag_report.temperature;
|
||||
raw.magnetometer_timestamp[i] = mag_report.timestamp;
|
||||
raw.magnetometer_errcount[i] = mag_report.error_count;
|
||||
raw.magnetometer_temp[i] = mag_report.temperature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::baro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool baro_updated;
|
||||
orb_check(_baro_sub, &baro_updated);
|
||||
for (unsigned i = 0; i < _baro_count; i++) {
|
||||
bool baro_updated;
|
||||
orb_check(_baro_sub[i], &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
if (baro_updated) {
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
|
||||
|
||||
raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
|
||||
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
|
||||
raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
|
||||
raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar
|
||||
raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
|
||||
raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro_timestamp = _barometer.timestamp;
|
||||
raw.baro_priority = _baro_prio;
|
||||
}
|
||||
|
||||
orb_check(_baro1_sub, &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
struct baro_report baro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
|
||||
|
||||
raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
|
||||
raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
|
||||
raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro1_timestamp = baro_report.timestamp;
|
||||
raw.baro1_priority = _baro1_prio;
|
||||
raw.baro_timestamp[i] = _barometer.timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1307,12 +1154,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
|||
if (updated) {
|
||||
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
|
||||
|
||||
raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
|
||||
raw.differential_pressure_timestamp = _diff_pres.timestamp;
|
||||
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
|
||||
raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa;
|
||||
raw.differential_pressure_timestamp[0] = _diff_pres.timestamp;
|
||||
raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa;
|
||||
|
||||
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
|
||||
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
|
||||
(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
|
||||
|
||||
_airspeed.timestamp = _diff_pres.timestamp;
|
||||
|
||||
|
|
@ -1320,11 +1167,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
|||
_airspeed.indicated_airspeed_m_s = math::max(0.0f,
|
||||
calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
|
||||
_airspeed.true_airspeed_m_s = math::max(0.0f,
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f,
|
||||
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
|
||||
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
|
||||
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
|
||||
|
||||
_airspeed.air_temperature_celsius = air_temperature_celsius;
|
||||
|
||||
|
|
@ -1352,16 +1199,17 @@ Sensors::vehicle_control_mode_poll()
|
|||
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
|
||||
|
||||
/* switching from non-HIL to HIL mode */
|
||||
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
|
||||
if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
|
||||
_hil_enabled = true;
|
||||
_publishing = false;
|
||||
_armed = vcontrol_mode.flag_armed;
|
||||
|
||||
/* switching from HIL to non-HIL mode */
|
||||
|
||||
} else if (!_publishing && !_hil_enabled) {
|
||||
_hil_enabled = false;
|
||||
_publishing = true;
|
||||
_armed = vcontrol_mode.flag_armed;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1391,7 +1239,7 @@ Sensors::parameter_update_poll(bool forced)
|
|||
unsigned accel_count = 0;
|
||||
|
||||
/* run through all gyro sensors */
|
||||
for (unsigned s = 0; s < 3; s++) {
|
||||
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
|
||||
|
||||
res = ERROR;
|
||||
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
|
|
@ -1405,7 +1253,7 @@ Sensors::parameter_update_poll(bool forced)
|
|||
bool config_ok = false;
|
||||
|
||||
/* run through all stored calibrations */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
|
||||
/* initially status is ok per config */
|
||||
failed = false;
|
||||
|
||||
|
|
@ -1457,7 +1305,7 @@ Sensors::parameter_update_poll(bool forced)
|
|||
}
|
||||
|
||||
/* run through all accel sensors */
|
||||
for (unsigned s = 0; s < 3; s++) {
|
||||
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
|
||||
|
||||
res = ERROR;
|
||||
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
|
||||
|
|
@ -1471,7 +1319,7 @@ Sensors::parameter_update_poll(bool forced)
|
|||
bool config_ok = false;
|
||||
|
||||
/* run through all stored calibrations */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
|
||||
/* initially status is ok per config */
|
||||
failed = false;
|
||||
|
||||
|
|
@ -1523,7 +1371,7 @@ Sensors::parameter_update_poll(bool forced)
|
|||
}
|
||||
|
||||
/* run through all mag sensors */
|
||||
for (unsigned s = 0; s < 3; s++) {
|
||||
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
|
||||
|
||||
/* set a valid default rotation (same as board).
|
||||
* if the mag is configured, this might be replaced
|
||||
|
|
@ -1544,7 +1392,7 @@ Sensors::parameter_update_poll(bool forced)
|
|||
bool config_ok = false;
|
||||
|
||||
/* run through all stored calibrations */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
|
||||
/* initially status is ok per config */
|
||||
failed = false;
|
||||
|
||||
|
|
@ -2099,6 +1947,26 @@ Sensors::task_main_trampoline(int argc, char *argv[])
|
|||
sensors::g_sensors->task_main();
|
||||
}
|
||||
|
||||
int
|
||||
Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
|
||||
unsigned *priorities, unsigned *errcount)
|
||||
{
|
||||
unsigned group_count = orb_group_count(meta);
|
||||
|
||||
if (group_count > SENSOR_COUNT_MAX) {
|
||||
group_count = SENSOR_COUNT_MAX;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < group_count; i++) {
|
||||
if (subs[i] < 0) {
|
||||
subs[i] = orb_subscribe_multi(meta, i);
|
||||
orb_priority(subs[i], (int32_t*)&priorities[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return group_count;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::task_main()
|
||||
{
|
||||
|
|
@ -2129,74 +1997,47 @@ Sensors::task_main()
|
|||
return;
|
||||
}
|
||||
|
||||
struct sensor_combined_s raw = {};
|
||||
|
||||
/* ensure no overflows can occur */
|
||||
static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
|
||||
"SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
|
||||
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
|
||||
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
|
||||
_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
|
||||
_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
|
||||
_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
|
||||
_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
|
||||
_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
|
||||
_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
|
||||
_mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2);
|
||||
|
||||
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
|
||||
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
|
||||
|
||||
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
|
||||
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
|
||||
|
||||
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
|
||||
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
|
||||
|
||||
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
|
||||
&raw.baro_priority[0], &raw.baro_errcount[0]);
|
||||
|
||||
_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
|
||||
_baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1);
|
||||
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
|
||||
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
|
||||
/*
|
||||
* get sensor priorities
|
||||
*/
|
||||
orb_priority(_gyro_sub, &_gyro_prio);
|
||||
orb_priority(_accel_sub, &_accel_prio);
|
||||
orb_priority(_mag_sub, &_mag_prio);
|
||||
orb_priority(_gyro1_sub, &_gyro1_prio);
|
||||
orb_priority(_accel1_sub, &_accel1_prio);
|
||||
orb_priority(_mag1_sub, &_mag1_prio);
|
||||
orb_priority(_gyro2_sub, &_gyro2_prio);
|
||||
orb_priority(_accel2_sub, &_accel2_prio);
|
||||
orb_priority(_mag2_sub, &_mag2_prio);
|
||||
orb_priority(_baro_sub, &_baro_prio);
|
||||
orb_priority(_baro1_sub, &_baro1_prio);
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vcontrol_mode_sub, 200);
|
||||
|
||||
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
|
||||
orb_set_interval(_gyro_sub, 4);
|
||||
|
||||
/*
|
||||
* do advertisements
|
||||
*/
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
raw.timestamp = hrt_absolute_time();
|
||||
raw.adc_voltage_v[0] = 0.0f;
|
||||
raw.adc_voltage_v[1] = 0.0f;
|
||||
raw.adc_voltage_v[2] = 0.0f;
|
||||
raw.adc_voltage_v[3] = 0.0f;
|
||||
|
||||
/* set high initial error counts to deselect gyros */
|
||||
raw.gyro_errcount = 100000;
|
||||
raw.gyro1_errcount = 100000;
|
||||
raw.gyro2_errcount = 100000;
|
||||
|
||||
/* set high initial error counts to deselect accels */
|
||||
raw.accelerometer_errcount = 100000;
|
||||
raw.accelerometer1_errcount = 100000;
|
||||
raw.accelerometer2_errcount = 100000;
|
||||
|
||||
/* set high initial error counts to deselect mags */
|
||||
raw.magnetometer_errcount = 100000;
|
||||
raw.magnetometer1_errcount = 100000;
|
||||
raw.magnetometer2_errcount = 100000;
|
||||
|
||||
memset(&_battery_status, 0, sizeof(_battery_status));
|
||||
_battery_status.voltage_v = -1.0f;
|
||||
_battery_status.voltage_filtered_v = -1.0f;
|
||||
|
|
@ -2219,8 +2060,8 @@ Sensors::task_main()
|
|||
/* wakeup source(s) */
|
||||
px4_pollfd_struct_t fds[1];
|
||||
|
||||
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
|
||||
fds[0].fd = _gyro_sub;
|
||||
/* use the gyro to pace output */
|
||||
fds[0].fd = _gyro_sub[0];
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
_task_should_exit = false;
|
||||
|
|
@ -2245,6 +2086,21 @@ Sensors::task_main()
|
|||
/* check vehicle status for changes to publication state */
|
||||
vehicle_control_mode_poll();
|
||||
|
||||
/* keep adding sensors as long as we are not armed */
|
||||
if (!_armed) {
|
||||
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
|
||||
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
|
||||
|
||||
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
|
||||
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
|
||||
|
||||
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
|
||||
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
|
||||
|
||||
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
|
||||
&raw.baro_priority[0], &raw.baro_errcount[0]);
|
||||
}
|
||||
|
||||
/* the timestamp of the raw struct is updated by the gyro_poll() method */
|
||||
/* copy most recent sensor data */
|
||||
gyro_poll(raw);
|
||||
|
|
@ -2253,13 +2109,13 @@ Sensors::task_main()
|
|||
baro_poll(raw);
|
||||
|
||||
/* work out if main gyro timed out and fail over to alternate gyro */
|
||||
if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) {
|
||||
if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) {
|
||||
|
||||
/* if the secondary failed as well, go to the tertiary */
|
||||
if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) {
|
||||
fds[0].fd = _gyro2_sub;
|
||||
if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) {
|
||||
fds[0].fd = _gyro_sub[2];
|
||||
} else {
|
||||
fds[0].fd = _gyro1_sub;
|
||||
fds[0].fd = _gyro_sub[1];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue