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

Merge branch 'ekf_params' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2014-05-15 13:14:25 +02:00
parent 97e6090700 ec409a1337
commit ba51ab2545
20 changed files with 3362 additions and 3745 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.fw_apps
+1 -1
View File
@@ -6,7 +6,7 @@
#
# Start the attitude and position estimator
#
fw_att_pos_estimator start
ekf_att_pos_estimator start
#
# Start attitude controller
ROMFS/px4fmu_common/init.d/rc.mc_apps
+1
View File
@@ -5,6 +5,7 @@
#
attitude_estimator_ekf start
#ekf_att_pos_estimator start
position_estimator_inav start
mc_att_control start
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+6
View File
@@ -35,6 +35,12 @@ then
param set MPC_TILTMAX_AIR 45.0
param set MPC_TILTMAX_LND 15.0
param set MPC_LAND_SPEED 1.0
param set PE_VELNE_NOISE 0.5
param set PE_VELNE_NOISE 0.7
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
fi
set PWM_RATE 400
makefiles/config_px4fmu-v1_default.mk
+1 -1
View File
@@ -70,7 +70,7 @@ MODULES += modules/gpio_led
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/fw_att_pos_estimator
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
#MODULES += examples/flow_position_estimator
makefiles/config_px4fmu-v2_default.mk
+1 -1
View File
@@ -79,7 +79,7 @@ MODULES += modules/gpio_led
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/fw_att_pos_estimator
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
src/drivers/gps/gps.cpp
+3 -3
View File
@@ -278,14 +278,14 @@ GPS::task_main()
_report.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)400e3f;
_report.alt = (int32_t)1200e3f;
_report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 3.0f;
_report.epv_m = 7.0f;
_report.eph_m = 0.9f;
_report.epv_m = 1.8f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
-812
View File
@@ -1,812 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file KalmanNav.cpp
*
* Kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
#include <systemlib/err.h>
#include <geo/geo.h>
// constants
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rPressAlt(this, "R_PRESS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
memset(&ref, 0, sizeof(ref));
F.zero();
G.zero();
V.zero();
HAtt.zero();
RAtt.zero();
HPos.zero();
RPos.zero();
// initial state covariance matrix
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize rotation quaternion with a single raw sensor measurement
_sensors.update();
q = init(
_sensors.accelerometer_m_s2[0],
_sensors.accelerometer_m_s2[1],
_sensors.accelerometer_m_s2[2],
_sensors.magnetometer_ga[0],
_sensors.magnetometer_ga[1],
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = q.to_dcm();
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
HPos(5, 8) = 1.0f;
// initialize all parameters
updateParams();
}
math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay, -az);
initialPitch = atan2(ax, -az);
cosRoll = cosf(initialRoll);
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
return math::Quaternion(q0, q1, q2, q3);
}
void KalmanNav::update()
{
using namespace math;
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
// poll for new data
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
} else if (ret == 0) { // timeout
return;
}
// get new timestamp
uint64_t newTimeStamp = hrt_absolute_time();
// check updated subscriptions
if (_param_update.updated()) updateParams();
bool gpsUpdate = _gps.updated();
bool sensorsUpdate = _sensors.updated();
// get new information from subscriptions
// this clears update flag
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
warnx("initialized EKF attitude");
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2
//&& _gps.counter_pos_valid > 10
) {
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// set reference position for
// local position
lat0 = lat;
lon0 = lon;
alt0 = alt;
map_projection_init(&ref, lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
double(vN), double(vE), double(vD),
lat, lon, double(alt));
}
// prediction step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
//printf("nav: %4d Hz, miss #: %4d\n",
// _navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
}
void KalmanNav::updatePublications()
{
using namespace math;
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);
_pos.vel_n = vN;
_pos.vel_e = vE;
_pos.vel_d = vD;
_pos.yaw = psi;
// local position publication
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
map_projection_project(&ref, lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
_localPos.v_xy_valid = true;
_localPos.v_z_valid = true;
_localPos.x = x;
_localPos.y = y;
_localPos.z = alt0 - alt;
_localPos.vx = vN;
_localPos.vy = vE;
_localPos.vz = vD;
_localPos.yaw = psi;
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = lat * M_RAD_TO_DEG;
_localPos.ref_lon = lon * M_RAD_TO_DEG;
_localPos.ref_alt = 0;
_localPos.landed = landed;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
_att.pitch = theta;
_att.yaw = psi;
_att.rollspeed = _sensors.gyro_rad_s[0];
_att.pitchspeed = _sensors.gyro_rad_s[1];
_att.yawspeed = _sensors.gyro_rad_s[2];
// TODO, add gyro offsets to filter
_att.rate_offsets[0] = 0.0f;
_att.rate_offsets[1] = 0.0f;
_att.rate_offsets[2] = 0.0f;
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = C_nb(i, j);
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
_att.R_valid = true;
_att.q_valid = true;
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized) {
_pos.update();
_localPos.update();
}
if (_attitudeInitialized)
_att.update();
}
int KalmanNav::predictState(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// attitude prediction
if (_attitudeInitialized) {
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = q.to_dcm();
// euler update
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
// XXX position prediction using speed
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
F(0, 1) = -(omega * sinL + vE * tanL / R);
F(0, 2) = vN / R;
F(0, 4) = 1.0f / R;
F(0, 6) = -omega * sinL;
F(0, 8) = -vE / RSq;
F(1, 0) = omega * sinL + vE * tanL / R;
F(1, 2) = omega * cosL + vE / R;
F(1, 3) = -1.0f / R;
F(1, 8) = vN / RSq;
F(2, 0) = -vN / R;
F(2, 1) = -omega * cosL - vE / R;
F(2, 4) = -tanL / R;
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
F(2, 8) = vE * tanL / RSq;
F(3, 1) = -fD;
F(3, 2) = fE;
F(3, 3) = vD / R;
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
F(3, 5) = vN / R;
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
F(4, 0) = fD;
F(4, 2) = -fN;
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
F(4, 4) = (vN * tanL + vD) / R;
F(4, 5) = 2 * omega * cosL + vE / R;
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
vN * vE / (R * cosLSq);
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
F(5, 0) = -fE;
F(5, 1) = fN;
F(5, 3) = -2 * vN / R;
F(5, 4) = -2 * (omega * cosL + vE / R);
F(5, 6) = 2 * omega * vE * sinL;
F(5, 8) = (vN * vN + vE * vE) / RSq;
F(6, 3) = 1 / R;
F(6, 8) = -vN / RSq;
F(7, 4) = 1 / (R * cosL);
F(7, 6) = vE * tanL / (R * cosL);
F(7, 8) = -vE / (cosL * RSq);
F(8, 5) = -1;
// G Matrix
// Titterton pg. 291
G(0, 0) = -C_nb(0, 0);
G(0, 1) = -C_nb(0, 1);
G(0, 2) = -C_nb(0, 2);
G(1, 0) = -C_nb(1, 0);
G(1, 1) = -C_nb(1, 1);
G(1, 2) = -C_nb(1, 2);
G(2, 0) = -C_nb(2, 0);
G(2, 1) = -C_nb(2, 1);
G(2, 2) = -C_nb(2, 2);
G(3, 3) = C_nb(0, 0);
G(3, 4) = C_nb(0, 1);
G(3, 5) = C_nb(0, 2);
G(4, 3) = C_nb(1, 0);
G(4, 4) = C_nb(1, 1);
G(4, 5) = C_nb(1, 2);
G(5, 3) = C_nb(2, 0);
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
int KalmanNav::correctAtt()
{
using namespace math;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
// float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
// float sinPsi = sinf(psi);
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
if (yMag > M_PI_F) yMag -= 2*M_PI_F;
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(1, 1) = 1.0e10;
RAttAdjust(2, 2) = 1.0e10;
RAttAdjust(3, 3) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
}
// accel predicted measurement
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// calculate residual
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
// HAccel
HAtt(1, 1) = cosTheta;
HAtt(2, 0) = -cosPhi * cosTheta;
HAtt(2, 1) = sinPhi * sinTheta;
HAtt(3, 0) = sinPhi * cosTheta;
HAtt(3, 1) = cosPhi * sinTheta;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
warnx("numerical failure in att correction");
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
if (!ignoreAccel) {
phi += xCorrect(PHI);
theta += xCorrect(THETA);
}
psi += xCorrect(PSI);
// attitude also affects nav velocities
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HAtt * P;
// fault detection
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:"); y.print();
}
// update quaternions from euler
// angle correction
q.from_euler(phi, theta, psi);
return ret_ok;
}
int KalmanNav::correctPos()
{
using namespace math;
// residual
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
y(4) = _gps.alt / 1.0e3f - alt;
y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (!isfinite(val)) {
// abort correction and return
warnx("numerical failure in gps correction");
// fallback to GPS
vN = _gps.vel_n_m_s;
vE = _gps.vel_e_m_s;
vD = _gps.vel_d_m_s;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HPos * P;
// fault detetcion
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5))));
}
counter++;
return ret_ok;
}
void KalmanNav::updateParams()
{
using namespace math;
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
V(2, 2) = _vGyro.get(); // gyro z
// accel noise
V(3, 3) = _vAccel.get(); // accel x, m/s^2
V(4, 4) = _vAccel.get(); // accel y
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(1, 1) = noiseAccelSq; // normalized direction
RAtt(2, 2) = noiseAccelSq;
RAtt(3, 3) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseGpsAlt = _rGpsAlt.get();
float noisePressAlt = _rPressAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
RPos(5, 5) = noisePressAlt * noisePressAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
-194
View File
@@ -1,194 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file KalmanNav.hpp
*
* kalman filter navigation code
*/
#pragma once
//#define MATRIX_ASSERT
//#define VECTOR_ASSERT
#include <nuttx/config.h>
#include <mathlib/mathlib.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <lib/geo/geo.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include <unistd.h>
/**
* Kalman filter navigation class
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
* Discrete-time extended Kalman filter
*/
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
// kalman filter
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
math::Matrix<9,9> P; /**< state covariance matrix */
math::Matrix<9,9> P0; /**< initial state covariance matrix */
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
// attitude
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */
uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */
uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */
uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */
uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
struct map_projection_reference_s ref; /**< local projection reference */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */
control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */
control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */
control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */
control::BlockParamFloat _magDip; /**< magnetic inclination with level */
control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParamFloat _g; /**< gravitational constant */
control::BlockParamFloat _faultPos; /**< fault detection threshold for position */
control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
};
src/modules/att_pos_estimator_ekf/kalman_main.cpp
-157
View File
@@ -1,157 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: James Goppert
*
* 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 kalman_main.cpp
* Combined attitude / position estimator.
*
* @author James Goppert
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int daemon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int kalman_demo_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int att_pos_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 30,
8192,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running\n");
exit(0);
} else {
warnx("not started\n");
exit(1);
}
}
usage("unrecognized command");
exit(1);
}
int kalman_demo_thread_main(int argc, char *argv[])
{
warnx("starting");
using namespace math;
thread_running = true;
KalmanNav nav(NULL, "KF");
while (!thread_should_exit) {
nav.update();
}
warnx("exiting.");
thread_running = false;
return 0;
}
src/modules/att_pos_estimator_ekf/module.mk
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Full attitude / position Extended Kalman Filter
#
MODULE_COMMAND = att_pos_estimator_ekf
SRCS = kalman_main.cpp \
KalmanNav.cpp \
params.c
src/modules/att_pos_estimator_ekf/params.c
-49
View File
@@ -1,49 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
src/modules/ekf_att_pos_estimator/estimator.cpp
+2609
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_att_pos_estimator/estimator.h → src/modules/ekf_att_pos_estimator/estimator.h
+111 -18
View File
@@ -20,7 +20,7 @@ public:
float z;
float length(void) const;
Vector3f zero(void) const;
void zero(void);
};
class Mat3f
@@ -33,6 +33,7 @@ public:
Mat3f();
void identity();
Mat3f transpose(void) const;
};
@@ -45,14 +46,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2);
const unsigned int n_states = 21;
const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
// extern bool staticMode;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
@@ -82,6 +78,88 @@ public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
}
struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
} magstate;
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
@@ -96,6 +174,7 @@ public:
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float statesAtRngTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -104,6 +183,10 @@ public:
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Mat3f Tbn; // transformation matrix from body to NED coordinates
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
@@ -115,26 +198,30 @@ public:
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS; // ratio f true to equivalent airspeed
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
@@ -148,11 +235,13 @@ public:
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
@@ -172,6 +261,10 @@ void FuseMagnetometer();
void FuseAirspeed();
void FuseRangeFinder();
void FuseOpticalFlow();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
@@ -192,7 +285,7 @@ void StoreStates(uint64_t timestamp_ms);
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float statesForFusion[n_states], uint64_t msec);
int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
@@ -206,7 +299,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
@@ -218,7 +311,7 @@ void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3]);
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
@@ -243,7 +336,7 @@ void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]);
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
@@ -251,7 +344,7 @@ bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp → src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+342 -95
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -90,20 +91,19 @@
*
* @ingroup apps
*/
extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
static uint64_t last_run = 0;
static uint64_t IMUmsec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis()
{
return IMUmsec;
}
static void print_status();
class FixedwingEstimator
{
public:
@@ -152,6 +152,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _global_pos_pub; /**< global position */
@@ -191,7 +192,13 @@ private:
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
bool _initialized;
bool _baro_init;
bool _gps_initialized;
uint64_t _gps_start_time;
uint64_t _filter_start_time;
bool _gyro_valid;
bool _accel_valid;
bool _mag_valid;
int _mavlink_fd;
@@ -201,6 +208,19 @@ private:
int32_t height_delay_ms;
int32_t mag_delay_ms;
int32_t tas_delay_ms;
float velne_noise;
float veld_noise;
float posne_noise;
float posd_noise;
float mag_noise;
float gyro_pnoise;
float acc_pnoise;
float gbias_pnoise;
float abias_pnoise;
float mage_pnoise;
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -209,6 +229,19 @@ private:
param_t height_delay_ms;
param_t mag_delay_ms;
param_t tas_delay_ms;
param_t velne_noise;
param_t veld_noise;
param_t posne_noise;
param_t posd_noise;
param_t mag_noise;
param_t gyro_pnoise;
param_t acc_pnoise;
param_t gbias_pnoise;
param_t abias_pnoise;
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
@@ -271,6 +304,8 @@ FixedwingEstimator::FixedwingEstimator() :
_vstatus_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_mission_sub(-1),
_home_sub(-1),
/* publications */
_att_pub(-1),
@@ -278,6 +313,25 @@ FixedwingEstimator::FixedwingEstimator() :
_local_pos_pub(-1),
_estimator_status_pub(-1),
_att({}),
_gyro({}),
_accel({}),
_mag({}),
_airspeed({}),
_baro({}),
_vstatus({}),
_global_pos({}),
_local_pos({}),
_gps({}),
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
#ifdef SENSOR_COMBINED_SUB
_sensor_combined({}),
#endif
_baro_ref(0.0f),
_baro_gps_offset(0.0f),
@@ -292,18 +346,35 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_initialized(false),
_baro_init(false),
_gps_initialized(false),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_mavlink_fd(-1),
_ekf(nullptr)
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
last_run = hrt_absolute_time();
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
_parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS");
_parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS");
_parameter_handles.velne_noise = param_find("PE_VELNE_NOISE");
_parameter_handles.veld_noise = param_find("PE_VELD_NOISE");
_parameter_handles.posne_noise = param_find("PE_POSNE_NOISE");
_parameter_handles.posd_noise = param_find("PE_POSD_NOISE");
_parameter_handles.mag_noise = param_find("PE_MAG_NOISE");
_parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE");
_parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE");
_parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE");
_parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
/* fetch initial parameter values */
parameters_update();
@@ -317,6 +388,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) {
res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
close(fd);
if (res) {
warnx("G SCALE FAIL");
}
}
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
@@ -324,6 +399,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) {
res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
close(fd);
if (res) {
warnx("A SCALE FAIL");
}
}
fd = open(MAG_DEVICE_PATH, O_RDONLY);
@@ -331,6 +410,10 @@ FixedwingEstimator::FixedwingEstimator() :
if (fd > 0) {
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
close(fd);
if (res) {
warnx("M SCALE FAIL");
}
}
}
@@ -368,6 +451,37 @@ FixedwingEstimator::parameters_update()
param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms));
param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms));
param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms));
param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise));
param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise));
param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise));
param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise));
param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise));
param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise));
param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise));
param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise));
param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
if (_ekf) {
// _ekf->yawVarScale = 1.0f;
// _ekf->windVelSigma = 0.1f;
_ekf->dAngBiasSigma = _parameters.gbias_pnoise;
_ekf->dVelBiasSigma = _parameters.abias_pnoise;
_ekf->magEarthSigma = _parameters.mage_pnoise;
_ekf->magBodySigma = _parameters.magb_pnoise;
// _ekf->gndHgtSigma = 0.02f;
_ekf->vneSigma = _parameters.velne_noise;
_ekf->vdSigma = _parameters.veld_noise;
_ekf->posNeSigma = _parameters.posne_noise;
_ekf->posDSigma = _parameters.posd_noise;
_ekf->magMeasurementSigma = _parameters.mag_noise;
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
_ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
}
return OK;
}
@@ -392,13 +506,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
estimator::g_estimator->task_main();
}
float dt = 0.0f; // time lapsed since last covariance prediction
void
FixedwingEstimator::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
float dt = 0.0f; // time lapsed since last covariance prediction
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!");
@@ -412,6 +527,7 @@ FixedwingEstimator::task_main()
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
@@ -431,26 +547,11 @@ FixedwingEstimator::task_main()
orb_set_interval(_sensor_combined_sub, 4);
#endif
/* sets also parameters in the EKF object */
parameters_update();
/* set initial filter state */
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
_ekf->fuseHgtData = false;
_ekf->fuseMagData = false;
_ekf->fuseVtasData = false;
_ekf->statesInitialised = false;
/* initialize measurement data */
_ekf->VtasMeas = 0.0f;
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
_ekf->dVelIMU.x = 0.0f;
_ekf->dVelIMU.y = 0.0f;
_ekf->dVelIMU.z = 0.0f;
_ekf->dAngIMU.x = 0.0f;
_ekf->dAngIMU.y = 0.0f;
_ekf->dAngIMU.z = 0.0f;
Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
/* wakeup source(s) */
struct pollfd fds[2];
@@ -466,9 +567,8 @@ FixedwingEstimator::task_main()
fds[1].events = POLLIN;
#endif
hrt_abstime start_time = hrt_absolute_time();
bool newDataGps = false;
bool newHgtData = false;
bool newAdsData = false;
bool newDataMag = false;
@@ -503,19 +603,52 @@ FixedwingEstimator::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
bool mag_updated;
hrt_abstime last_sensor_timestamp;
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
#else
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
#endif
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/**
* PART ONE: COLLECT ALL DATA
**/
hrt_abstime last_sensor_timestamp;
/* load local copies */
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@@ -529,27 +662,46 @@ FixedwingEstimator::task_main()
}
last_sensor_timestamp = _gyro.timestamp;
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
last_run = _gyro.timestamp;
/* guard against too large deltaT's */
if (deltaT > 1.0f)
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
if (isfinite(_gyro.x) &&
isfinite(_gyro.y) &&
isfinite(_gyro.z)) {
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
@@ -565,6 +717,8 @@ FixedwingEstimator::task_main()
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
@@ -575,23 +729,43 @@ FixedwingEstimator::task_main()
IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (deltaT > 1.0f || deltaT < 0.000001f)
if (!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;
_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];
if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
isfinite(_sensor_combined.gyro_rad_s[1]) &&
isfinite(_sensor_combined.gyro_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
_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];
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
}
if (accel_updated) {
_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];
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
@@ -635,11 +809,13 @@ FixedwingEstimator::task_main()
perf_count(_perf_gps);
if (_gps.fix_type < 3) {
gps_updated = false;
newDataGps = false;
} else {
/* store time of valid GPS measurement */
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
_ekf->ResetPosition();
@@ -660,6 +836,21 @@ FixedwingEstimator::task_main()
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
newDataGps = true;
}
@@ -672,10 +863,17 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude;
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
if (!_baro_init) {
_baro_ref = _baro.altitude;
_baro_init = true;
warnx("ALT REF INIT");
}
newHgtData = true;
} else {
newHgtData = false;
}
#ifndef SENSOR_COMBINED_SUB
@@ -684,6 +882,8 @@ FixedwingEstimator::task_main()
if (mag_updated) {
_mag_valid = true;
perf_count(_perf_mag);
#ifndef SENSOR_COMBINED_SUB
@@ -727,6 +927,8 @@ FixedwingEstimator::task_main()
*/
int check = _ekf->CheckAndBound();
const char* ekfname = "[ekf] ";
switch (check) {
case 0:
/* all ok */
@@ -735,26 +937,38 @@ FixedwingEstimator::task_main()
{
const char* str = "NaN in states, resetting";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 2:
{
const char* str = "stale IMU data, resetting";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 3:
{
const char* str = "switching dynamic / static state";
const char* str = "switching to dynamic state";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
default:
{
const char* str = "unknown reset condition";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
}
}
// If non-zero, we got a problem
// warn on fatal resets
if (check == 1) {
warnx("NUMERIC ERROR IN FILTER");
}
// If non-zero, we got a filter reset
if (check) {
struct ekf_status_report ekf_report;
@@ -770,7 +984,7 @@ FixedwingEstimator::task_main()
rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
// Copy all states or at least all that we can fit
int i = 0;
unsigned i = 0;
unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
@@ -786,6 +1000,23 @@ FixedwingEstimator::task_main()
} else {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
}
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
// Let the system re-initialize itself
continue;
}
@@ -793,52 +1024,66 @@ FixedwingEstimator::task_main()
* PART TWO: EXECUTE THE FILTER
**/
// Wait long enough to ensure all sensors updated once
// XXX we rather want to check all updated
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
float initVelNED[3];
if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
initVelNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7;
double lon = _gps.lon * 1e-7;
float alt = _gps.alt * 1e-3;
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
_ekf->InitialiseFilter(_ekf->velNED);
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_gps_offset = gps_alt - _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
_local_pos.ref_lat = _gps.lat;
_local_pos.ref_lon = _gps.lon;
_local_pos.ref_alt = alt;
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = _baro_ref + _baro_gps_offset;
_local_pos.ref_timestamp = _gps.timestamp_position;
// Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude;
_ekf->baroHgt = _baro.altitude - _baro_ref;
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
// XXX this is not multithreading safe
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
_gps_initialized = true;
} else if (!_ekf->statesInitialised) {
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
initVelNED[0] = 0.0f;
initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
}
}
@@ -872,10 +1117,10 @@ FixedwingEstimator::task_main()
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
_ekf->CovariancePrediction(dt);
_ekf->summedDelAng = _ekf->summedDelAng.zero();
_ekf->summedDelVel = _ekf->summedDelVel.zero();
_ekf->summedDelAng.zero();
_ekf->summedDelVel.zero();
dt = 0.0f;
}
@@ -926,9 +1171,9 @@ FixedwingEstimator::task_main()
_ekf->fusePosData = false;
}
if (newAdsData && _ekf->statesInitialised) {
if (newHgtData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
@@ -1017,7 +1262,7 @@ FixedwingEstimator::task_main()
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
_local_pos.z = _ekf->states[9];
_local_pos.z = _ekf->states[9] + _baro_gps_offset;
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
@@ -1050,6 +1295,8 @@ FixedwingEstimator::task_main()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec;
_global_pos.eph = _gps.eph_m;
_global_pos.epv = _gps.epv_m;
}
if (_local_pos.v_xy_valid) {
@@ -1102,7 +1349,7 @@ FixedwingEstimator::start()
ASSERT(_estimator_task == -1);
/* start the task */
_estimator_task = task_spawn_cmd("fw_att_pos_estimator",
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
6000,
@@ -1136,7 +1383,7 @@ FixedwingEstimator::print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
@@ -1180,13 +1427,13 @@ int FixedwingEstimator::trip_nan() {
_ekf->states[5] = nan_val;
usleep(100000);
// warnx("tripping covariance #1 with NaN values");
// KH[2][2] = nan_val; // intermediate result used for covariance updates
// usleep(100000);
warnx("tripping covariance #1 with NaN values");
_ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
usleep(100000);
// warnx("tripping covariance #2 with NaN values");
// KHP[5][5] = nan_val; // intermediate result used for covariance updates
// usleep(100000);
warnx("tripping covariance #2 with NaN values");
_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #3 with NaN values");
_ekf->P[3][3] = nan_val; // covariance matrix
@@ -1208,10 +1455,10 @@ int FixedwingEstimator::trip_nan() {
return ret;
}
int fw_att_pos_estimator_main(int argc, char *argv[])
int ekf_att_pos_estimator_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: fw_att_pos_estimator {start|stop|status}");
errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
if (!strcmp(argv[1], "start")) {
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
+271
View File
@@ -0,0 +1,271 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 fw_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
*
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
/**
* Airspeed measurement noise.
*
* Increasing this value will make the filter trust this sensor
* less and trust other sensors more.
*
* @min 0.5
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
/**
* Velocity measurement noise in north-east (horizontal) direction.
*
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
/**
* Position noise in north-east (horizontal) direction
*
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
/**
* Gyro process noise
*
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
* This noise controls how much the filter trusts the gyro measurements.
* Increasing it makes the filter trust the gyro less and other sensors more.
*
* @min 0.001
* @max 0.05
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
/**
* Accelerometer process noise
*
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
* Increasing this value makes the filter trust the accelerometer less
* and other sensors more.
*
* @min 0.05
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
/**
* Gyro bias estimate process noise
*
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
* Increasing this value will make the gyro bias converge faster but noisier.
*
* @min 0.0000001
* @max 0.00001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
/**
* Accelerometer bias estimate process noise
*
* Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
* Increasing this value makes the bias estimation faster and noisier.
*
* @min 0.0001
* @max 0.001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
/**
* Magnetometer earth frame offsets process noise
*
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
* Increasing this value makes the magnetometer earth bias estimate converge
* faster but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
/**
* Magnetometer body frame offsets process noise
*
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
* Increasing this value makes the magnetometer body bias estimate converge faster
* but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Threshold for filter initialization.
*
* If the standard deviation of the GPS position estimate is below this threshold
* in meters, the filter will initialize.
*
* @min 0.3
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
src/modules/fw_att_pos_estimator/module.mk → src/modules/ekf_att_pos_estimator/module.mk
+1 -1
View File
@@ -35,7 +35,7 @@
# Main Attitude and Position Estimator for Fixed Wing Aircraft
#
MODULE_COMMAND = fw_att_pos_estimator
MODULE_COMMAND = ekf_att_pos_estimator
SRCS = fw_att_pos_estimator_main.cpp \
fw_att_pos_estimator_params.c \
src/modules/fw_att_control/fw_att_control_main.cpp
+4 -4
View File
@@ -783,7 +783,7 @@ FixedwingAttitudeControl::task_main()
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
}
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -792,16 +792,16 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", yaw_u);
warnx("yaw_u %.4f", (double)yaw_u);
}
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
warnx("throttle_sp %.4f", throttle_sp);
warnx("throttle_sp %.4f", (double)throttle_sp);
}
} else {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
/*
src/modules/fw_att_pos_estimator/estimator.cpp
-2248
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c
-117
View File
@@ -1,117 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 fw_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
*
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
src/modules/mavlink/mavlink_main.cpp
+11 -2
View File
@@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
ssize_t ret = write(uart, ch, desired);
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
if (desired > buf_free) {
desired = buf_free;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
warnx("TX FAIL");
}
}