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

Merge branch 'master' into offboard2

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-16 12:12:43 +02:00
parent d2553bfd29 f892a7278a
commit 8c4b35cc23
511 changed files with 23010 additions and 11290 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
-815
View File
@@ -1,815 +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;
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;
// XXX map_projection has internal global
// states that multiple things could change,
// should make map_projection take reference
// lat/lon and not have init
map_projection_init(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.global_valid = true;
_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(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 = getLatDegE7();
_localPos.ref_lon = getLonDegE7();
_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
-192
View File
@@ -1,192 +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 <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 */
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/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+7 -6
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2012-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
@@ -34,9 +32,12 @@
****************************************************************************/
/*
* @file attitude_estimator_ekf_main.c
* @file attitude_estimator_ekf_main.cpp
*
* Extended Kalman Filter for Attitude Estimation.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -111,7 +112,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
@@ -407,7 +408,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+2
View File
@@ -40,6 +40,7 @@
*/
#include "attitude_estimator_ekf_params.h"
#include <math.h>
/* Extended Kalman Filter covariances */
@@ -113,6 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));
src/modules/attitude_estimator_ekf/module.mk
+2
View File
@@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/cross.c
MODULE_STACKSIZE = 1200
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+5 -4
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 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
@@ -36,6 +34,9 @@
/*
* @file attitude_estimator_so3_main.cpp
*
* @author Hyon Lim <limhyon@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
@@ -131,7 +132,7 @@ usage(const char *reason)
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
* to task_spawn_cmd().
*/
int attitude_estimator_so3_main(int argc, char *argv[])
{
src/modules/attitude_estimator_so3/module.mk
+2
View File
@@ -6,3 +6,5 @@ MODULE_COMMAND = attitude_estimator_so3
SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c
MODULE_STACKSIZE = 1200
src/modules/commander/accelerometer_calibration.cpp
+40 -26
View File
@@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3,3> board_rotation;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
}
if (old_done_count != done_count)
if (old_done_count != done_count) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done)
if (done) {
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
@@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f)
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i])
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
@@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 0; // [ g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 0; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 1; // [ -g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 1; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 2; // [ 0, g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 2; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 3; // [ 0, -g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 3; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
return 4; // [ 0, 0, g ]
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return 4; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
return 5; // [ 0, 0, -g ]
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return 5; // [ 0, 0, -g ]
}
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
@@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
for (int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++) {
accel_sum[i] += sensor.accelerometer_m_s2[i];
}
count++;
@@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
continue;
}
if (errcount > samples_num / 10)
if (errcount > samples_num / 10) {
return ERROR;
}
}
for (int i = 0; i < 3; i++) {
@@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f)
return ERROR; // Singular matrix
if (det == 0.0f) {
return ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK)
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
}
/* copy results to accel_T */
for (int i = 0; i < 3; i++) {
src/modules/commander/airspeed_calibration.cpp
+13 -7
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2500;
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -82,16 +82,21 @@ int do_airspeed_calibration(int mavlink_fd)
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@@ -107,11 +112,12 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
src/modules/commander/commander.cpp
+383 -323
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_helper.cpp
+20 -18
View File
@@ -113,17 +113,22 @@ void buzzer_deinit()
close(buzzer);
}
void set_tune(int tune) {
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
ioctl(buzzer, TONE_SET_ALARM, tune);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
@@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
@@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
@@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
@@ -198,16 +206,10 @@ int led_init()
return ERROR;
}
/* the blue LED is only available on FMUv1 but not FMUv2 */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (ioctl(leds, LED_ON, LED_BLUE)) {
warnx("Blue LED: ioctl fail\n");
return ERROR;
}
#endif
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE);
/* we consider the amber led mandatory */
if (ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: ioctl fail\n");
return ERROR;
@@ -217,11 +219,7 @@ int led_init()
rgbleds = open(RGBLED_DEVICE_PATH, 0);
if (rgbleds == -1) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
#else
warnx("No RGB LED found");
#endif
warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
}
return 0;
@@ -254,22 +252,25 @@ int led_off(int led)
void rgbled_set_color(rgbled_color_t color)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
}
}
void rgbled_set_mode(rgbled_mode_t mode)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
}
}
void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
@@ -309,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
src/modules/commander/commander_tests/commander_tests.cpp
+1 -2
View File
@@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
state_machine_helper_test();
//state_machine_test();
stateMachineHelperTest();
return 0;
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+273 -98
View File
@@ -49,13 +49,12 @@ public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual const char* run_tests();
virtual void runTests(void);
private:
const char* arming_state_transition_test();
const char* arming_state_transition_arm_disarm_test();
const char* main_state_transition_test();
const char* is_safe_test();
bool armingStateTransitionTest();
bool mainStateTransitionTest();
bool isSafeTest();
};
StateMachineHelperTest::StateMachineHelperTest() {
@@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() {
StateMachineHelperTest::~StateMachineHelperTest() {
}
const char*
StateMachineHelperTest::arming_state_transition_test()
bool StateMachineHelperTest::armingStateTransitionTest(void)
{
// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
// to simulate machine state prior to testing an arming state transition. This structure is also
// use to represent the expected machine state after the transition has been requested.
typedef struct {
arming_state_t arming_state; // vehicle_status_s.arming_state
bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transtion, the requested state to transition to and finally the expected
// machine state after transition.
typedef struct {
const char* assertMsg; // Text to show when test case fails
ArmingTransitionVolatileState_t current_state; // Machine state prior to transtion
hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available
bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
#define ATT_READY_TO_ARM true
#define ATT_NOT_READY_TO_ARM false
#define ATT_SENSORS_INITIALIZED true
#define ATT_SENSORS_NOT_INITIALIZED false
#define ATT_SAFETY_AVAILABLE true
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{ "transition: init to standby",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby error",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to reboot",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to init",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to standby error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to reboot",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to standby",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to armed error",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed error to standby error",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby error to reboot",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to armed",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to reboot",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// hil on tests, standby error to standby not normally allowed
{ "transition: standby error to standby, hil on",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// Safety switch arming tests
{ "transition: init to standby, no safety switch",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby, safety switch off",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// standby error
{ "transition: armed error to standby error requested standby",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{ "no transition: init to armed",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby to armed error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to init",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to reboot",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to armed",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to reboot",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to armed",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to standby",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: reboot to armed",
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: in air restore to standby",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Sensor tests
{ "no transition: init to standby - sensors not initialized",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests
{ "no transition: init to standby, safety switch on",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
};
struct vehicle_status_s status;
struct safety_s safety;
arming_state_t new_arming_state;
struct safety_s safety;
struct actuator_armed_s armed;
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status.hil_state = test->hil_state;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
}
// Identical states.
status.arming_state = ARMING_STATE_INIT;
new_arming_state = ARMING_STATE_INIT;
mu_assert("no transition: identical states",
TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
// INIT to STANDBY.
armed.armed = false;
armed.ready_to_arm = false;
status.arming_state = ARMING_STATE_INIT;
status.condition_system_sensors_initialized = true;
new_arming_state = ARMING_STATE_STANDBY;
mu_assert("transition: init to standby",
TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
mu_assert("not armed", !armed.armed);
mu_assert("ready to arm", armed.ready_to_arm);
// INIT to STANDBY, sensors not initialized.
armed.armed = false;
armed.ready_to_arm = false;
status.arming_state = ARMING_STATE_INIT;
status.condition_system_sensors_initialized = false;
new_arming_state = ARMING_STATE_STANDBY;
mu_assert("no transition: sensors not initialized",
TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
mu_assert("not armed", !armed.armed);
mu_assert("not ready to arm", !armed.ready_to_arm);
return 0;
return true;
}
const char*
StateMachineHelperTest::arming_state_transition_arm_disarm_test()
{
struct vehicle_status_s status;
struct safety_s safety;
arming_state_t new_arming_state;
struct actuator_armed_s armed;
// TODO(sjwilks): ARM then DISARM.
return 0;
}
const char*
StateMachineHelperTest::main_state_transition_test()
bool StateMachineHelperTest::mainStateTransitionTest(void)
{
struct vehicle_status_s current_state;
main_state_t new_main_state;
@@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test()
// Identical states.
current_state.main_state = MAIN_STATE_MANUAL;
new_main_state = MAIN_STATE_MANUAL;
mu_assert("no transition: identical states",
ut_assert("no transition: identical states",
TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// AUTO to MANUAL.
current_state.main_state = MAIN_STATE_AUTO;
new_main_state = MAIN_STATE_MANUAL;
mu_assert("transition changed: auto to manual",
ut_assert("transition changed: auto to manual",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to SEATBELT.
// MANUAL to ALTCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_SEATBELT;
mu_assert("tranisition: manual to seatbelt",
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("tranisition: manual to altctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
// MANUAL to SEATBELT, invalid local altitude.
// MANUAL to ALTCTRL, invalid local altitude.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = false;
new_main_state = MAIN_STATE_SEATBELT;
mu_assert("no transition: invalid local altitude",
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to EASY.
// MANUAL to POSCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_EASY;
mu_assert("transition: manual to easy",
new_main_state = MAIN_STATE_POSCTL;
ut_assert("transition: manual to posctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
// MANUAL to EASY, invalid local position.
// MANUAL to POSCTRL, invalid local position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = false;
new_main_state = MAIN_STATE_EASY;
mu_assert("no transition: invalid position",
new_main_state = MAIN_STATE_POSCTL;
ut_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to AUTO.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_global_position_valid = true;
new_main_state = MAIN_STATE_AUTO;
mu_assert("transition: manual to auto",
ut_assert("transition: manual to auto",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
// MANUAL to AUTO, invalid global position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_global_position_valid = false;
new_main_state = MAIN_STATE_AUTO;
mu_assert("no transition: invalid global position",
ut_assert("no transition: invalid global position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
return 0;
return true;
}
const char*
StateMachineHelperTest::is_safe_test()
bool StateMachineHelperTest::isSafeTest(void)
{
struct vehicle_status_s current_state;
struct safety_s safety;
@@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test()
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
armed.armed = false;
armed.lockdown = true;
safety.safety_switch_available = true;
safety.safety_off = true;
mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = true;
mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = false;
safety.safety_off = false;
mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
return 0;
return true;
}
const char*
StateMachineHelperTest::run_tests()
void StateMachineHelperTest::runTests(void)
{
mu_run_test(arming_state_transition_test);
mu_run_test(arming_state_transition_arm_disarm_test);
mu_run_test(main_state_transition_test);
mu_run_test(is_safe_test);
return 0;
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
}
void
state_machine_helper_test()
void stateMachineHelperTest(void)
{
StateMachineHelperTest* test = new StateMachineHelperTest();
test->UnitTest::print_results(test->run_tests());
test->runTests();
test->printResults();
}
src/modules/commander/commander_tests/state_machine_helper_test.h
+1 -1
View File
@@ -39,6 +39,6 @@
#ifndef STATE_MACHINE_HELPER_TEST_H_
#define STATE_MACHINE_HELPER_TEST_
void state_machine_helper_test();
void stateMachineHelperTest(void);
#endif /* STATE_MACHINE_HELPER_TEST_H_ */
src/modules/commander/gyro_calibration.cpp
+10 -8
View File
@@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else {
poll_errcount++;
@@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
warn("WARNING: failed to apply new offsets for gyro");
}
close(fd);
@@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
uint64_t last_time = hrt_absolute_time();
@@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2 * M_PI_F;
if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
if (mag < -M_PI_F) mag += 2 * M_PI_F;
if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2 * M_PI_F;
if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
if (diff < -M_PI_F) diff += 2 * M_PI_F;
if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
baseline_integral += diff;
mag_last = mag;
src/modules/commander/mag_calibration.cpp
+36 -11
View File
@@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
@@ -121,9 +121,24 @@ int do_mag_calibration(int mavlink_fd)
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
/* clean up */
if (x != NULL) {
free(x);
}
if (y != NULL) {
free(y);
}
if (z != NULL) {
free(z);
}
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
@@ -163,8 +178,9 @@ int do_mag_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
if (calibration_counter % (calibration_maxcount / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
}
} else {
poll_errcount++;
@@ -198,14 +214,17 @@ int do_mag_calibration(int mavlink_fd)
}
}
if (x != NULL)
if (x != NULL) {
free(x);
}
if (y != NULL)
if (y != NULL) {
free(y);
}
if (z != NULL)
if (z != NULL) {
free(z);
}
if (res == OK) {
/* apply calibration and set parameters */
@@ -234,23 +253,29 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
src/modules/commander/module.mk
+4
View File
@@ -47,3 +47,7 @@ SRCS = commander.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/commander/px4_custom_mode.h
+2 -2
View File
@@ -12,8 +12,8 @@
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
};
src/modules/commander/rc_calibration.cpp
+3 -3
View File
@@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
src/modules/commander/state_machine_helper.cpp
+86 -91
View File
@@ -69,10 +69,44 @@ static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool failsafe_state_changed = true;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char *state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
/*
* Perform an atomic state update
*/
@@ -85,7 +119,6 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
ret = TRANSITION_NOT_CHANGED;
} else {
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
@@ -94,95 +127,43 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
armed->lockdown = false;
}
switch (new_arming_state) {
case ARMING_STATE_INIT:
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
/* allow going back from INIT for calibration */
if (status->arming_state == ARMING_STATE_STANDBY) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
}
break;
case ARMING_STATE_STANDBY:
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED
|| status->hil_state == HIL_STATE_ON) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = true;
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
break;
case ARMING_STATE_ARMED:
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_ARMED) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_STANDBY_ERROR:
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
/* XXX implement */
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
@@ -191,8 +172,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED)
warnx("arming transition rejected");
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
return ret;
}
@@ -234,7 +222,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
@@ -245,7 +233,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
@@ -266,7 +254,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case MAIN_STATE_OFFBOARD:
/* need global position estimate */
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
@@ -351,6 +339,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
@@ -362,26 +351,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -57,7 +57,7 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed);
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
src/modules/dataman/dataman.c
+77 -27
View File
@@ -1,10 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Jean Cyr
* Lorenz Meier
* Julian Oes
* Thomas Gubler
* 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
@@ -37,6 +33,11 @@
/**
* @file dataman.c
* DATAMANAGER driver.
*
* @author Jean Cyr
* @author Lorenz Meier
* @author Julian Oes
* @author Thomas Gubler
*/
#include <nuttx/config.h>
@@ -44,7 +45,9 @@
#include <stdlib.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <queue.h>
#include <string.h>
#include "dataman.h"
@@ -60,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t
__EXPORT int dm_clear(dm_item_t item);
__EXPORT int dm_restart(dm_reset_reason restart_type);
/* Types of function calls supported by the worker task */
/** Types of function calls supported by the worker task */
typedef enum {
dm_write_func = 0,
dm_read_func,
@@ -69,11 +72,12 @@ typedef enum {
dm_number_of_funcs
} dm_function_t;
/* Work task work item */
/** Work task work item */
typedef struct {
sq_entry_t link; /**< list linkage */
sem_t wait_sem;
dm_function_t func;
unsigned char first;
unsigned char func;
ssize_t result;
union {
struct {
@@ -98,6 +102,8 @@ typedef struct {
};
} work_q_item_t;
const size_t k_work_item_allocation_chunk_size = 8;
/* Usage statistics */
static unsigned g_func_counts[dm_number_of_funcs];
@@ -175,9 +181,23 @@ create_work_item(void)
unlock_queue(&g_free_q);
/* If we there weren't any free items then obtain memory for a new one */
if (item == NULL)
item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
/* If we there weren't any free items then obtain memory for a new ones */
if (item == NULL) {
item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
if (item) {
item->first = 1;
lock_queue(&g_free_q);
for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
(item + i)->first = 0;
sq_addfirst(&(item + i)->link, &(g_free_q.q));
}
/* Update the queue size and potentially the maximum queue size */
g_free_q.size += k_work_item_allocation_chunk_size - 1;
if (g_free_q.size > g_free_q.max_size)
g_free_q.max_size = g_free_q.size;
unlock_queue(&g_free_q);
}
}
/* If we got one then lock the item*/
if (item)
@@ -409,31 +429,31 @@ _clear(dm_item_t item)
return result;
}
/* Tell the data manager about the type of the last reset */
/** Tell the data manager about the type of the last reset */
static int
_restart(dm_reset_reason reason)
{
unsigned char buffer[2];
int offset, result = 0;
int offset = 0, result = 0;
/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
/* Loop through all of the data segments and delete those that are not persistent */
offset = 0;
while (1) {
size_t len;
/* Get data segment at current offset */
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
result = -1;
/* must be at eof */
break;
}
len = read(g_task_fd, buffer, sizeof(buffer));
if (len == 0)
if (len != sizeof(buffer)) {
/* must be at eof */
break;
}
/* check if segment contains data */
if (buffer[0]) {
@@ -441,12 +461,12 @@ _restart(dm_reset_reason reason)
/* Whether data gets deleted depends on reset type and data segment's persistence setting */
if (reason == DM_INIT_REASON_POWER_ON) {
if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
if (buffer[1] > DM_PERSIST_POWER_ON_RESET) {
clear_entry = 1;
}
} else {
if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) {
clear_entry = 1;
}
}
@@ -478,7 +498,7 @@ _restart(dm_reset_reason reason)
return result;
}
/* write to the data manager file */
/** Write to the data manager file */
__EXPORT ssize_t
dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
{
@@ -503,7 +523,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}
/* Retrieve from the data manager file */
/** Retrieve from the data manager file */
__EXPORT ssize_t
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{
@@ -594,6 +614,20 @@ task_main(int argc, char *argv[])
sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
else
close(g_task_fd);
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
@@ -603,7 +637,7 @@ task_main(int argc, char *argv[])
return -1;
}
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
warnx("Could not seek data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema); /* Don't want to hang startup */
@@ -612,6 +646,23 @@ task_main(int argc, char *argv[])
fsync(g_task_fd);
/* see if we need to erase any items based on restart type */
int sys_restart_val;
if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
warnx("Power on restart");
_restart(DM_INIT_REASON_POWER_ON);
}
else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
warnx("In flight restart");
_restart(DM_INIT_REASON_IN_FLIGHT);
}
else
warnx("Unknown restart");
}
else
warnx("Unknown restart");
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
/* worker thread is shutting down but still processing requests */
@@ -684,8 +735,8 @@ task_main(int argc, char *argv[])
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
break;
free(work);
if (work->first)
free(work);
}
destroy_q(&g_work_q);
@@ -703,12 +754,12 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the worker thread */
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
/* wait for the thread to actuall initialize */
/* wait for the thread to actually initialize */
sem_wait(&g_init_sema);
sem_destroy(&g_init_sema);
@@ -776,4 +827,3 @@ dataman_main(int argc, char *argv[])
exit(1);
}
src/modules/dataman/dataman.h
+13 -12
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -46,7 +46,7 @@
extern "C" {
#endif
/* Types of items that the data manager can store */
/** Types of items that the data manager can store */
typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
@@ -56,7 +56,7 @@ extern "C" {
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
/* The maximum number of instances for each item type */
/** The maximum number of instances for each item type */
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
@@ -65,23 +65,24 @@ extern "C" {
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
};
/* Data persistence levels */
/** Data persistence levels */
typedef enum {
DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */
DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */
DM_PERSIST_VOLATILE /* Data does not survive resets */
} dm_persitence_t;
/* The reason for the last reset */
/** The reason for the last reset */
typedef enum {
DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */
DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */
DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */
DM_INIT_REASON_VOLATILE /* Data does not survive reset */
} dm_reset_reason;
/* Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 126
/** Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 124
/* Retrieve from the data manager store */
/** Retrieve from the data manager store */
__EXPORT ssize_t
dm_read(
dm_item_t item, /* The item type to retrieve */
@@ -90,7 +91,7 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */
);
/* write to the data manager store */
/** write to the data manager store */
__EXPORT ssize_t
dm_write(
dm_item_t item, /* The item type to store */
@@ -100,13 +101,13 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */
);
/* Retrieve from the data manager store */
/** Erase all items of this type */
__EXPORT int
dm_clear(
dm_item_t item /* The item type to clear */
);
/* Tell the data manager about the type of the last reset */
/** Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(
dm_reset_reason restart_type /* The last reset type */
src/modules/dataman/module.mk
+2
View File
@@ -38,3 +38,5 @@
MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
src/modules/ekf_att_pos_estimator/estimator.cpp
+2609
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/estimator.h
+352
View File
@@ -0,0 +1,352 @@
#include <math.h>
#include <stdint.h>
#pragma once
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f
#define pi 3.141592657f
#define earthRate 0.000072921f
#define earthRadius 6378145.0f
#define earthRadiusInv 1.5678540e-7f
class Vector3f
{
private:
public:
float x;
float y;
float z;
float length(void) const;
void zero(void);
};
class Mat3f
{
private:
public:
Vector3f x;
Vector3f y;
Vector3f z;
Mat3f();
void identity();
Mat3f transpose(void) const;
};
Vector3f operator*(float sclIn1, Vector3f vecIn1);
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2);
const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
struct ekf_status_report {
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
float states[n_states];
bool statesNaN;
bool covarianceNaN;
bool kalmanGainsNaN;
};
class AttPosEKF {
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
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
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)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
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;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float 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 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
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;
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool 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;
bool numericalProtection;
unsigned storeIndex;
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void FuseVelposNED();
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);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
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], 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);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
int CheckAndBound();
void ResetPosition();
void ResetVelocity();
void ZeroVariables();
void GetFilterState(struct ekf_status_report *state);
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], float declination);
protected:
bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};
uint32_t millis();
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+1523
View File
File diff suppressed because it is too large Load Diff
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/fixedwing_att_control/module.mk → src/modules/ekf_att_pos_estimator/module.mk
+6 -6
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,11 @@
############################################################################
#
# Fixedwing Attitude Control application
# Main Attitude and Position Estimator for Fixed Wing Aircraft
#
MODULE_COMMAND = fixedwing_att_control
MODULE_COMMAND = ekf_att_pos_estimator
SRCS = fixedwing_att_control_main.c \
fixedwing_att_control_att.c \
fixedwing_att_control_rate.c
SRCS = fw_att_pos_estimator_main.cpp \
fw_att_pos_estimator_params.c \
estimator.cpp
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
-169
View File
@@ -1,169 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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 fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_att.h"
struct fw_att_control_params {
float roll_p;
float rollrate_lim;
float pitch_p;
float pitchrate_lim;
float yawrate_lim;
float pitch_roll_compensation_p;
};
struct fw_pos_control_param_handles {
param_t roll_p;
param_t rollrate_lim;
param_t pitch_p;
param_t pitchrate_lim;
param_t yawrate_lim;
param_t pitch_roll_compensation_p;
};
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->roll_p = param_find("FW_ROLL_P");
h->rollrate_lim = param_find("FW_ROLLR_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
h->yawrate_lim = param_find("FW_YAWR_LIM");
h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
{
param_get(h->roll_p, &(p->roll_p));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
return OK;
}
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp)
{
static int counter = 0;
static bool initialized = false;
static struct fw_att_control_params p;
static struct fw_pos_control_param_handles h;
static PID_t roll_controller;
static PID_t pitch_controller;
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
counter++;
return 0;
}
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
-367
View File
@@ -1,367 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_att_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
#include "fixedwing_att_control_att.h"
/* Prototypes */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_att_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/* Main Thread */
int fixedwing_att_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/* Check if there is a new position measurement or attitude setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (att_sp_updated)
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
printf("FW ATT CONTROL: Did not get a valid R\n");
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* set manual setpoints if required */
if (control_mode.flag_control_manual_enabled) {
if (control_mode.flag_control_attitude_enabled) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* execute attitude control if requested */
if (control_mode.flag_control_attitude_enabled) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) &&
isfinite(actuators.control[1]) &&
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
close(att_sub);
close(actuator_pub);
close(rates_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
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 fixedwing_att_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_att_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_att_control is running\n");
} else {
printf("\tfixedwing_att_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
-211
View File
@@ -1,211 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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 fixedwing_att_control_rate.c
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Implementation of a fixed wing attitude controller.
*
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
/*
* Controller parameters, accessible via MAVLink
*
*/
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
/* feedforward compensation */
PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */
struct fw_rate_control_params {
float rollrate_p;
float rollrate_i;
float rollrate_awu;
float pitchrate_p;
float pitchrate_i;
float pitchrate_awu;
float yawrate_p;
float yawrate_i;
float yawrate_awu;
float pitch_thr_ff;
};
struct fw_rate_control_param_handles {
param_t rollrate_p;
param_t rollrate_i;
param_t rollrate_awu;
param_t pitchrate_p;
param_t pitchrate_i;
param_t pitchrate_awu;
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_awu;
param_t pitch_thr_ff;
};
/* Internal Prototypes */
static int parameters_init(struct fw_rate_control_param_handles *h);
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p);
static int parameters_init(struct fw_rate_control_param_handles *h)
{
/* PID parameters */
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU");
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
return OK;
}
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p)
{
param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
return OK;
}
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators)
{
static int counter = 0;
static bool initialized = false;
static struct fw_rate_control_params p;
static struct fw_rate_control_param_handles h;
static PID_t roll_rate_controller;
static PID_t pitch_rate_controller;
static PID_t yaw_rate_controller;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
}
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
/* pitch rate (PI) */
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
counter++;
return 0;
}
src/modules/fixedwing_backside/fixedwing.cpp
+2 -2
View File
@@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
} else if (_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
-479
View File
@@ -1,479 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_pos_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
struct fw_pos_control_params {
float heading_p;
float headingr_p;
float headingr_i;
float headingr_lim;
float xtrack_p;
float altitude_p;
float roll_lim;
float pitch_lim;
};
struct fw_pos_control_param_handles {
param_t heading_p;
param_t headingr_p;
param_t headingr_i;
param_t headingr_lim;
param_t xtrack_p;
param_t altitude_p;
param_t roll_lim;
param_t pitch_lim;
};
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
/* Prototypes */
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
/**
* Deamon management function.
*/
__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Parameter management
*/
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->heading_p = param_find("FW_HEAD_P");
h->headingr_p = param_find("FW_HEADR_P");
h->headingr_i = param_find("FW_HEADR_I");
h->headingr_lim = param_find("FW_HEADR_LIM");
h->xtrack_p = param_find("FW_XTRACK_P");
h->altitude_p = param_find("FW_ALT_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
{
param_get(h->heading_p, &(p->heading_p));
param_get(h->headingr_p, &(p->headingr_p));
param_get(h->headingr_i, &(p->headingr_i));
param_get(h->headingr_lim, &(p->headingr_lim));
param_get(h->xtrack_p, &(p->xtrack_p));
param_get(h->altitude_p, &(p->altitude_p));
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}
/* Main Thread */
int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing pos control] started\n");
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint;
memset(&global_setpoint, 0, sizeof(global_setpoint));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err));
struct parameter_update_s param_update;
memset(&param_update, 0, sizeof(param_update));
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* publish attitude setpoint */
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
attitude_setpoint.thrust = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
struct pollfd fds[2] = {
{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }
};
bool global_sp_updated_set_once = false;
float psi_track = 0.0f;
int counter = 0;
struct fw_pos_control_params p;
struct fw_pos_control_param_handles h;
PID_t heading_controller;
PID_t heading_rate_controller;
PID_t offtrack_controller;
PID_t altitude_controller;
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(fw_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
}
if (global_sp_updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("next wp direction: %0.4f\n", (double)psi_track);
}
/* Simple Horizontal Control */
if (global_sp_updated_set_once) {
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* wrap difference back onto -pi..pi range */
psi_e = _wrap_pi(psi_e);
if (verbose) {
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
printf("delta_psi_c %.4f ", (double)delta_psi_c);
printf("psi_c %.4f ", (double)psi_c);
printf("att.yaw %.4f ", (double)att.yaw);
printf("psi_e %.4f ", (double)psi_e);
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
/* limit turn rate */
if (psi_rate_c > p.headingr_lim) {
psi_rate_c = p.headingr_lim;
} else if (psi_rate_c < -p.headingr_lim) {
psi_rate_c = -p.headingr_lim;
}
float psi_rate_e = psi_rate_c - att.yawspeed;
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
if (ground_speed < 10.0f) {
ground_speed = 10.0f;
}
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
}
if (verbose && counter % 100 == 0)
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
} else {
if (verbose && counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
}
/* Very simple Altitude Control */
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
// XXX need speed control
attitude_setpoint.thrust = 0.7f;
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
/* measure in what intervals the controller runs */
perf_count(fw_interval_perf);
counter++;
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
}
}
printf("[fixedwing_pos_control] exiting.\n");
thread_running = false;
close(attitude_setpoint_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
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 fixedwing_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_pos_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_pos_control is running\n");
} else {
printf("\tfixedwing_pos_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/fixedwing_pos_control/module.mk
-40
View File
@@ -1,40 +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.
#
############################################################################
#
# Fixedwing PositionControl application
#
MODULE_COMMAND = fixedwing_pos_control
SRCS = fixedwing_pos_control_main.c
src/modules/fw_att_control/fw_att_control_main.cpp
+40 -22
View File
@@ -173,6 +173,8 @@ private:
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
} _parameters; /**< local copies of interesting parameters */
@@ -211,6 +213,8 @@ private:
param_t trim_yaw;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
param_t man_roll_max;
param_t man_pitch_max;
} _parameter_handles; /**< handles for interesting parameters */
@@ -269,7 +273,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace att_control
@@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
/* fetch initial parameter values */
parameters_update();
}
@@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
/* pitch control parameters */
@@ -660,18 +671,24 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s) ||
/* if airspeed is not updating, we assume the normal average speed */
if (!isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
} else {
airspeed = _airspeed.indicated_airspeed_m_s;
airspeed = _airspeed.true_airspeed_m_s;
}
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
@@ -689,20 +706,21 @@ FixedwingAttitudeControl::task_main()
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do 45 degrees, the mapping is
* -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude. If more than 45 degrees are desired,
* a scaling parameter can be applied to the remote.
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
throttle_sp = _manual.throttle;
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
throttle_sp = _manual.z;
_actuators.control[4] = _manual.flaps;
/*
@@ -765,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,
@@ -774,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);
}
/*
@@ -808,10 +826,10 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.roll;
_actuators.control[1] = _manual.pitch;
_actuators.control[2] = _manual.yaw;
_actuators.control[3] = _manual.throttle;
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}
src/modules/fw_att_control/fw_att_control_params.c
+10
View File
@@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
// @DisplayName Max Manual Roll
// @Description Max roll for manual control in attitude stabilized mode
// @Range 0.0 to 90.0
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
// @DisplayName Max Manual Pitch
// @Description Max pitch for manual control in attitude stabilized mode
// @Range 0.0 to 90.0
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+103 -47
View File
@@ -89,6 +89,7 @@
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
@@ -134,6 +135,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for body frame accelerations */
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
@@ -147,13 +149,14 @@ private:
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
struct range_finder_report _range_finder; /**< range finder report */
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
@@ -181,7 +184,7 @@ private:
/* Landingslope object */
Landingslope landingslope;
float flare_curve_alt_last;
float flare_curve_alt_rel_last;
/* heading hold */
float target_bearing;
@@ -239,6 +242,7 @@ private:
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
float range_finder_rel_alt;
} _parameters; /**< local copies of interesting parameters */
@@ -283,6 +287,7 @@ private:
param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
param_t range_finder_rel_alt;
} _parameter_handles; /**< handles for interesting parameters */
@@ -308,6 +313,12 @@ private:
*/
bool vehicle_airspeed_poll();
/**
* Check for range finder updates.
*/
bool range_finder_poll();
/**
* Check for position updates.
*/
@@ -328,6 +339,11 @@ private:
*/
void navigation_capabilities_publish();
/**
* Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
*/
float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
/**
* Control position.
*/
@@ -345,7 +361,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
/*
* Reset takeoff state
@@ -384,6 +400,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
@@ -403,7 +420,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
launch_detected(false),
last_manual(false),
usePreTakeoffThrust(false),
flare_curve_alt_last(0.0f),
flare_curve_alt_rel_last(0.0f),
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
@@ -417,7 +434,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_control_mode(),
_global_pos(),
_pos_sp_triplet(),
_sensor_combined()
_sensor_combined(),
_range_finder()
{
_nav_capabilities.turn_distance = 0.0f;
@@ -442,6 +460,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -531,6 +550,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -626,6 +647,20 @@ FixedwingPositionControl::vehicle_airspeed_poll()
return airspeed_updated;
}
bool
FixedwingPositionControl::range_finder_poll()
{
/* check if there is a range finder measurement */
bool range_finder_updated;
orb_check(_range_finder_sub, &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
}
return range_finder_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -754,6 +789,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
{
float rel_alt_estimated = current_alt - land_setpoint_alt;
/* only use range finder if:
* parameter (range_finder_use_relative_alt) > 0
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
return rel_alt_estimated;
}
return range_finder.distance;
}
bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
@@ -896,12 +948,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
@@ -911,7 +965,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@@ -920,16 +974,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
/* avoid climbout */
if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
{
flare_curve_alt = pos_sp_triplet.current.alt;
flare_curve_alt_rel = 0.0f; // stay on ground
land_stayonground = true;
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_pitch_angle_rad,
0.0f, throttle_max, throttle_land,
@@ -941,7 +995,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
//warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
flare_curve_alt_last = flare_curve_alt;
flare_curve_alt_rel_last = flare_curve_alt_rel;
} else {
/* intersect glide slope:
@@ -949,20 +1003,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* if current position is higher or within 10m of slope follow the glide slope
* if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
* */
float altitude_desired = _global_pos.alt;
if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
/* stay on slope */
altitude_desired = landing_slope_alt_desired;
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
land_onslope = true;
}
} else {
/* continue horizontally */
altitude_desired = math::max(_global_pos.alt, L_altitude);
altitude_desired_rel = math::max(relative_alt, L_altitude_rel);
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1051,16 +1105,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (0/* easy mode enabled */) {
} else if (0/* posctrl mode enabled */) {
/** EASY FLIGHT **/
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to easy */) {
_seatbelt_hold_heading = _att.yaw;
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* easy on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
@@ -1073,44 +1127,44 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// climb_out = true;
// }
/* if in seatbelt mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_manual.z;
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* seatbelt mode enabled */) {
} else if (0/* altctrl mode enabled */) {
/** SEATBELT FLIGHT **/
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to seatbelt */) {
_seatbelt_hold_heading = _att.yaw;
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* seatbelt on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
/* if in seatbelt mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_manual.z;
/* user switched off throttle */
if (_manual.throttle < 0.1f) {
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
@@ -1120,15 +1174,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1185,6 +1239,7 @@ FixedwingPositionControl::task_main()
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
@@ -1264,6 +1319,7 @@ FixedwingPositionControl::task_main()
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+11
View File
@@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
/**
* Relative altitude threshold for range finder measurements for use during landing
*
* range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
* set to < 0 to disable
* the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
src/modules/fw_pos_control_l1/landingslope.cpp
+27 -7
View File
@@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues()
_horizontal_slope_displacement = (_flare_length - _d1);
}
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
{
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
return getLandingSlopeRelativeAltitude(wp_landing_distance);
else
return 0.0f;
}
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude)
{
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
{
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude);
else
return wp_altitude;
}
float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
else
return wp_landing_altitude;
return 0.0f;
}
float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
{
return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
}
src/modules/fw_pos_control_l1/landingslope.h
+28 -3
View File
@@ -63,11 +63,26 @@ public:
Landingslope() {}
~Landingslope() {}
/**
*
* @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeRelativeAltitude(float wp_landing_distance);
/**
*
* @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
* Performs check if aircraft is in front of waypoint to avoid climbout
*/
float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude);
/**
*
@@ -76,13 +91,22 @@ public:
*/
float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
/**
*
* @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative
}
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude;
}
/**
@@ -95,8 +119,9 @@ public:
}
float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
src/modules/mavlink/mavlink.c
+6
View File
@@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
/**
* Use/Accept HIL GPS message (even if not in HIL mode)
* If set to 1 incomming HIL GPS messages are parsed.
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
mavlink_system_t mavlink_system = {
100,
src/modules/position_estimator_mc/position_estimator_mc_params.c → src/modules/mavlink/mavlink_commands.cpp
Executable → Regular
+33 -28
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,35 +31,43 @@
*
****************************************************************************/
/*
* @file position_estimator_mc_params.c
*
* Parameters for position_estimator_mc
/**
* @file mavlink_commands.cpp
* Mavlink commands stream implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "position_estimator_mc_params.h"
#include "mavlink_commands.h"
/* Kalman Filter covariances */
/* gps measurement noise standard deviation */
PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
int parameters_init(struct position_estimator_mc_param_handles *h)
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
{
h->addNoise = param_find("POS_EST_ADDN");
h->sigma = param_find("POS_EST_SIGMA");
h->r = param_find("POS_EST_R");
h->baro_param_handle = param_find("POS_EST_BARO");
return OK;
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
}
int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
MavlinkCommandsStream::~MavlinkCommandsStream()
{
param_get(h->addNoise, &(p->addNoise));
param_get(h->sigma, &(p->sigma));
param_get(h->r, &(p->R));
param_get(h->baro_param_handle, &(p->baro));
return OK;
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
if (_cmd_sub->update(t)) {
/* only send commands for other systems/components */
if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
_cmd->target_system,
_cmd->target_component,
_cmd->command,
_cmd->confirmation,
_cmd->param1,
_cmd->param2,
_cmd->param3,
_cmd->param4,
_cmd->param5,
_cmd->param6,
_cmd->param7);
}
}
}
src/modules/fixedwing_att_control/fixedwing_att_control_att.h → src/modules/mavlink/mavlink_commands.h
+29 -15
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,19 +31,35 @@
*
****************************************************************************/
/* @file Fixed Wing Attitude Control */
/**
* @file mavlink_commands.h
* Mavlink commands stream definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
#define FIXEDWING_ATT_CONTROL_ATT_H_
#ifndef MAVLINK_COMMANDS_H_
#define MAVLINK_COMMANDS_H_
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp);
class Mavlink;
class MavlinkCommansStream;
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
#include "mavlink_main.h"
class MavlinkCommandsStream
{
private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
~MavlinkCommandsStream();
void update(const hrt_abstime t);
};
#endif /* MAVLINK_COMMANDS_H_ */
src/modules/mavlink/mavlink_main.cpp
+248 -13
View File
@@ -81,6 +81,7 @@
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -166,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
@@ -185,12 +186,26 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
}
}
ssize_t ret = write(uart, ch, desired);
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
if (ret != desired) {
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
/* 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) {
warnx("TX FAIL");
}
}
}
static void usage(void);
@@ -202,15 +217,23 @@ Mavlink::Mavlink() :
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
_use_hil_gps(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
_mission_pub(-1),
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@@ -261,7 +284,6 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
}
Mavlink::~Mavlink()
@@ -394,6 +416,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
return false;
}
void
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
inst->pass_message(msg);
}
}
}
int
Mavlink::get_uart_fd(unsigned index)
{
@@ -463,11 +497,13 @@ void Mavlink::mavlink_update_system(void)
static param_t param_system_id;
static param_t param_component_id;
static param_t param_system_type;
static param_t param_use_hil_gps;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
param_use_hil_gps = param_find("MAV_USEHILGPS");
initialized = true;
}
@@ -492,6 +528,11 @@ void Mavlink::mavlink_update_system(void)
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
param_get(param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
@@ -550,6 +591,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* open uart */
_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
if (_uart_fd < 0) {
return _uart_fd;
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
@@ -690,9 +736,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
static mavlink_message_t tx_msg;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
@@ -808,10 +854,10 @@ void Mavlink::publish_mission()
{
/* Initialize mission publication if necessary */
if (_mission_pub < 0) {
_mission_pub = orb_advertise(ORB_ID(mission), &mission);
_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
} else {
orb_publish(ORB_ID(mission), _mission_pub, &mission);
orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
}
}
@@ -1486,6 +1532,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
@@ -1498,6 +1546,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
statustext.severity = MAV_SEVERITY_INFO;
int i = 0;
while (i < len - 1) {
@@ -1616,6 +1666,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
}
}
int
Mavlink::message_buffer_init(int size)
{
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
return (_message_buffer.data == 0) ? ERROR : OK;
}
void
Mavlink::message_buffer_destroy()
{
_message_buffer.size = 0;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
free(_message_buffer.data);
}
int
Mavlink::message_buffer_count()
{
int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
if (n < 0) {
n += _message_buffer.size;
}
return n;
}
int
Mavlink::message_buffer_is_empty()
{
return _message_buffer.read_ptr == _message_buffer.write_ptr;
}
bool
Mavlink::message_buffer_write(void *ptr, int size)
{
// bytes available to write
int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
if (available < 0) {
available += _message_buffer.size;
}
if (size > available) {
// buffer overflow
return false;
}
char *c = (char *) ptr;
int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer
if (n < size) {
// message goes over end of the buffer
memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
_message_buffer.write_ptr = 0;
} else {
n = 0;
}
// now: n = bytes already written
int p = size - n; // number of bytes to write
memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
return true;
}
int
Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
{
// bytes available to read
int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
if (available == 0) {
return 0; // buffer is empty
}
int n = 0;
if (available > 0) {
// read pointer is before write pointer, all available bytes can be read
n = available;
*is_part = false;
} else {
// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
n = _message_buffer.size - _message_buffer.read_ptr;
*is_part = _message_buffer.write_ptr > 0;
}
*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
return n;
}
void
Mavlink::message_buffer_mark_read(int n)
{
_message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
}
void
Mavlink::pass_message(mavlink_message_t *msg)
{
if (_passing_on) {
/* size is 8 bytes plus variable payload */
int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_write(msg, size);
pthread_mutex_unlock(&_message_buffer_mutex);
}
}
int
Mavlink::task_main(int argc, char *argv[])
{
@@ -1632,7 +1801,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1672,10 +1841,22 @@ Mavlink::task_main(int argc, char *argv[])
break;
case 'f':
_forwarding_on = true;
break;
case 'p':
_passing_on = true;
break;
case 'v':
_verbose = true;
break;
case 'w':
_wait_to_transmit = true;
break;
default:
err_flag = true;
break;
@@ -1740,6 +1921,17 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on) {
/* initialize message buffer if multiplexing is on */
if (OK != message_buffer_init(500)) {
errx(1, "can't allocate message buffer, exiting");
}
/* initialize message buffer mutex */
pthread_mutex_init(&_message_buffer_mutex, NULL);
}
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
@@ -1766,6 +1958,8 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
@@ -1783,6 +1977,9 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
@@ -1826,6 +2023,9 @@ Mavlink::task_main(int argc, char *argv[])
set_hil_enabled(status->hil_state == HIL_STATE_ON);
}
/* update commands stream */
commands_stream.update(t);
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
@@ -1884,6 +2084,37 @@ Mavlink::task_main(int argc, char *argv[])
}
}
/* pass messages from other UARTs */
if (_passing_on) {
bool is_part;
void *read_ptr;
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
/* write first part of buffer */
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
/* write second part of buffer if there is some */
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
}
}
}
perf_end(_loop_perf);
}
@@ -1928,6 +2159,10 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
if (_passing_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
/* destroy log buffer */
mavlink_logbuffer_destroy(&_logbuffer);
@@ -1969,7 +2204,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
2000,
(main_t)&Mavlink::start_helper,
(const char **)argv);
@@ -2067,7 +2302,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]");
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
}
int mavlink_main(int argc, char *argv[])
src/modules/mavlink/mavlink_main.h
+50 -2
View File
@@ -138,6 +138,8 @@ public:
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
int get_uart_fd();
@@ -153,10 +155,14 @@ public:
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; };
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
/**
* Handle waypoint related messages.
*/
@@ -196,6 +202,16 @@ public:
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
protected:
Mavlink *next;
@@ -209,7 +225,10 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
@@ -218,7 +237,6 @@ private:
orb_advert_t _mission_pub;
struct mission_s mission;
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
@@ -234,6 +252,8 @@ private:
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
int _uart_fd;
int _baudrate;
int _datarate;
@@ -252,6 +272,18 @@ private:
bool _flow_control_enabled;
struct mavlink_message_buffer {
int write_ptr;
int read_ptr;
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
/**
* Send one parameter.
*
@@ -315,6 +347,22 @@ private:
int configure_stream(const char *stream_name, const float rate);
void configure_stream_threadsafe(const char *stream_name, const float rate);
int message_buffer_init(int size);
void message_buffer_destroy();
int message_buffer_count();
int message_buffer_is_empty();
bool message_buffer_write(void *ptr, int size);
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
void pass_message(mavlink_message_t *msg);
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
src/modules/mavlink/mavlink_messages.cpp
+119 -32
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
#include "mavlink_messages.h"
@@ -123,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (status->main_state == MAIN_STATE_SEATBELT) {
} else if (status->main_state == MAIN_STATE_ALTCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
} else if (status->main_state == MAIN_STATE_EASY) {
} else if (status->main_state == MAIN_STATE_POSCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -262,22 +263,21 @@ protected:
void send(const hrt_abstime t)
{
if (status_sub->update(t)) {
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
};
@@ -641,6 +641,47 @@ protected:
};
class MavlinkStreamViconPositionEstimate : public MavlinkStream
{
public:
const char *get_name()
{
return "VICON_POSITION_ESTIMATE";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamViconPositionEstimate();
}
private:
MavlinkOrbSubscription *pos_sub;
struct vehicle_vicon_position_s *pos;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t)
{
if (pos_sub->update(t)) {
mavlink_msg_vicon_position_estimate_send(_channel,
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->roll,
pos->pitch,
pos->yaw);
}
}
};
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
@@ -778,11 +819,11 @@ protected:
void send(const hrt_abstime t)
{
bool updated = status_sub->update(t);
updated |= pos_sp_triplet_sub->update(t);
updated |= act_sub->update(t);
bool updated = act_sub->update(t);
(void)pos_sp_triplet_sub->update(t);
(void)status_sub->update(t);
if (updated) {
if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
@@ -1097,10 +1138,10 @@ protected:
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
manual->pitch * 1000,
manual->yaw * 1000,
manual->throttle * 1000,
manual->x * 1000,
manual->y * 1000,
manual->z * 1000,
manual->r * 1000,
0);
}
}
@@ -1253,8 +1294,6 @@ protected:
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
@@ -1265,11 +1304,57 @@ protected:
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
}
}
};
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
const char *get_name()
{
return "DISTANCE_SENSOR";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamDistanceSensor();
}
private:
MavlinkOrbSubscription *range_sub;
struct range_finder_report *range;
protected:
void subscribe(Mavlink *mavlink)
{
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
range = (struct range_finder_report *)range_sub->get_data();
}
void send(const hrt_abstime t)
{
if (range_sub->update(t)) {
uint8_t type;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
}
};
@@ -1300,5 +1385,7 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
new MavlinkStreamDistanceSensor(),
new MavlinkStreamViconPositionEstimate(),
nullptr
};
src/modules/mavlink/mavlink_orb_subscription.cpp
+1
View File
@@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
if (_updated) {
orb_copy(_topic, _fd, _data);
_published = true;
return true;
}
}
src/modules/mavlink/mavlink_receiver.cpp
+79 -80
View File
@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
_manual_sub(-1),
_global_pos_pub(-1),
_local_pos_pub(-1),
_attitude_pub(-1),
@@ -162,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
*
* Accept HIL GPS messages if use_hil_gps flag is true.
* This allows to provide fake gps measurements to the system.
*/
if (_mavlink->get_hil_enabled()) {
switch (msg->msgid) {
@@ -169,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_hil_sensor(msg);
break;
case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg);
break;
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
handle_message_hil_state_quaternion(msg);
break;
@@ -181,6 +178,23 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
}
}
if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg);
break;
default:
break;
}
}
/* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true);
}
void
@@ -203,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
_mavlink->_task_should_exit = true;
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP ID");
return;
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
@@ -222,12 +242,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
@@ -245,6 +263,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -253,7 +272,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
if (_flow_pub <= 0) {
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
@@ -287,7 +306,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
@@ -312,7 +331,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (_vicon_position_pub <= 0) {
if (_vicon_position_pub < 0) {
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
@@ -373,7 +392,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub <= 0) {
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
@@ -401,7 +420,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub <= 0) {
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
@@ -415,47 +434,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
/* rc channels */
{
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
rc.timestamp = hrt_absolute_time();
rc.chan_count = 4;
manual.timestamp = hrt_absolute_time();
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
rc.chan[0].scaled = man.x / 1000.0f;
rc.chan[1].scaled = man.y / 1000.0f;
rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
if (_rc_pub == 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else {
orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
}
}
/* manual control */
{
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
if (_manual_pub == 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
}
@@ -619,11 +611,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.differential_pressure_timestamp = timestamp;
/* publish combined sensor topic */
if (_sensors_pub > 0) {
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
if (_sensors_pub < 0) {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
} else {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
}
}
@@ -638,11 +630,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
@@ -694,11 +686,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
if (_gps_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
} else {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
}
}
@@ -752,11 +744,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
if (_attitude_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
if (_attitude_pub < 0) {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
} else {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
}
}
@@ -766,7 +758,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp;
hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -774,30 +765,35 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
if (_global_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
} else {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
}
}
/* local position */
{
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
_hil_local_alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat;
hil_local_pos.ref_lon = hil_state.lon;
hil_local_pos.ref_lat = lat;
hil_local_pos.ref_lon = lon;
hil_local_pos.ref_alt = _hil_local_alt0;
}
float x;
float y;
map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
@@ -816,11 +812,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed;
if (_local_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
if (_local_pos_pub < 0) {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
} else {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
}
}
@@ -857,11 +853,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
}
@@ -885,8 +881,6 @@ MavlinkReceiver::receive_thread(void *arg)
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
@@ -913,6 +907,11 @@ MavlinkReceiver::receive_thread(void *arg)
/* handle packet with parameter component */
_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
if (_mavlink->get_forwarding_on()) {
/* forward any messages to other mavlink instances */
Mavlink::forward_message(&msg, _mavlink);
}
}
}
}
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -120,7 +120,6 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
int _manual_sub;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
orb_advert_t _attitude_pub;
@@ -143,4 +142,5 @@ private:
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
float _hil_local_alt0;
struct map_projection_reference_s _hil_local_proj_ref;
};
src/modules/mavlink/mavlink_stream.h
+1 -1
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file mavlink_stream.cpp
* @file mavlink_stream.h
* Mavlink messages stream definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
src/modules/mavlink/module.mk
+6 -1
View File
@@ -42,6 +42,11 @@ SRCS += mavlink_main.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp
mavlink_rate_limiter.cpp \
mavlink_commands.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1024
src/modules/mc_att_control/mc_att_control_main.cpp
+55 -36
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,9 +32,13 @@
****************************************************************************/
/**
* @file mc_att_control_main.c
* @file mc_att_control_main.cpp
* Multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
@@ -71,7 +72,7 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
/**
@@ -156,8 +157,11 @@ private:
param_t yaw_rate_i;
param_t yaw_rate_d;
param_t yaw_ff;
param_t yaw_rate_max;
param_t rc_scale_yaw;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -166,8 +170,11 @@ private:
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
float yaw_rate_max; /**< max yaw rate */
float rc_scale_yaw;
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
} _params;
/**
@@ -221,9 +228,9 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main attitude control task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace mc_att_control
@@ -272,6 +279,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_params.yaw_ff = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_rates_prev.zero();
_rates_sp.zero();
@@ -294,8 +306,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
/* fetch initial parameter values */
parameters_update();
@@ -330,7 +344,7 @@ MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll */
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
@@ -340,7 +354,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
/* pitch */
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
@@ -350,7 +364,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
/* yaw */
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
@@ -361,8 +375,16 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
/* manual control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
return OK;
}
@@ -404,7 +426,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
@@ -466,7 +487,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */
_v_att_sp.thrust = _manual_control_sp.throttle;
_v_att_sp.thrust = _manual_control_sp.z;
publish_att_sp = true;
}
@@ -483,24 +504,19 @@ MulticopterAttitudeControl::control_attitude(float dt)
// reset_yaw_sp = true;
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
if (yaw_offs < - yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale_yaw;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
}
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
/* reset yaw setpint to current position if needed */
@@ -513,8 +529,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.roll;
_v_att_sp.pitch_body = _manual_control_sp.pitch;
_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
@@ -627,6 +643,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit yaw rate */
_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}
@@ -807,7 +826,7 @@ MulticopterAttitudeControl::start()
_control_task = task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
2000,
(main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
src/modules/mc_att_control/mc_att_control_params.c
+45 -3
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
/**
* @file mc_att_control_params.c
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <systemlib/param/param.h>
@@ -173,3 +174,44 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
/**
* Max yaw rate
*
* Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
/**
* Max manual roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
/**
* Max manual pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
/**
* Max manual yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
src/modules/mc_pos_control/mc_pos_control_main.cpp
+174 -154
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* 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
@@ -40,6 +39,8 @@
* Output of velocity controller is thrust vector that splitted to thrust direction
* (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -62,9 +63,10 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
@@ -114,20 +116,21 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
int _global_pos_sub; /**< vehicle local position */
int _local_pos_sub; /**< vehicle local position */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct actuator_armed_s _arming; /**< actuator arming status */
struct vehicle_global_position_s _global_pos; /**< vehicle global position */
struct vehicle_local_position_s _local_pos; /**< vehicle local position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct {
@@ -145,23 +148,17 @@ private:
param_t xy_vel_d;
param_t xy_vel_max;
param_t xy_ff;
param_t tilt_max;
param_t tilt_max_air;
param_t land_speed;
param_t land_tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t tilt_max_land;
} _params_handles; /**< handles for interesting parameters */
struct {
float thr_min;
float thr_max;
float tilt_max;
float tilt_max_air;
float land_speed;
float land_tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
float tilt_max_land;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
@@ -172,14 +169,15 @@ private:
math::Vector<3> sp_offs_max;
} _params;
double _lat_sp;
double _lon_sp;
float _alt_sp;
struct map_projection_reference_s _ref_pos;
float _ref_alt;
hrt_abstime _ref_timestamp;
bool _reset_lat_lon_sp;
bool _reset_pos_sp;
bool _reset_alt_sp;
bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */
math::Vector<3> _pos;
math::Vector<3> _pos_sp;
math::Vector<3> _vel;
math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */
@@ -202,9 +200,13 @@ private:
static float scale_control(float ctl, float end, float dz);
/**
* Reset lat/lon to current position
* Update reference for local position projection
*/
void reset_lat_lon_sp();
void update_ref();
/**
* Reset position setpoint to current position
*/
void reset_pos_sp();
/**
* Reset altitude setpoint to current altitude
@@ -224,7 +226,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace pos_control
@@ -252,31 +254,32 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_sub(-1),
_manual_sub(-1),
_arming_sub(-1),
_global_pos_sub(-1),
_local_pos_sub(-1),
_pos_sp_triplet_sub(-1),
/* publications */
_att_sp_pub(-1),
_pos_sp_triplet_pub(-1),
_local_pos_sp_pub(-1),
_global_vel_sp_pub(-1),
_lat_sp(0.0),
_lon_sp(0.0),
_alt_sp(0.0f),
_ref_alt(0.0f),
_ref_timestamp(0),
_reset_lat_lon_sp(true),
_reset_alt_sp(true),
_use_global_alt(false)
_reset_pos_sp(true),
_reset_alt_sp(true)
{
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
memset(&_arming, 0, sizeof(_arming));
memset(&_global_pos, 0, sizeof(_global_pos));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
memset(&_ref_pos, 0, sizeof(_ref_pos));
_params.pos_p.zero();
_params.vel_p.zero();
_params.vel_i.zero();
@@ -285,6 +288,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params.vel_ff.zero();
_params.sp_offs_max.zero();
_pos.zero();
_pos_sp.zero();
_vel.zero();
_vel_sp.zero();
_vel_prev.zero();
@@ -303,11 +308,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.xy_vel_d = param_find("MPC_XY_VEL_D");
_params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX");
_params_handles.xy_ff = param_find("MPC_XY_FF");
_params_handles.tilt_max = param_find("MPC_TILT_MAX");
_params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR");
_params_handles.land_speed = param_find("MPC_LAND_SPEED");
_params_handles.land_tilt_max = param_find("MPC_LAND_TILT");
_params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND");
/* fetch initial parameter values */
parameters_update(true);
@@ -345,17 +348,18 @@ MulticopterPositionControl::parameters_update(bool force)
orb_check(_params_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
}
if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max, &_params.tilt_max);
param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
_params.tilt_max_air = math::radians(_params.tilt_max_air);
param_get(_params_handles.land_speed, &_params.land_speed);
param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
_params.tilt_max_land = math::radians(_params.tilt_max_land);
float v;
param_get(_params_handles.xy_p, &v);
@@ -402,33 +406,39 @@ MulticopterPositionControl::poll_subscriptions()
orb_check(_att_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
}
orb_check(_att_sp_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
orb_check(_control_mode_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
orb_check(_manual_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
orb_check(_arming_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
}
orb_check(_global_pos_sub, &updated);
orb_check(_local_pos_sub, &updated);
if (updated)
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
}
float
@@ -452,13 +462,40 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
}
void
MulticopterPositionControl::reset_lat_lon_sp()
MulticopterPositionControl::update_ref()
{
if (_reset_lat_lon_sp) {
_reset_lat_lon_sp = false;
_lat_sp = _global_pos.lat;
_lon_sp = _global_pos.lon;
mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp);
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp;
if (_ref_timestamp != 0) {
/* calculate current position setpoint in global frame */
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_ref_timestamp != 0) {
/* reproject position setpoint to new reference */
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
_ref_timestamp = _local_pos.ref_timestamp;
}
}
void
MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
@@ -467,25 +504,8 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt;
mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp);
}
}
void
MulticopterPositionControl::select_alt(bool global)
{
if (global != _use_global_alt) {
_use_global_alt = global;
if (global) {
/* switch from barometric to global altitude */
_alt_sp += _global_pos.alt - _global_pos.baro_alt;
} else {
/* switch from global to barometric altitude */
_alt_sp += _global_pos.baro_alt - _global_pos.alt;
}
_pos_sp(2) = _pos(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}
@@ -506,7 +526,7 @@ MulticopterPositionControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
parameters_update(true);
@@ -537,8 +557,7 @@ MulticopterPositionControl::task_main()
/* wakeup source */
struct pollfd fds[1];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -546,8 +565,9 @@ MulticopterPositionControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
@@ -564,7 +584,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_lat_lon_sp = true;
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
@@ -572,44 +592,41 @@ MulticopterPositionControl::task_main()
was_armed = _control_mode.flag_armed;
update_ref();
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_vel(0) = _global_pos.vel_n;
_vel(1) = _global_pos.vel_e;
_vel(2) = _global_pos.vel_d;
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
sp_move_rate.zero();
float alt = _global_pos.alt;
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* select altitude source and update setpoint */
select_alt(_global_pos.global_valid);
if (!_use_global_alt) {
alt = _global_pos.baro_alt;
}
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* reset lat/lon setpoint to current position if needed */
reset_lat_lon_sp();
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
sp_move_rate(0) = _manual.x;
sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
@@ -625,74 +642,47 @@ MulticopterPositionControl::task_main()
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp);
_alt_sp -= sp_move_rate(2) * dt;
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]);
pos_sp_offs(0) /= _params.sp_offs_max(0);
pos_sp_offs(1) /= _params.sp_offs_max(1);
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2);
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
}
/* fill position setpoint triplet */
_pos_sp_triplet.previous.valid = true;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = true;
_pos_sp_triplet.nav_state = NAV_STATE_NONE;
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
_pos_sp_triplet.current.lat = _lat_sp;
_pos_sp_triplet.current.lon = _lon_sp;
_pos_sp_triplet.current.alt = _alt_sp;
_pos_sp_triplet.current.yaw = _att_sp.yaw_body;
_pos_sp_triplet.current.loiter_radius = 0.0f;
_pos_sp_triplet.current.loiter_direction = 1.0f;
_pos_sp_triplet.current.pitch_min = 0.0f;
/* publish position setpoint triplet */
if (_pos_sp_triplet_pub > 0) {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
} else {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else {
/* always use AMSL altitude for AUTO */
select_alt(true);
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_lat_lon_sp = true;
_reset_pos_sp = true;
_reset_alt_sp = true;
/* update position setpoint */
_lat_sp = _pos_sp_triplet.current.lat;
_lon_sp = _pos_sp_triplet.current.lon;
_alt_sp = _pos_sp_triplet.current.alt;
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
@@ -701,11 +691,25 @@ MulticopterPositionControl::task_main()
} else {
/* no waypoint, loiter, reset position setpoint if needed */
reset_lat_lon_sp();
reset_pos_sp();
reset_alt_sp();
}
}
/* fill local position setpoint */
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
/* publish local position setpoint */
if (_local_pos_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
@@ -729,9 +733,7 @@ MulticopterPositionControl::task_main()
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err;
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
pos_err(2) = -(_alt_sp - alt);
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
@@ -741,7 +743,7 @@ MulticopterPositionControl::task_main()
}
if (!_control_mode.flag_control_position_enabled) {
_reset_lat_lon_sp = true;
_reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
@@ -780,7 +782,7 @@ MulticopterPositionControl::task_main()
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual.throttle;
i = _manual.z;
if (i < _params.thr_min) {
i = _params.thr_min;
@@ -839,16 +841,17 @@ MulticopterPositionControl::task_main()
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max;
float tilt_max = _params.tilt_max_air;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f)
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
@@ -939,8 +942,9 @@ MulticopterPositionControl::task_main()
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f)
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
@@ -999,6 +1003,18 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
@@ -1021,7 +1037,7 @@ MulticopterPositionControl::task_main()
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_lat_lon_sp = true;
_reset_pos_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
@@ -1046,7 +1062,7 @@ MulticopterPositionControl::start()
_control_task = task_spawn_cmd("mc_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
2000,
(main_t)&MulticopterPositionControl::task_main_trampoline,
nullptr);
@@ -1060,18 +1076,21 @@ MulticopterPositionControl::start()
int mc_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
errx(1, "usage: mc_pos_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (pos_control::g_control != nullptr)
if (pos_control::g_control != nullptr) {
errx(1, "already running");
}
pos_control::g_control = new MulticopterPositionControl;
if (pos_control::g_control == nullptr)
if (pos_control::g_control == nullptr) {
errx(1, "alloc failed");
}
if (OK != pos_control::g_control->start()) {
delete pos_control::g_control;
@@ -1083,8 +1102,9 @@ int mc_pos_control_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "stop")) {
if (pos_control::g_control == nullptr)
if (pos_control::g_control == nullptr) {
errx(1, "not running");
}
delete pos_control::g_control;
pos_control::g_control = nullptr;
src/modules/mc_pos_control/mc_pos_control_params.c
+26 -18
View File
@@ -35,6 +35,8 @@
/**
* @file mc_pos_control_params.c
* Multicopter position controller parameters.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <systemlib/param/param.h>
@@ -98,8 +100,9 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
/**
* Maximum vertical velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
@@ -108,7 +111,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
/**
* Vertical velocity feed forward
*
* Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
* Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
@@ -153,8 +156,9 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
/**
* Maximum horizontal velocity
*
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
@@ -163,7 +167,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
/**
* Horizontal velocity feed forward
*
* Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
* Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
@@ -172,31 +176,35 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
/**
* Maximum tilt
* Maximum tilt angle in air
*
* Limits maximum tilt in AUTO and EASY modes.
* Limits maximum tilt in AUTO and POSCTRL modes during flight.
*
* @unit deg
* @min 0.0
* @max 1.57
* @max 90.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
/**
* Maximum tilt during landing
*
* Limits maximum tilt angle on landing.
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f);
/**
* Landing descend rate
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
/**
* Maximum landing tilt
*
* Limits maximum tilt on landing.
*
* @min 0.0
* @max 1.57
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);
src/modules/navigator/geofence.cpp
+3 -2
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
/**
* @file geofence.cpp
* Provides functions for handling the geofence
*
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "geofence.h"
src/modules/navigator/geofence.h
+3 -2
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
/**
* @file geofence.h
* Provides functions for handling the geofence
*
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef GEOFENCE_H_
src/modules/navigator/geofence_params.c
-1
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/modules/navigator/mission_feasibility_checker.cpp
+7 -6
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
/**
* @file mission_feasibility_checker.cpp
* Provides checks if mission is feasible given the navigation capabilities
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#include "mission_feasibility_checker.h"
@@ -100,8 +101,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -125,8 +126,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
src/modules/navigator/mission_feasibility_checker.h
+4 -2
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,7 +33,11 @@
/**
* @file mission_feasibility_checker.h
* Provides checks if mission is feasible given the navigation capabilities
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#ifndef MISSION_FEASIBILITY_CHECKER_H_
#define MISSION_FEASIBILITY_CHECKER_H_
src/modules/navigator/module.mk
+2
View File
@@ -45,3 +45,5 @@ SRCS = navigator_main.cpp \
geofence_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
src/modules/navigator/navigator_main.cpp
+55 -85
View File
@@ -1,10 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,7 +31,7 @@
*
****************************************************************************/
/**
* @file navigator_main.c
* @file navigator_main.cpp
* Implementation of the main navigation state machine.
*
* Handles missions, geo fencing and failsafe navigation behavior.
@@ -177,7 +173,7 @@ private:
class Mission _mission;
bool _mission_item_valid; /**< current mission item valid */
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _global_pos_valid; /**< track changes of global_position */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@@ -510,7 +506,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
{
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -543,7 +539,7 @@ Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -611,7 +607,7 @@ Navigator::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -690,84 +686,56 @@ Navigator::task_main()
if (fds[6].revents & POLLIN) {
vehicle_status_update();
/* evaluate state machine from commander and set the navigator mode accordingly */
/* evaluate state requested by commander */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
bool stick_mode = false;
/* publish position setpoint triplet on each status update if navigator active */
_pos_sp_triplet_updated = true;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
/* switch to RTL if not already landed after RTL and home position set */
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
request_loiter_or_ready();
break;
case NAV_STATE_MISSION:
request_mission_if_available();
break;
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND &&
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
stick_mode = true;
break;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
request_loiter_or_ready();
stick_mode = true;
case NAV_STATE_LAND:
dispatch(EVENT_LAND_REQUESTED);
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
request_mission_if_available();
stick_mode = true;
}
break;
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
request_mission_if_available();
stick_mode = true;
}
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
request_mission_if_available();
}
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
request_loiter_or_ready();
break;
case NAV_STATE_MISSION:
request_mission_if_available();
break;
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND &&
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
case NAV_STATE_LAND:
dispatch(EVENT_LAND_REQUESTED);
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
request_mission_if_available();
}
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
on_mission_item_reached();
}
}
@@ -775,6 +743,8 @@ Navigator::task_main()
/* navigator shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
}
/* parameters updated */
@@ -813,17 +783,15 @@ Navigator::task_main()
if (fds[1].revents & POLLIN) {
global_position_update();
/* publish position setpoint triplet on each position update if navigator active */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
/* publish position setpoint triplet on each position update if navigator active */
_pos_sp_triplet_updated = true;
if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
if (myState == NAV_STATE_LAND && !_global_pos_valid) {
/* got global position when landing, update setpoint */
start_land();
}
_global_pos_valid = _global_pos.global_valid;
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
@@ -848,6 +816,8 @@ Navigator::task_main()
}
}
_global_pos_valid = _vstatus.condition_global_position_valid;
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
@@ -878,7 +848,7 @@ Navigator::start()
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
2000,
(main_t)&Navigator::task_main_trampoline,
nullptr);
@@ -893,9 +863,9 @@ Navigator::start()
void
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
if (_global_pos.global_valid) {
if (_global_pos_valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
@@ -1317,7 +1287,7 @@ Navigator::set_rtl_item()
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
@@ -1377,7 +1347,7 @@ Navigator::set_rtl_item()
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
_mission_item.pitch_min = 0.0f;
@@ -1539,7 +1509,7 @@ Navigator::check_mission_item_reached()
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}
src/modules/navigator/navigator_mission.cpp
+2 -1
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +33,8 @@
/**
* @file navigator_mission.cpp
* Helper class to access missions
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <string.h>
src/modules/navigator/navigator_mission.h
+2 -1
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +33,8 @@
/**
* @file navigator_mission.h
* Helper class to access missions
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef NAVIGATOR_MISSION_H
src/modules/navigator/navigator_params.c
-3
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/modules/navigator/navigator_state.h
+38 -4
View File
@@ -1,8 +1,42 @@
/*
* navigator_state.h
/****************************************************************************
*
* Created on: 27.01.2014
* Author: ton
* Copyright (c) 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 navigator_state.h
*
* Navigator state
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_STATE_H_
src/modules/position_estimator/module.mk
-44
View File
@@ -1,44 +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.
#
############################################################################
#
# Makefile to build the position estimator
#
MODULE_COMMAND = position_estimator
# XXX this should be converted to a deamon, its a pretty bad example app
MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT
MODULE_STACKSIZE = 4096
SRCS = position_estimator_main.c
src/modules/position_estimator/position_estimator_main.c
-423
View File
@@ -1,423 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 position_estimator_main.c
* Model-identification based position estimator for multirotors
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <poll.h>
#define N_STATES 6
#define ERROR_COVARIANCE_INIT 3
#define R_EARTH 6371000.0
#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
#define REPROJECTION_COUNTER_LIMIT 125
__EXPORT int position_estimator_main(int argc, char *argv[]);
static uint16_t position_estimator_counter_position_information;
/* values for map projection */
static double phi_1;
static double sin_phi_1;
static double cos_phi_1;
static double lambda_0;
static double scale;
/**
* Initializes the map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
phi_1 = lat_0 / 180.0 * M_PI;
lambda_0 = lon_0 / 180.0 * M_PI;
sin_phi_1 = sin(phi_1);
cos_phi_1 = cos(phi_1);
/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
const double r_earth = 6371000;
double lat1 = phi_1;
double lon1 = lambda_0;
double lat2 = phi_1 + 0.5 / 180 * M_PI;
double lon2 = lambda_0 + 0.5 / 180 * M_PI;
double sin_lat_2 = sin(lat2);
double cos_lat_2 = cos(lat2);
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
/* 2) calculate distance rho on plane */
double k_bar = 0;
double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
if (0 != c)
k_bar = c / sin(c);
double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
double rho = sqrt(pow(x2, 2) + pow(y2, 2));
scale = d / rho;
}
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
static void map_projection_project(double lat, double lon, float *x, float *y)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double phi = lat / 180.0 * M_PI;
double lambda = lon / 180.0 * M_PI;
double sin_phi = sin(phi);
double cos_phi = cos(phi);
double k_bar = 0;
/* using small angle approximation (formula in comment is without aproximation) */
double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
if (0 != c)
k_bar = c / sin(c);
/* using small angle approximation (formula in comment is without aproximation) */
*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
}
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
static void map_projection_reproject(float x, float y, double *lat, double *lon)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double x_descaled = x / scale;
double y_descaled = y / scale;
double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
double sin_c = sin(c);
double cos_c = cos(c);
double lat_sphere = 0;
if (c != 0)
lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
else
lat_sphere = asin(cos_c * sin_phi_1);
// printf("lat_sphere = %.10f\n",lat_sphere);
double lon_sphere = 0;
if (phi_1 == M_PI / 2) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
} else if (phi_1 == -M_PI / 2) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
} else {
lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
//using small angle approximation
// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
// if(denominator != 0)
// {
// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
// }
// else
// {
// ...
// }
}
// printf("lon_sphere = %.10f\n",lon_sphere);
*lat = lat_sphere * 180.0 / M_PI;
*lon = lon_sphere * 180.0 / M_PI;
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_main(int argc, char *argv[])
{
/* welcome user */
printf("[multirotor position_estimator] started\n");
/* initialize values */
static float u[2] = {0, 0};
static float z[3] = {0, 0, 0};
static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0};
static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0
};
static float xapo1[N_STATES];
static float Papo1[36];
static float gps_covariance[3] = {0.0f, 0.0f, 0.0f};
static uint16_t counter = 0;
position_estimator_counter_position_information = 0;
uint8_t predict_only = 1;
bool gps_valid = false;
bool new_initialization = true;
static double lat_current = 0.0d;//[°]] --> 47.0
static double lon_current = 0.0d; //[°]] -->8.5
float alt_current = 0.0f;
//TODO: handle flight without gps but with estimator
/* subscribe to vehicle status, attitude, gps */
struct vehicle_gps_position_s gps;
gps.fix_type = 0;
struct vehicle_status_s vstatus;
struct vehicle_attitude_s att;
int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to attitude at 100 Hz */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* wait until gps signal turns valid, only then can we initialize the projection */
while (gps.fix_type < 3) {
struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
* this choice is critical, since the vehicle status might not
* actually change, if this app is started after GPS lock was
* aquired.
*/
if (poll(fds, 1, 5000)) {
/* Wait for the GPS update to propagate (we have some time) */
usleep(5000);
/* Read wether the vehicle status changed */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
gps_valid = (gps.fix_type > 2);
}
}
/* get gps value for first initialization */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
lat_current = ((double)(gps.lat)) * 1e-7;
lon_current = ((double)(gps.lon)) * 1e-7;
alt_current = gps.alt * 1e-3;
/* initialize coordinates */
map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */
struct vehicle_local_position_s local_pos = {
.x = 0,
.y = 0,
.z = 0
};
orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
while (1) {
/*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */
struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} };
if (poll(fds, 1, 5000) <= 0) {
/* error / timeout */
} else {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
/* got attitude, updating pos as well */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/*copy attitude */
u[0] = att.roll;
u[1] = att.pitch;
/* initialize map projection with the last estimate (not at full rate) */
if (gps.fix_type > 2) {
/* Project gps lat lon (Geographic coordinate system) to plane*/
map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1]));
local_pos.x = z[0];
local_pos.y = z[1];
/* negative offset from initialization altitude */
local_pos.z = alt_current - (gps.alt) * 1e-3;
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
}
// gps_covariance[0] = gps.eph; //TODO: needs scaling
// gps_covariance[1] = gps.eph;
// gps_covariance[2] = gps.epv;
// } else {
// /* we can not use the gps signal (it is of low quality) */
// predict_only = 1;
// }
// // predict_only = 0; //TODO: only for testing, removeme, XXX
// // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX
// // usleep(100000); //TODO: only for testing, removeme, XXX
// /*Get new estimation (this is calculated in the plane) */
// //TODO: if new_initialization == true: use 0,0,0, else use xapo
// if (true == new_initialization) { //TODO,XXX: uncomment!
// xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
// xapo[2] = 0;
// xapo[4] = 0;
// position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
// } else {
// position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
// }
// /* Copy values from xapo1 to xapo */
// int i;
// for (i = 0; i < N_STATES; i++) {
// xapo[i] = xapo1[i];
// }
// if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
// /* Reproject from plane to geographic coordinate system */
// // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
// map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
// // //DEBUG
// // if(counter%500 == 0)
// // {
// // printf("phi_1: %.10f\n", phi_1);
// // printf("lambda_0: %.10f\n", lambda_0);
// // printf("lat_estimated: %.10f\n", lat_current);
// // printf("lon_estimated: %.10f\n", lon_current);
// // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
// // fflush(stdout);
// //
// // }
// // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
// // {
// /* send out */
// global_pos.lat = lat_current;
// global_pos.lon = lon_current;
// global_pos.alt = xapo1[4];
// global_pos.vx = xapo1[1];
// global_pos.vy = xapo1[3];
// global_pos.vz = xapo1[5];
/* publish current estimate */
// orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
// }
// else
// {
// printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]);
// fflush(stdout);
// }
// }
counter++;
}
}
return 0;
}
src/modules/position_estimator_inav/position_estimator_inav_main.c
+168 -72
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* 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
@@ -35,6 +34,8 @@
/**
* @file position_estimator_inav_main.c
* Model-identification based position estimator for multirotors
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <unistd.h>
@@ -57,6 +58,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/optical_flow.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
@@ -95,8 +97,9 @@ static void usage(const char *reason);
*/
static void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
@@ -112,8 +115,9 @@ static void usage(const char *reason)
*/
int position_estimator_inav_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
@@ -125,8 +129,9 @@ int position_estimator_inav_main(int argc, char *argv[])
verbose_mode = false;
if (argc > 1)
if (!strcmp(argv[2], "-v"))
if (!strcmp(argv[2], "-v")) {
verbose_mode = true;
}
thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
@@ -163,16 +168,19 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) {
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]);
fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
fwrite(s, 1, n, f);
free(s);
}
fsync(fileno(f));
fclose(f);
}
@@ -191,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f };
float x_est_prev[3], y_est_prev[3], z_est_prev[3];
memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev));
memset(z_est_prev, 0, sizeof(z_est_prev));
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
@@ -206,6 +219,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
struct map_projection_reference_s ref;
memset(&ref, 0, sizeof(ref));
hrt_abstime home_timestamp = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
@@ -238,7 +254,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation
float sonar_prev = 0.0f;
hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
@@ -257,6 +277,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct home_position_s home;
memset(&home, 0, sizeof(home));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
@@ -274,10 +296,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int home_position_sub = orb_subscribe(ORB_ID(home_position));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
orb_advert_t vehicle_global_position_pub = -1;
struct position_estimator_inav_params params;
struct position_estimator_inav_param_handles pos_inav_param_handles;
@@ -325,7 +348,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
global_pos.baro_valid = true;
}
}
}
@@ -425,6 +447,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* calculate time from previous update */
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp;
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
@@ -475,10 +501,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/* convert raw flow to angular flow */
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k;
flow_ang[1] = flow.flow_raw_y * params.flow_k;
flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
@@ -503,8 +529,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate)
if (!flow_accurate) {
w_flow *= 0.05f;
}
flow_valid = true;
@@ -516,32 +543,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_updates++;
}
/* home position */
orb_check(home_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(home_position), home_position_sub, &home);
if (home.timestamp != home_timestamp) {
home_timestamp = home.timestamp;
double est_lat, est_lon;
float est_alt;
if (ref_inited) {
/* calculate current estimated position in global frame */
est_alt = local_pos.ref_alt - local_pos.z;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
}
/* update reference */
map_projection_init(&ref, home.lat, home.lon);
/* update baro offset */
baro_offset += home.alt - local_pos.ref_alt;
local_pos.ref_lat = home.lat;
local_pos.ref_lon = home.lon;
local_pos.ref_alt = home.alt;
local_pos.ref_timestamp = home.timestamp;
if (ref_inited) {
/* reproject position estimate with new reference */
map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
z_est[0] = -(est_alt - local_pos.ref_alt);
}
ref_inited = true;
}
}
/* vehicle GPS position */
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
bool reset_est = false;
} else {
if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
gps_valid = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
gps_valid = false;
if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
}
if (gps_valid) {
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
/* initialize reference position if needed */
if (!ref_inited) {
if (ref_init_start == 0) {
@@ -549,18 +617,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
/* update baro offset */
baro_offset -= z_est[0];
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_alt = alt + z_est[0];
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
x_est[2] = accel_NED[0];
y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s;
z_est[0] = 0.0f;
y_est[2] = accel_NED[1];
local_pos.ref_lat = lat;
local_pos.ref_lon = lon;
local_pos.ref_alt = alt;
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
}
@@ -569,11 +644,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
/* reset position estimate when GPS becomes good */
if (reset_est) {
x_est[0] = gps_proj[0];
x_est[1] = gps.vel_n_m_s;
x_est[2] = accel_NED[0];
y_est[0] = gps_proj[1];
y_est[1] = gps.vel_e_m_s;
y_est[2] = accel_NED[1];
}
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
@@ -587,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_gps[2][1] = 0.0f;
}
w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
}
} else {
@@ -704,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est);
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
}
/* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
float x_est_prev[3], y_est_prev[3];
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps));
corr_baro = 0;
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
} else {
memcpy(z_est_prev, z_est, sizeof(z_est));
}
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est);
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
}
@@ -744,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_flow, 0, sizeof(corr_flow));
} else {
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
}
}
@@ -763,7 +864,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
landed = false;
landed_time = 0;
}
@@ -808,7 +909,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (t > pub_last + pub_interval) {
pub_last = t;
/* publish local position */
local_pos.xy_valid = can_estimate_xy && use_gps_xy;
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z;
@@ -831,40 +932,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
/* publish global position */
global_pos.global_valid = local_pos.xy_global;
if (local_pos.xy_global && local_pos.z_global) {
/* publish global position */
global_pos.timestamp = t;
global_pos.time_gps_usec = gps.time_gps_usec;
if (local_pos.xy_global) {
double est_lat, est_lon;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.time_gps_usec = gps.time_gps_usec;
}
global_pos.alt = local_pos.ref_alt - local_pos.z;
/* set valid values even if position is not valid */
if (local_pos.v_xy_valid) {
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
}
if (local_pos.z_global) {
global_pos.alt = local_pos.ref_alt - local_pos.z;
}
if (local_pos.z_valid) {
global_pos.baro_alt = baro_offset - local_pos.z;
}
if (local_pos.v_z_valid) {
global_pos.vel_d = local_pos.vz;
global_pos.yaw = local_pos.yaw;
// TODO implement dead-reckoning
global_pos.eph = gps.eph_m;
global_pos.epv = gps.epv_m;
if (vehicle_global_position_pub < 0) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}
global_pos.yaw = local_pos.yaw;
global_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}
src/modules/position_estimator_inav/position_estimator_inav_params.c
+2 -2
View File
@@ -50,13 +50,13 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
-58
View File
@@ -1,58 +0,0 @@
/*
* kalman_dlqe1.c
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
const real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
real32_T y;
int32_T i0;
real32_T b_y[3];
int32_T i1;
real32_T f0;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
b_y[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_y[i0] += C[i1] * A[i1 + 3 * i0];
}
y += b_y[i0] * x_aposteriori_k[i0];
}
y = z - y;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + K[i0] * y;
}
//printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
}
/* End of code generation (kalman_dlqe1.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
-30
View File
@@ -1,30 +0,0 @@
/*
* kalman_dlqe1.h
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
#ifndef __KALMAN_DLQE1_H__
#define __KALMAN_DLQE1_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe1.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe1_initialize.c
*
* Code generation for function 'kalman_dlqe1_initialize'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
#include "kalman_dlqe1_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (kalman_dlqe1_initialize.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
-30
View File
@@ -1,30 +0,0 @@
/*
* kalman_dlqe1_initialize.h
*
* Code generation for function 'kalman_dlqe1_initialize'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
#ifndef __KALMAN_DLQE1_INITIALIZE_H__
#define __KALMAN_DLQE1_INITIALIZE_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1_initialize(void);
#endif
/* End of code generation (kalman_dlqe1_initialize.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe1_terminate.c
*
* Code generation for function 'kalman_dlqe1_terminate'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
#include "kalman_dlqe1_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe1_terminate.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
-30
View File
@@ -1,30 +0,0 @@
/*
* kalman_dlqe1_terminate.h
*
* Code generation for function 'kalman_dlqe1_terminate'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
#ifndef __KALMAN_DLQE1_TERMINATE_H__
#define __KALMAN_DLQE1_TERMINATE_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1_terminate(void);
#endif
/* End of code generation (kalman_dlqe1_terminate.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* kalman_dlqe1_types.h
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
#ifndef __KALMAN_DLQE1_TYPES_H__
#define __KALMAN_DLQE1_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe1_types.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
-119
View File
@@ -1,119 +0,0 @@
/*
* kalman_dlqe2.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
real32_T A[9];
real32_T y;
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T b_k1[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
b_k1[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_k1[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
}
/* End of code generation (kalman_dlqe2.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe2.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __KALMAN_DLQE2_H__
#define __KALMAN_DLQE2_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe2.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe2_initialize.c
*
* Code generation for function 'kalman_dlqe2_initialize'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
#include "kalman_dlqe2_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe2_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (kalman_dlqe2_initialize.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe2_initialize.h
*
* Code generation for function 'kalman_dlqe2_initialize'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_INITIALIZE_H__
#define __KALMAN_DLQE2_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2_initialize(void);
#endif
/* End of code generation (kalman_dlqe2_initialize.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe2_terminate.c
*
* Code generation for function 'kalman_dlqe2_terminate'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
#include "kalman_dlqe2_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe2_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe2_terminate.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe2_terminate.h
*
* Code generation for function 'kalman_dlqe2_terminate'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_TERMINATE_H__
#define __KALMAN_DLQE2_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2_terminate(void);
#endif
/* End of code generation (kalman_dlqe2_terminate.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* kalman_dlqe2_types.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_TYPES_H__
#define __KALMAN_DLQE2_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe2_types.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
-137
View File
@@ -1,137 +0,0 @@
/*
* kalman_dlqe3.c
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "randn.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,
real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])
{
real32_T A[9];
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real_T b;
real32_T y;
real32_T b_y[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T b_k1[3];
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
}
if (addNoise == 1.0F) {
b = randn();
z += sigma * (real32_T)b;
}
if (posUpdate != 0.0F) {
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
b_y[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_y[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
} else {
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
}
}
}
/* End of code generation (kalman_dlqe3.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
-33
View File
@@ -1,33 +0,0 @@
/*
* kalman_dlqe3.h
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
#ifndef __KALMAN_DLQE3_H__
#define __KALMAN_DLQE3_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe3.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe3_data.c
*
* Code generation for function 'kalman_dlqe3_data'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
uint32_T method;
uint32_T state[2];
uint32_T b_method;
uint32_T b_state;
uint32_T c_state[2];
boolean_T state_not_empty;
/* Function Declarations */
/* Function Definitions */
/* End of code generation (kalman_dlqe3_data.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
-38
View File
@@ -1,38 +0,0 @@
/*
* kalman_dlqe3_data.h
*
* Code generation for function 'kalman_dlqe3_data'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_DATA_H__
#define __KALMAN_DLQE3_DATA_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
extern uint32_T method;
extern uint32_T state[2];
extern uint32_T b_method;
extern uint32_T b_state;
extern uint32_T c_state[2];
extern boolean_T state_not_empty;
/* Variable Definitions */
/* Function Declarations */
#endif
/* End of code generation (kalman_dlqe3_data.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
-47
View File
@@ -1,47 +0,0 @@
/*
* kalman_dlqe3_initialize.c
*
* Code generation for function 'kalman_dlqe3_initialize'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_initialize.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe3_initialize(void)
{
int32_T i;
static const uint32_T uv0[2] = { 362436069U, 0U };
rt_InitInfAndNaN(8U);
state_not_empty = FALSE;
b_state = 1144108930U;
b_method = 7U;
method = 0U;
for (i = 0; i < 2; i++) {
c_state[i] = 362436069U + 158852560U * (uint32_T)i;
state[i] = uv0[i];
}
if (state[1] == 0U) {
state[1] = 521288629U;
}
}
/* End of code generation (kalman_dlqe3_initialize.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
-33
View File
@@ -1,33 +0,0 @@
/*
* kalman_dlqe3_initialize.h
*
* Code generation for function 'kalman_dlqe3_initialize'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_INITIALIZE_H__
#define __KALMAN_DLQE3_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3_initialize(void);
#endif
/* End of code generation (kalman_dlqe3_initialize.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe3_terminate.c
*
* Code generation for function 'kalman_dlqe3_terminate'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe3_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe3_terminate.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
-33
View File
@@ -1,33 +0,0 @@
/*
* kalman_dlqe3_terminate.h
*
* Code generation for function 'kalman_dlqe3_terminate'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_TERMINATE_H__
#define __KALMAN_DLQE3_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3_terminate(void);
#endif
/* End of code generation (kalman_dlqe3_terminate.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* kalman_dlqe3_types.h
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:30 2013
*
*/
#ifndef __KALMAN_DLQE3_TYPES_H__
#define __KALMAN_DLQE3_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe3_types.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
-136
View File
@@ -1,136 +0,0 @@
/*
* positionKalmanFilter1D.c
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
real32_T P_aposteriori[9])
{
int32_T i0;
real32_T f0;
int32_T k;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
real32_T y;
real32_T K[3];
real32_T S;
int8_T I[9];
/* prediction */
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
}
x_aposteriori[i0] = f0 + B[i0] * u;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
}
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
}
P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
y = 0.0F;
for (k = 0; k < 3; k++) {
y += C[k] * x_aposteriori[k];
K[k] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
K[k] += C[i0] * P_apriori[i0 + 3 * k];
}
}
y = z - y;
S = 0.0F;
for (k = 0; k < 3; k++) {
S += K[k] * C[k];
}
S += R;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += P_apriori[i0 + 3 * k] * C[k];
}
K[i0] = f0 / S;
}
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] += K[i0] * y;
}
for (i0 = 0; i0 < 9; i0++) {
I[i0] = 0;
}
for (k = 0; k < 3; k++) {
I[k + 3 * k] = 1;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
P_aposteriori[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
}
}
}
} else {
for (i0 = 0; i0 < 9; i0++) {
P_aposteriori[i0] = P_apriori[i0];
}
}
}
/* End of code generation (positionKalmanFilter1D.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D.h
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_H__
#define __POSITIONKALMANFILTER1D_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
#endif
/* End of code generation (positionKalmanFilter1D.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
-157
View File
@@ -1,157 +0,0 @@
/*
* positionKalmanFilter1D_dT.c
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
x_aposteriori[3], real32_T P_aposteriori[9])
{
real32_T A[9];
int32_T i;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T K[3];
real32_T f0;
int32_T i0;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T fv0[3];
real32_T y;
static const int8_T iv2[3] = { 1, 0, 0 };
real32_T S;
int8_T I[9];
/* dynamics */
A[0] = 1.0F;
A[3] = dT;
A[6] = -0.5F * dT * dT;
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = -dT;
for (i = 0; i < 3; i++) {
A[2 + 3 * i] = (real32_T)iv0[i];
}
/* prediction */
K[0] = 0.5F * dT * dT;
K[1] = dT;
K[2] = 0.0F;
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
}
x_aposteriori[i] = f0 + K[i] * u;
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
b_A[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
}
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
}
P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
f0 = 0.0F;
for (i = 0; i < 3; i++) {
f0 += (real32_T)iv1[i] * x_aposteriori[i];
fv0[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
}
}
y = z - f0;
f0 = 0.0F;
for (i = 0; i < 3; i++) {
f0 += fv0[i] * (real32_T)iv2[i];
}
S = f0 + (real32_T)R;
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
}
K[i] = f0 / S;
}
for (i = 0; i < 3; i++) {
x_aposteriori[i] += K[i] * y;
}
for (i = 0; i < 9; i++) {
I[i] = 0;
}
for (i = 0; i < 3; i++) {
I[i + 3 * i] = 1;
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
P_aposteriori[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
}
}
}
} else {
for (i = 0; i < 9; i++) {
P_aposteriori[i] = P_apriori[i];
}
}
}
/* End of code generation (positionKalmanFilter1D_dT.c) */

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