mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 05:57:34 +08:00
Merged move of additional apps out of NuttX folders
This commit is contained in:
Lorenz Meier
@@ -0,0 +1,771 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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"
|
||||
|
||||
// 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),
|
||||
// ekf matrices
|
||||
F(9, 9),
|
||||
G(9, 6),
|
||||
P(9, 9),
|
||||
P0(9, 9),
|
||||
V(6, 6),
|
||||
// attitude measurement ekf matrices
|
||||
HAtt(6, 9),
|
||||
RAtt(6, 6),
|
||||
// position measurement ekf matrices
|
||||
HPos(6, 9),
|
||||
RPos(6, 6),
|
||||
// attitude representations
|
||||
C_nb(),
|
||||
q(),
|
||||
// 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)),
|
||||
_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),
|
||||
// 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;
|
||||
|
||||
// initial state covariance matrix
|
||||
P0 = Matrix::identity(9) * 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 quaternions
|
||||
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||
|
||||
// initialize dcm
|
||||
C_nb = Dcm(q);
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
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 &&
|
||||
_sensors.accelerometer_counter > 10 &&
|
||||
_sensors.gyro_counter > 10 &&
|
||||
_sensors.magnetometer_counter > 10) {
|
||||
if (correctAtt() == ret_ok) _attitudeInitCounter++;
|
||||
|
||||
if (_attitudeInitCounter > 100) {
|
||||
printf("[kalman_demo] initialized EKF attitude\n");
|
||||
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
|
||||
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);
|
||||
_positionInitialized = true;
|
||||
printf("[kalman_demo] initialized EKF state with GPS\n");
|
||||
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
|
||||
double(vN), double(vE), double(vD),
|
||||
lat, lon, alt);
|
||||
}
|
||||
|
||||
// prediciton 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 / 20 // 20 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;
|
||||
|
||||
// position publication
|
||||
_pos.timestamp = _pubTimeStamp;
|
||||
_pos.time_gps_usec = _gps.timestamp_position;
|
||||
_pos.valid = true;
|
||||
_pos.lat = getLatDegE7();
|
||||
_pos.lon = getLonDegE7();
|
||||
_pos.alt = float(alt);
|
||||
_pos.relative_alt = float(alt); // TODO, make relative
|
||||
_pos.vx = vN;
|
||||
_pos.vy = vE;
|
||||
_pos.vz = vD;
|
||||
_pos.hdg = psi;
|
||||
|
||||
// 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();
|
||||
|
||||
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) {
|
||||
Vector3 w(_sensors.gyro_rad_s);
|
||||
|
||||
// attitude
|
||||
q = q + q.derivative(w) * dt;
|
||||
|
||||
// renormalize quaternion if needed
|
||||
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
|
||||
q = q.unit();
|
||||
}
|
||||
|
||||
// C_nb update
|
||||
C_nb = Dcm(q);
|
||||
|
||||
// euler update
|
||||
EulerAngles euler(C_nb);
|
||||
phi = euler.getPhi();
|
||||
theta = euler.getTheta();
|
||||
psi = euler.getPsi();
|
||||
|
||||
// specific acceleration in nav frame
|
||||
Vector3 accelB(_sensors.accelerometer_m_s2);
|
||||
Vector3 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 predictioon equations
|
||||
// for discrte time EKF
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * 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 measurement
|
||||
Vector3 zMag(_sensors.magnetometer_ga);
|
||||
//float magNorm = zMag.norm();
|
||||
zMag = zMag.unit();
|
||||
|
||||
// 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
|
||||
float bN = cosf(dip) * cosf(dec);
|
||||
float bE = cosf(dip) * sinf(dec);
|
||||
float bD = sinf(dip);
|
||||
Vector3 bNav(bN, bE, bD);
|
||||
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
|
||||
|
||||
// accel measurement
|
||||
Vector3 zAccel(_sensors.accelerometer_m_s2);
|
||||
float accelMag = zAccel.norm();
|
||||
zAccel = zAccel.unit();
|
||||
|
||||
// ignore accel correction when accel mag not close to g
|
||||
Matrix RAttAdjust = RAtt;
|
||||
|
||||
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
|
||||
|
||||
if (ignoreAccel) {
|
||||
RAttAdjust(3, 3) = 1.0e10;
|
||||
RAttAdjust(4, 4) = 1.0e10;
|
||||
RAttAdjust(5, 5) = 1.0e10;
|
||||
|
||||
} else {
|
||||
//printf("correcting attitude with accel\n");
|
||||
}
|
||||
|
||||
// accel predicted measurement
|
||||
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
|
||||
|
||||
// combined measurement
|
||||
Vector zAtt(6);
|
||||
Vector zAttHat(6);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
zAtt(i) = zMag(i);
|
||||
zAtt(i + 3) = zAccel(i);
|
||||
zAttHat(i) = zMagHat(i);
|
||||
zAttHat(i + 3) = zAccelHat(i);
|
||||
}
|
||||
|
||||
// HMag , HAtt (0-2,:)
|
||||
float tmp1 =
|
||||
cosPsi * cosTheta * bN +
|
||||
sinPsi * cosTheta * bE -
|
||||
sinTheta * bD;
|
||||
HAtt(0, 1) = -(
|
||||
cosPsi * sinTheta * bN +
|
||||
sinPsi * sinTheta * bE +
|
||||
cosTheta * bD
|
||||
);
|
||||
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
|
||||
HAtt(1, 0) =
|
||||
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
|
||||
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
|
||||
cosPhi * cosTheta * bD;
|
||||
HAtt(1, 1) = sinPhi * tmp1;
|
||||
HAtt(1, 2) = -(
|
||||
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
|
||||
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
|
||||
);
|
||||
HAtt(2, 0) = -(
|
||||
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
|
||||
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
|
||||
(sinPhi * cosTheta) * bD
|
||||
);
|
||||
HAtt(2, 1) = cosPhi * tmp1;
|
||||
HAtt(2, 2) = -(
|
||||
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
|
||||
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
|
||||
);
|
||||
|
||||
// HAccel , HAtt (3-5,:)
|
||||
HAtt(3, 1) = cosTheta;
|
||||
HAtt(4, 0) = -cosPhi * cosTheta;
|
||||
HAtt(4, 1) = sinPhi * sinTheta;
|
||||
HAtt(5, 0) = sinPhi * cosTheta;
|
||||
HAtt(5, 1) = cosPhi * sinTheta;
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Vector y = zAtt - zAttHat; // residual
|
||||
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
|
||||
Matrix K = P * HAtt.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
|
||||
// check correciton is sane
|
||||
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||
float val = xCorrect(i);
|
||||
|
||||
if (isnan(val) || isinf(val)) {
|
||||
// abort correction and return
|
||||
printf("[kalman_demo] numerical failure in att correction\n");
|
||||
// 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.dot(S.inverse() * y);
|
||||
|
||||
if (beta > _faultAtt.get()) {
|
||||
printf("fault in attitude: beta = %8.4f\n", (double)beta);
|
||||
printf("y:\n"); y.print();
|
||||
printf("zMagHat:\n"); zMagHat.print();
|
||||
printf("zMag:\n"); zMag.print();
|
||||
printf("bNav:\n"); bNav.print();
|
||||
}
|
||||
|
||||
// update quaternions from euler
|
||||
// angle correction
|
||||
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||
|
||||
return ret_ok;
|
||||
}
|
||||
|
||||
int KalmanNav::correctPos()
|
||||
{
|
||||
using namespace math;
|
||||
|
||||
// residual
|
||||
Vector y(6);
|
||||
y(0) = _gps.vel_n_m_s - vN;
|
||||
y(1) = _gps.vel_e_m_s - vE;
|
||||
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(4) = double(_gps.alt) / 1.0e3 - alt;
|
||||
y(5) = double(_sensors.baro_alt_meter) - alt;
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
|
||||
Matrix K = P * HPos.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
|
||||
// check correction is sane
|
||||
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||
float val = xCorrect(i);
|
||||
|
||||
if (isnan(val) || isinf(val)) {
|
||||
// abort correction and return
|
||||
printf("[kalman_demo] numerical failure in gps correction\n");
|
||||
// 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 += double(xCorrect(ALT));
|
||||
|
||||
// update state covariance
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
P = P - K * HPos * P;
|
||||
|
||||
// fault detetcion
|
||||
float beta = y.dot(S.inverse() * y);
|
||||
|
||||
if (beta > _faultPos.get()) {
|
||||
printf("fault in gps: beta = %8.4f\n", (double)beta);
|
||||
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
|
||||
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))));
|
||||
}
|
||||
|
||||
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
|
||||
RAtt(1, 1) = noiseMagSq;
|
||||
RAtt(2, 2) = noiseMagSq;
|
||||
|
||||
// accelerometer noise
|
||||
float noiseAccelSq = _rAccel.get() * _rAccel.get();
|
||||
|
||||
// bound noise to prevent singularities
|
||||
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
|
||||
|
||||
RAtt(3, 3) = noiseAccelSq; // normalized direction
|
||||
RAtt(4, 4) = noiseAccelSq;
|
||||
RAtt(5, 5) = 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
|
||||
}
|
||||
@@ -0,0 +1,180 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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 <controllib/block/UOrbSubscription.hpp>
|
||||
#include <controllib/block/UOrbPublication.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.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() {};
|
||||
/**
|
||||
* 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 F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix P; /**< state covariance matrix */
|
||||
math::Matrix P0; /**< initial state covariance matrix */
|
||||
math::Matrix V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix RPos; /**< position measurement noise matrix */
|
||||
// attitude
|
||||
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
// publications
|
||||
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
control::UOrbPublication<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, alt; /**< lat, lon, alt, radians */
|
||||
// parameters
|
||||
control::BlockParam<float> _vGyro; /**< gyro process noise */
|
||||
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
|
||||
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
|
||||
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
|
||||
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
|
||||
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
|
||||
control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */
|
||||
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
|
||||
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
|
||||
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
|
||||
control::BlockParam<float> _g; /**< gravitational constant */
|
||||
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
|
||||
control::BlockParam<float> _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; }
|
||||
};
|
||||
@@ -0,0 +1,152 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Example User <mail@example.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file kalman_demo.cpp
|
||||
* Demonstration of control library
|
||||
*/
|
||||
|
||||
#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 <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 deamon_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);
|
||||
|
||||
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\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 att_pos_estimator_ekf_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("kalman_demo already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("kalman_demo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4096,
|
||||
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) {
|
||||
printf("\tkalman_demo app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tkalman_demo app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int kalman_demo_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("[kalman_demo] starting\n");
|
||||
|
||||
using namespace math;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
KalmanNav nav(NULL, "KF");
|
||||
|
||||
while (!thread_should_exit) {
|
||||
nav.update();
|
||||
}
|
||||
|
||||
printf("[kalman_demo] exiting.\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
|
||||
# XXX this might be intended for the spawned deamon, validate
|
||||
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
|
||||
|
||||
SRCS = kalman_main.cpp \
|
||||
KalmanNav.cpp \
|
||||
params.c
|
||||
@@ -0,0 +1,16 @@
|
||||
#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, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.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);
|
||||
@@ -0,0 +1,169 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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); //P Controller
|
||||
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 Fixed Wing Attitude Control */
|
||||
|
||||
#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
|
||||
#define FIXEDWING_ATT_CONTROL_ATT_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>
|
||||
|
||||
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);
|
||||
|
||||
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
|
||||
@@ -0,0 +1,370 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_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_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 vstatus_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_status), vstatus_sub, &vstatus);
|
||||
|
||||
gyro[0] = att.rollspeed;
|
||||
gyro[1] = att.pitchspeed;
|
||||
gyro[2] = att.yawspeed;
|
||||
|
||||
/* control */
|
||||
|
||||
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
|
||||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
/* 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;
|
||||
|
||||
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
||||
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
|
||||
/* 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();
|
||||
|
||||
/* 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;
|
||||
|
||||
/* pass through flaps */
|
||||
if (isfinite(manual_sp.flaps)) {
|
||||
actuators.control[4] = manual_sp.flaps;
|
||||
|
||||
} else {
|
||||
actuators.control[4] = 0.0f;
|
||||
}
|
||||
|
||||
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
/* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 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("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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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); // 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); // 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); // 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 Fixed Wing Attitude Rate Control */
|
||||
|
||||
#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
|
||||
#define FIXEDWING_ATT_CONTROL_RATE_H_
|
||||
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[],
|
||||
struct actuator_controls_s *actuators);
|
||||
|
||||
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
|
||||
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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 Attitude Control application
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fixedwing_att_control
|
||||
|
||||
SRCS = fixedwing_att_control_main.c \
|
||||
fixedwing_att_control_att.c \
|
||||
fixedwing_att_control_rate.c
|
||||
@@ -0,0 +1,170 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 fixedwing_backside_main.cpp
|
||||
* @author James Goppert
|
||||
*
|
||||
* Fixedwing backside controller using control library
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <controllib/fixedwing.hpp>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
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 */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int fixedwing_backside_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int control_demo_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Test function
|
||||
*/
|
||||
void test();
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\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_backside_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("control_demo already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("control_demo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
5120,
|
||||
control_demo_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
test();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tcontrol_demo app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tcontrol_demo app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int control_demo_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("[control_Demo] starting\n");
|
||||
|
||||
using namespace control;
|
||||
|
||||
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
|
||||
|
||||
thread_running = true;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
autopilot.update();
|
||||
}
|
||||
|
||||
printf("[control_demo] exiting.\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void test()
|
||||
{
|
||||
printf("beginning control lib test\n");
|
||||
control::basicBlocksTest();
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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 backside controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fixedwing_backside
|
||||
|
||||
SRCS = fixedwing_backside_main.cpp
|
||||
@@ -0,0 +1,71 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
// currently tuned for easystar from arkhangar in HIL
|
||||
//https://github.com/arktools/arkhangar
|
||||
|
||||
// 16 is max name length
|
||||
|
||||
// gyro low pass filter
|
||||
PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
|
||||
|
||||
// yaw washout
|
||||
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
|
||||
|
||||
// stabilization mode
|
||||
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
|
||||
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
|
||||
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
|
||||
|
||||
// psi -> phi -> p
|
||||
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg
|
||||
|
||||
// velocity -> theta
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
|
||||
|
||||
|
||||
// theta -> q
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
|
||||
|
||||
// h -> thr
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
|
||||
|
||||
// crosstrack
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain
|
||||
|
||||
// speed command
|
||||
PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity
|
||||
PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity
|
||||
PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
|
||||
|
||||
// rate of climb
|
||||
// this is what rate of climb is commanded (in m/s)
|
||||
// when the pitch stick is fully defelcted in simple mode
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
|
||||
|
||||
// rate of climb -> thr
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
|
||||
@@ -0,0 +1,479 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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(¶m_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); //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);
|
||||
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
|
||||
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //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((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||
(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("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);
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
Executable
+42
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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 multirotor attitude controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = multirotor_att_control
|
||||
|
||||
SRCS = multirotor_att_control_main.c \
|
||||
multirotor_attitude_control.c \
|
||||
multirotor_rate_control.c
|
||||
@@ -0,0 +1,485 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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
|
||||
* 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 multirotor_att_control_main.c
|
||||
*
|
||||
* Implementation of multirotor attitude control main loop.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
#include <getopt.h>
|
||||
#include <time.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include "multirotor_attitude_control.h"
|
||||
#include "multirotor_rate_control.h"
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
|
||||
|
||||
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
|
||||
|
||||
static bool thread_should_exit;
|
||||
static int mc_task;
|
||||
static bool motor_test_mode = false;
|
||||
|
||||
static orb_advert_t actuator_pub;
|
||||
|
||||
static struct vehicle_status_s state;
|
||||
|
||||
static int
|
||||
mc_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* declare and safely initialize all structs */
|
||||
memset(&state, 0, sizeof(state));
|
||||
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 manual_control_setpoint_s manual;
|
||||
memset(&manual, 0, sizeof(manual));
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
struct offboard_control_setpoint_s offboard_sp;
|
||||
memset(&offboard_sp, 0, sizeof(offboard_sp));
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
memset(&rates_sp, 0, sizeof(rates_sp));
|
||||
|
||||
struct actuator_controls_s actuators;
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
||||
/*
|
||||
* Do not rate-limit the loop to prevent aliasing
|
||||
* if rate-limiting would be desired later, the line below would
|
||||
* enable it.
|
||||
*
|
||||
* rate-limit the attitude subscription to 200Hz to pace our loop
|
||||
* orb_set_interval(att_sub, 5);
|
||||
*/
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = att_sub, .events = POLLIN },
|
||||
{ .fd = param_sub, .events = POLLIN }
|
||||
};
|
||||
|
||||
/* publish actuator controls */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
|
||||
actuators.control[i] = 0.0f;
|
||||
}
|
||||
|
||||
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
||||
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
|
||||
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
|
||||
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
|
||||
|
||||
/* welcome user */
|
||||
printf("[multirotor_att_control] starting\n");
|
||||
|
||||
/* store last control mode to detect mode switches */
|
||||
bool flag_control_manual_enabled = false;
|
||||
bool flag_control_attitude_enabled = false;
|
||||
bool flag_system_armed = false;
|
||||
|
||||
/* store if yaw position or yaw speed has been changed */
|
||||
bool control_yaw_position = true;
|
||||
|
||||
/* store if we stopped a yaw movement */
|
||||
bool first_time_after_yaw_speed_control = true;
|
||||
|
||||
/* prepare the handle for the failsafe throttle */
|
||||
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
|
||||
float failsafe_throttle = 0.0f;
|
||||
|
||||
|
||||
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(mc_err_perf);
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[1].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 */
|
||||
// XXX no params here yet
|
||||
}
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
perf_begin(mc_loop_perf);
|
||||
|
||||
/* get a local copy of system state */
|
||||
bool updated;
|
||||
orb_check(state_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
}
|
||||
|
||||
/* get a local copy of manual setpoint */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
/* get a local copy of attitude setpoint */
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
|
||||
/* get a local copy of rates setpoint */
|
||||
orb_check(setpoint_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
|
||||
}
|
||||
|
||||
/* get a local copy of the current sensor values */
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
|
||||
/** STEP 1: Define which input is the dominating control input */
|
||||
if (state.flag_control_offboard_enabled) {
|
||||
/* offboard inputs */
|
||||
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
|
||||
rates_sp.roll = offboard_sp.p1;
|
||||
rates_sp.pitch = offboard_sp.p2;
|
||||
rates_sp.yaw = offboard_sp.p3;
|
||||
rates_sp.thrust = offboard_sp.p4;
|
||||
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
|
||||
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
|
||||
att_sp.roll_body = offboard_sp.p1;
|
||||
att_sp.pitch_body = offboard_sp.p2;
|
||||
att_sp.yaw_body = offboard_sp.p3;
|
||||
att_sp.thrust = offboard_sp.p4;
|
||||
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
/* STEP 2: publish the result to the vehicle actuators */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
}
|
||||
|
||||
|
||||
} else if (state.flag_control_manual_enabled) {
|
||||
|
||||
if (state.flag_control_attitude_enabled) {
|
||||
|
||||
/* initialize to current yaw if switching to manual or att control */
|
||||
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
||||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||
state.flag_system_armed != flag_system_armed) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
}
|
||||
|
||||
static bool rc_loss_first_time = true;
|
||||
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
if (state.rc_signal_lost) {
|
||||
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
|
||||
param_get(failsafe_throttle_handle, &failsafe_throttle);
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
|
||||
/*
|
||||
* Only go to failsafe throttle if last known throttle was
|
||||
* high enough to create some lift to make hovering state likely.
|
||||
*
|
||||
* This is to prevent that someone landing, but not disarming his
|
||||
* multicopter (throttle = 0) does not make it jump up in the air
|
||||
* if shutting down his remote.
|
||||
*/
|
||||
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
|
||||
att_sp.thrust = failsafe_throttle;
|
||||
|
||||
} else {
|
||||
att_sp.thrust = 0.0f;
|
||||
}
|
||||
|
||||
/* keep current yaw, do not attempt to go to north orientation,
|
||||
* since if the pilot regains RC control, he will be lost regarding
|
||||
* the current orientation.
|
||||
*/
|
||||
if (rc_loss_first_time)
|
||||
att_sp.yaw_body = att.yaw;
|
||||
|
||||
rc_loss_first_time = false;
|
||||
|
||||
} else {
|
||||
rc_loss_first_time = true;
|
||||
|
||||
att_sp.roll_body = manual.roll;
|
||||
att_sp.pitch_body = manual.pitch;
|
||||
|
||||
/* set attitude if arming */
|
||||
if (!flag_control_attitude_enabled && state.flag_system_armed) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
}
|
||||
|
||||
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
|
||||
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
|
||||
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
|
||||
rates_sp.yaw = manual.yaw;
|
||||
control_yaw_position = false;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* This mode SHOULD be the default mode, which is:
|
||||
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
|
||||
*
|
||||
* However, we fall back to this setting for all other (nonsense)
|
||||
* settings as well.
|
||||
*/
|
||||
|
||||
/* only move setpoint if manual input is != 0 */
|
||||
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
|
||||
rates_sp.yaw = manual.yaw;
|
||||
control_yaw_position = false;
|
||||
first_time_after_yaw_speed_control = true;
|
||||
|
||||
} else {
|
||||
if (first_time_after_yaw_speed_control) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
first_time_after_yaw_speed_control = false;
|
||||
}
|
||||
|
||||
control_yaw_position = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
att_sp.thrust = manual.throttle;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* STEP 2: publish the controller output */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
|
||||
if (motor_test_mode) {
|
||||
printf("testmode");
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.pitch_body = 0.0f;
|
||||
att_sp.yaw_body = 0.0f;
|
||||
att_sp.thrust = 0.1f;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
/* STEP 2: publish the result to the vehicle actuators */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* manual rate inputs, from RC control or joystick */
|
||||
if (state.flag_control_rates_enabled &&
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
|
||||
rates_sp.roll = manual.roll;
|
||||
|
||||
rates_sp.pitch = manual.pitch;
|
||||
rates_sp.yaw = manual.yaw;
|
||||
rates_sp.thrust = manual.throttle;
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
|
||||
if (state.flag_control_attitude_enabled) {
|
||||
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
|
||||
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
}
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
perf_count(mc_interval_perf);
|
||||
|
||||
float gyro[3];
|
||||
|
||||
/* get current rate setpoint */
|
||||
bool rates_sp_valid = false;
|
||||
orb_check(rates_sp_sub, &rates_sp_valid);
|
||||
|
||||
if (rates_sp_valid) {
|
||||
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
|
||||
}
|
||||
|
||||
/* apply controller */
|
||||
gyro[0] = att.rollspeed;
|
||||
gyro[1] = att.pitchspeed;
|
||||
gyro[2] = att.yawspeed;
|
||||
|
||||
multirotor_control_rates(&rates_sp, gyro, &actuators);
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
/* update state */
|
||||
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
|
||||
flag_control_manual_enabled = state.flag_control_manual_enabled;
|
||||
flag_system_armed = state.flag_system_armed;
|
||||
|
||||
perf_end(mc_loop_perf);
|
||||
} /* end of poll call for attitude updates */
|
||||
} /* end of poll return value check */
|
||||
}
|
||||
|
||||
printf("[multirotor att control] stopping, disarming motors.\n");
|
||||
|
||||
/* 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(state_sub);
|
||||
close(manual_sub);
|
||||
close(actuator_pub);
|
||||
close(att_sp_pub);
|
||||
|
||||
perf_print_counter(mc_loop_perf);
|
||||
perf_free(mc_loop_perf);
|
||||
|
||||
fflush(stdout);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
|
||||
fprintf(stderr, " <mode> is 'rates' or 'attitude'\n");
|
||||
fprintf(stderr, " -t enables motor test mode with 10%% thrust\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int multirotor_att_control_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
unsigned int optioncount = 0;
|
||||
|
||||
while ((ch = getopt(argc, argv, "tm:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 't':
|
||||
motor_test_mode = true;
|
||||
optioncount += 1;
|
||||
break;
|
||||
|
||||
case ':':
|
||||
usage("missing parameter");
|
||||
break;
|
||||
|
||||
default:
|
||||
fprintf(stderr, "option: -%c\n", ch);
|
||||
usage("unrecognized option");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
argc -= optioncount;
|
||||
//argv += optioncount;
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1 + optioncount], "start")) {
|
||||
|
||||
thread_should_exit = false;
|
||||
mc_task = task_spawn("multirotor_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
2048,
|
||||
mc_thread_main,
|
||||
NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1 + optioncount], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @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
|
||||
* 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 multirotor_attitude_control.c
|
||||
* Implementation of attitude controller
|
||||
*/
|
||||
|
||||
#include "multirotor_attitude_control.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
|
||||
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
|
||||
|
||||
struct mc_att_control_params {
|
||||
float yaw_p;
|
||||
float yaw_i;
|
||||
float yaw_d;
|
||||
//float yaw_awu;
|
||||
//float yaw_lim;
|
||||
|
||||
float att_p;
|
||||
float att_i;
|
||||
float att_d;
|
||||
//float att_awu;
|
||||
//float att_lim;
|
||||
|
||||
//float att_xoff;
|
||||
//float att_yoff;
|
||||
};
|
||||
|
||||
struct mc_att_control_param_handles {
|
||||
param_t yaw_p;
|
||||
param_t yaw_i;
|
||||
param_t yaw_d;
|
||||
//param_t yaw_awu;
|
||||
//param_t yaw_lim;
|
||||
|
||||
param_t att_p;
|
||||
param_t att_i;
|
||||
param_t att_d;
|
||||
//param_t att_awu;
|
||||
//param_t att_lim;
|
||||
|
||||
//param_t att_xoff;
|
||||
//param_t att_yoff;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
static int parameters_init(struct mc_att_control_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p);
|
||||
|
||||
|
||||
static int parameters_init(struct mc_att_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->yaw_p = param_find("MC_YAWPOS_P");
|
||||
h->yaw_i = param_find("MC_YAWPOS_I");
|
||||
h->yaw_d = param_find("MC_YAWPOS_D");
|
||||
//h->yaw_awu = param_find("MC_YAWPOS_AWU");
|
||||
//h->yaw_lim = param_find("MC_YAWPOS_LIM");
|
||||
|
||||
h->att_p = param_find("MC_ATT_P");
|
||||
h->att_i = param_find("MC_ATT_I");
|
||||
h->att_d = param_find("MC_ATT_D");
|
||||
//h->att_awu = param_find("MC_ATT_AWU");
|
||||
//h->att_lim = param_find("MC_ATT_LIM");
|
||||
|
||||
//h->att_xoff = param_find("MC_ATT_XOFF");
|
||||
//h->att_yoff = param_find("MC_ATT_YOFF");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
|
||||
{
|
||||
param_get(h->yaw_p, &(p->yaw_p));
|
||||
param_get(h->yaw_i, &(p->yaw_i));
|
||||
param_get(h->yaw_d, &(p->yaw_d));
|
||||
//param_get(h->yaw_awu, &(p->yaw_awu));
|
||||
//param_get(h->yaw_lim, &(p->yaw_lim));
|
||||
|
||||
param_get(h->att_p, &(p->att_p));
|
||||
param_get(h->att_i, &(p->att_i));
|
||||
param_get(h->att_d, &(p->att_d));
|
||||
//param_get(h->att_awu, &(p->att_awu));
|
||||
//param_get(h->att_lim, &(p->att_lim));
|
||||
|
||||
//param_get(h->att_xoff, &(p->att_xoff));
|
||||
//param_get(h->att_yoff, &(p->att_yoff));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
|
||||
{
|
||||
static uint64_t last_run = 0;
|
||||
static uint64_t last_input = 0;
|
||||
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
if (last_input != att_sp->timestamp) {
|
||||
last_input = att_sp->timestamp;
|
||||
}
|
||||
|
||||
static int sensor_delay;
|
||||
sensor_delay = hrt_absolute_time() - att->timestamp;
|
||||
|
||||
static int motor_skip_counter = 0;
|
||||
|
||||
static PID_t pitch_controller;
|
||||
static PID_t roll_controller;
|
||||
|
||||
static struct mc_att_control_params p;
|
||||
static struct mc_att_control_param_handles h;
|
||||
|
||||
static bool initialized = false;
|
||||
|
||||
static float yaw_error;
|
||||
|
||||
/* initialize the pid controllers when the function is called for the first time */
|
||||
if (initialized == false) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
|
||||
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
if (motor_skip_counter % 500 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
|
||||
/* apply parameters */
|
||||
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
|
||||
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
|
||||
}
|
||||
|
||||
/* reset integral if on ground */
|
||||
if (att_sp->thrust < 0.1f) {
|
||||
pid_reset_integral(&pitch_controller);
|
||||
pid_reset_integral(&roll_controller);
|
||||
}
|
||||
|
||||
|
||||
/* calculate current control outputs */
|
||||
|
||||
/* control pitch (forward) output */
|
||||
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
|
||||
att->pitch, att->pitchspeed, deltaT);
|
||||
|
||||
/* control roll (left/right) output */
|
||||
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
|
||||
att->roll, att->rollspeed, deltaT);
|
||||
|
||||
if (control_yaw_position) {
|
||||
/* control yaw rate */
|
||||
|
||||
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
|
||||
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
|
||||
|
||||
yaw_error = att_sp->yaw_body - att->yaw;
|
||||
|
||||
if (yaw_error > M_PI_F) {
|
||||
yaw_error -= M_TWOPI_F;
|
||||
|
||||
} else if (yaw_error < -M_PI_F) {
|
||||
yaw_error += M_TWOPI_F;
|
||||
}
|
||||
|
||||
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
|
||||
}
|
||||
|
||||
rates_sp->thrust = att_sp->thrust;
|
||||
|
||||
motor_skip_counter++;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @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
|
||||
* 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 multirotor_attitude_control.h
|
||||
* Attitude control for multi rotors.
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
|
||||
#define MULTIROTOR_ATTITUDE_CONTROL_H_
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
|
||||
|
||||
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
|
||||
@@ -0,0 +1,230 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@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 multirotor_rate_control.c
|
||||
*
|
||||
* Implementation of rate controller
|
||||
*
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include "multirotor_rate_control.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
|
||||
|
||||
struct mc_rate_control_params {
|
||||
|
||||
float yawrate_p;
|
||||
float yawrate_d;
|
||||
float yawrate_i;
|
||||
//float yawrate_awu;
|
||||
//float yawrate_lim;
|
||||
|
||||
float attrate_p;
|
||||
float attrate_d;
|
||||
float attrate_i;
|
||||
//float attrate_awu;
|
||||
//float attrate_lim;
|
||||
|
||||
float rate_lim;
|
||||
};
|
||||
|
||||
struct mc_rate_control_param_handles {
|
||||
|
||||
param_t yawrate_p;
|
||||
param_t yawrate_i;
|
||||
param_t yawrate_d;
|
||||
//param_t yawrate_awu;
|
||||
//param_t yawrate_lim;
|
||||
|
||||
param_t attrate_p;
|
||||
param_t attrate_i;
|
||||
param_t attrate_d;
|
||||
//param_t attrate_awu;
|
||||
//param_t attrate_lim;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
static int parameters_init(struct mc_rate_control_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p);
|
||||
|
||||
|
||||
static int parameters_init(struct mc_rate_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->yawrate_p = param_find("MC_YAWRATE_P");
|
||||
h->yawrate_i = param_find("MC_YAWRATE_I");
|
||||
h->yawrate_d = param_find("MC_YAWRATE_D");
|
||||
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
|
||||
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
|
||||
|
||||
h->attrate_p = param_find("MC_ATTRATE_P");
|
||||
h->attrate_i = param_find("MC_ATTRATE_I");
|
||||
h->attrate_d = param_find("MC_ATTRATE_D");
|
||||
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
|
||||
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p)
|
||||
{
|
||||
param_get(h->yawrate_p, &(p->yawrate_p));
|
||||
param_get(h->yawrate_i, &(p->yawrate_i));
|
||||
param_get(h->yawrate_d, &(p->yawrate_d));
|
||||
//param_get(h->yawrate_awu, &(p->yawrate_awu));
|
||||
//param_get(h->yawrate_lim, &(p->yawrate_lim));
|
||||
|
||||
param_get(h->attrate_p, &(p->attrate_p));
|
||||
param_get(h->attrate_i, &(p->attrate_i));
|
||||
param_get(h->attrate_d, &(p->attrate_d));
|
||||
//param_get(h->attrate_awu, &(p->attrate_awu));
|
||||
//param_get(h->attrate_lim, &(p->attrate_lim));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[], struct actuator_controls_s *actuators)
|
||||
{
|
||||
static float roll_control_last = 0;
|
||||
static float pitch_control_last = 0;
|
||||
static uint64_t last_run = 0;
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
static uint64_t last_input = 0;
|
||||
|
||||
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
|
||||
|
||||
if (last_input != rate_sp->timestamp) {
|
||||
last_input = rate_sp->timestamp;
|
||||
}
|
||||
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
static int motor_skip_counter = 0;
|
||||
|
||||
static struct mc_rate_control_params p;
|
||||
static struct mc_rate_control_param_handles h;
|
||||
|
||||
static bool initialized = false;
|
||||
|
||||
/* initialize the pid controllers when the function is called for the first time */
|
||||
if (initialized == false) {
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
if (motor_skip_counter % 2500 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
|
||||
// (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
|
||||
}
|
||||
|
||||
/* calculate current control outputs */
|
||||
|
||||
/* control pitch (forward) output */
|
||||
float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(pitch_control)) {
|
||||
pitch_control_last = pitch_control;
|
||||
|
||||
} else {
|
||||
pitch_control = 0.0f;
|
||||
warnx("rej. NaN ctrl pitch");
|
||||
}
|
||||
|
||||
/* control roll (left/right) output */
|
||||
float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(roll_control)) {
|
||||
roll_control_last = roll_control;
|
||||
|
||||
} else {
|
||||
roll_control = 0.0f;
|
||||
warnx("rej. NaN ctrl roll");
|
||||
}
|
||||
|
||||
/* control yaw rate */
|
||||
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (!isfinite(yaw_rate_control)) {
|
||||
yaw_rate_control = 0.0f;
|
||||
warnx("rej. NaN ctrl yaw");
|
||||
}
|
||||
|
||||
actuators->control[0] = roll_control;
|
||||
actuators->control[1] = pitch_control;
|
||||
actuators->control[2] = yaw_rate_control;
|
||||
actuators->control[3] = rate_sp->thrust;
|
||||
|
||||
motor_skip_counter++;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @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
|
||||
* 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 multirotor_attitude_control.h
|
||||
* Attitude control for multi rotors.
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_RATE_CONTROL_H_
|
||||
#define MULTIROTOR_RATE_CONTROL_H_
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float rates[], struct actuator_controls_s *actuators);
|
||||
|
||||
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
|
||||
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Build multirotor position control
|
||||
#
|
||||
|
||||
MODULE_COMMAND = multirotor_pos_control
|
||||
|
||||
SRCS = multirotor_pos_control.c \
|
||||
multirotor_pos_control_params.c
|
||||
@@ -0,0 +1,238 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 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
|
||||
* 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 multirotor_pos_control.c
|
||||
*
|
||||
* Skeleton for multirotor position controller
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
#include <termios.h>
|
||||
#include <time.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "multirotor_pos_control_params.h"
|
||||
|
||||
|
||||
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 */
|
||||
|
||||
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of position controller.
|
||||
*/
|
||||
static int multirotor_pos_control_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);
|
||||
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\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_spawn().
|
||||
*/
|
||||
int multirotor_pos_control_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("multirotor pos control already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("multirotor pos control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 60,
|
||||
4096,
|
||||
multirotor_pos_control_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) {
|
||||
printf("\tmultirotor pos control app is running\n");
|
||||
} else {
|
||||
printf("\tmultirotor pos control app not started\n");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
static int
|
||||
multirotor_pos_control_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* welcome user */
|
||||
printf("[multirotor pos control] Control started, taking over position control\n");
|
||||
|
||||
/* structures */
|
||||
struct vehicle_status_s state;
|
||||
struct vehicle_attitude_s att;
|
||||
//struct vehicle_global_position_setpoint_s global_pos_sp;
|
||||
struct vehicle_local_position_setpoint_s local_pos_sp;
|
||||
struct vehicle_vicon_position_s local_pos;
|
||||
struct manual_control_setpoint_s manual;
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
|
||||
//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
|
||||
|
||||
/* publish attitude setpoint */
|
||||
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
|
||||
thread_running = true;
|
||||
|
||||
int loopcounter = 0;
|
||||
|
||||
struct multirotor_position_control_params p;
|
||||
struct multirotor_position_control_param_handles h;
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
|
||||
|
||||
while (1) {
|
||||
/* get a local copy of the vehicle state */
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
/* get a local copy of manual setpoint */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
/* get a local copy of local position */
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
|
||||
/* get a local copy of local position setpoint */
|
||||
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
|
||||
|
||||
if (loopcounter == 500) {
|
||||
parameters_update(&h, &p);
|
||||
loopcounter = 0;
|
||||
}
|
||||
|
||||
// if (state.state_machine == SYSTEM_STATE_AUTO) {
|
||||
|
||||
// XXX IMPLEMENT POSITION CONTROL HERE
|
||||
|
||||
float dT = 1.0f / 50.0f;
|
||||
|
||||
float x_setpoint = 0.0f;
|
||||
|
||||
// XXX enable switching between Vicon and local position estimate
|
||||
/* local pos is the Vicon position */
|
||||
|
||||
// XXX just an example, lacks rotation around world-body transformation
|
||||
att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
|
||||
att_sp.roll_body = 0.0f;
|
||||
att_sp.yaw_body = 0.0f;
|
||||
att_sp.thrust = 0.3f;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* publish new attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
/* set setpoint to current position */
|
||||
// XXX select pos reset channel on remote
|
||||
/* reset setpoint to current position (position hold) */
|
||||
// if (1 == 2) {
|
||||
// local_pos_sp.x = local_pos.x;
|
||||
// local_pos_sp.y = local_pos.y;
|
||||
// local_pos_sp.z = local_pos.z;
|
||||
// local_pos_sp.yaw = att.yaw;
|
||||
// }
|
||||
// }
|
||||
|
||||
/* run at approximately 50 Hz */
|
||||
usleep(20000);
|
||||
loopcounter++;
|
||||
|
||||
}
|
||||
|
||||
printf("[multirotor pos control] ending now...\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
fflush(stdout);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@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 multirotor_position_control_params.c
|
||||
*
|
||||
* Parameters for EKF filter
|
||||
*/
|
||||
|
||||
#include "multirotor_pos_control_params.h"
|
||||
|
||||
/* Extended Kalman Filter covariances */
|
||||
|
||||
/* controller parameters */
|
||||
PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
|
||||
|
||||
int parameters_init(struct multirotor_position_control_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->p = param_find("MC_POS_P");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
|
||||
{
|
||||
param_get(h->p, &(p->p));
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@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 multirotor_position_control_params.h
|
||||
*
|
||||
* Parameters for position controller
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct multirotor_position_control_params {
|
||||
float p;
|
||||
float i;
|
||||
float d;
|
||||
};
|
||||
|
||||
struct multirotor_position_control_param_handles {
|
||||
param_t p;
|
||||
param_t i;
|
||||
param_t d;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct multirotor_position_control_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
|
||||
@@ -0,0 +1,235 @@
|
||||
// /****************************************************************************
|
||||
// *
|
||||
// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
// * Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
// * @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
// * @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
// * @author Martin Rutschmann <rutmarti@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 multirotor_position_control.c
|
||||
// * Implementation of the position control for a multirotor VTOL
|
||||
// */
|
||||
|
||||
// #include <stdio.h>
|
||||
// #include <stdlib.h>
|
||||
// #include <stdio.h>
|
||||
// #include <stdint.h>
|
||||
// #include <math.h>
|
||||
// #include <stdbool.h>
|
||||
// #include <float.h>
|
||||
// #include <systemlib/pid/pid.h>
|
||||
|
||||
// #include "multirotor_position_control.h"
|
||||
|
||||
// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
|
||||
// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
|
||||
// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
|
||||
// {
|
||||
// static PID_t distance_controller;
|
||||
|
||||
// static int read_ret;
|
||||
// static global_data_position_t position_estimated;
|
||||
|
||||
// static uint16_t counter;
|
||||
|
||||
// static bool initialized;
|
||||
// static uint16_t pm_counter;
|
||||
|
||||
// static float lat_next;
|
||||
// static float lon_next;
|
||||
|
||||
// static float pitch_current;
|
||||
|
||||
// static float thrust_total;
|
||||
|
||||
|
||||
// if (initialized == false) {
|
||||
|
||||
// pid_init(&distance_controller,
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
|
||||
// PID_MODE_DERIVATIV_CALC, 150);//150
|
||||
|
||||
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
|
||||
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
|
||||
|
||||
// thrust_total = 0.0f;
|
||||
|
||||
// /* Position initialization */
|
||||
// /* Wait for new position estimate */
|
||||
// do {
|
||||
// read_ret = read_lock_position(&position_estimated);
|
||||
// } while (read_ret != 0);
|
||||
|
||||
// lat_next = position_estimated.lat;
|
||||
// lon_next = position_estimated.lon;
|
||||
|
||||
// /* attitude initialization */
|
||||
// global_data_lock(&global_data_attitude->access_conf);
|
||||
// pitch_current = global_data_attitude->pitch;
|
||||
// global_data_unlock(&global_data_attitude->access_conf);
|
||||
|
||||
// initialized = true;
|
||||
// }
|
||||
|
||||
// /* load new parameters with 10Hz */
|
||||
// if (counter % 50 == 0) {
|
||||
// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
|
||||
// /* check whether new parameters are available */
|
||||
// if (global_data_parameter_storage->counter > pm_counter) {
|
||||
// pid_set_parameters(&distance_controller,
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
|
||||
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
|
||||
|
||||
// //
|
||||
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
|
||||
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
|
||||
|
||||
// pm_counter = global_data_parameter_storage->counter;
|
||||
// printf("Position controller changed pid parameters\n");
|
||||
// }
|
||||
// }
|
||||
|
||||
// global_data_unlock(&global_data_parameter_storage->access_conf);
|
||||
// }
|
||||
|
||||
|
||||
// /* Wait for new position estimate */
|
||||
// do {
|
||||
// read_ret = read_lock_position(&position_estimated);
|
||||
// } while (read_ret != 0);
|
||||
|
||||
// /* Get next waypoint */ //TODO: add local copy
|
||||
|
||||
// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
|
||||
// lat_next = global_data_position_setpoint->x;
|
||||
// lon_next = global_data_position_setpoint->y;
|
||||
// global_data_unlock(&global_data_position_setpoint->access_conf);
|
||||
// }
|
||||
|
||||
// /* Get distance to waypoint */
|
||||
// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
|
||||
// // if(counter % 5 == 0)
|
||||
// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
|
||||
|
||||
// /* Get bearing to waypoint (direction on earth surface to next waypoint) */
|
||||
// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
|
||||
|
||||
// if (counter % 5 == 0)
|
||||
// printf("bearing: %.4f\n", bearing);
|
||||
|
||||
// /* Calculate speed in direction of bearing (needed for controller) */
|
||||
// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
|
||||
// // if(counter % 5 == 0)
|
||||
// // printf("speed_norm: %.4f\n", speed_norm);
|
||||
// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
|
||||
|
||||
// /* Control Thrust in bearing direction */
|
||||
// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
|
||||
// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
|
||||
|
||||
// // if(counter % 5 == 0)
|
||||
// // printf("horizontal thrust: %.4f\n", horizontal_thrust);
|
||||
|
||||
// /* Get total thrust (from remote for now) */
|
||||
// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
|
||||
// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
|
||||
// global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
// }
|
||||
|
||||
// const float max_gas = 500.0f;
|
||||
// thrust_total *= max_gas / 20000.0f; //TODO: check this
|
||||
// thrust_total += max_gas / 2.0f;
|
||||
|
||||
|
||||
// if (horizontal_thrust > thrust_total) {
|
||||
// horizontal_thrust = thrust_total;
|
||||
|
||||
// } else if (horizontal_thrust < -thrust_total) {
|
||||
// horizontal_thrust = -thrust_total;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// //TODO: maybe we want to add a speed controller later...
|
||||
|
||||
// /* Calclulate thrust in east and north direction */
|
||||
// float thrust_north = cosf(bearing) * horizontal_thrust;
|
||||
// float thrust_east = sinf(bearing) * horizontal_thrust;
|
||||
|
||||
// if (counter % 10 == 0) {
|
||||
// printf("thrust north: %.4f\n", thrust_north);
|
||||
// printf("thrust east: %.4f\n", thrust_east);
|
||||
// fflush(stdout);
|
||||
// }
|
||||
|
||||
// /* Get current attitude */
|
||||
// if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
|
||||
// pitch_current = global_data_attitude->pitch;
|
||||
// global_data_unlock(&global_data_attitude->access_conf);
|
||||
// }
|
||||
|
||||
// /* Get desired pitch & roll */
|
||||
// float pitch_desired = 0.0f;
|
||||
// float roll_desired = 0.0f;
|
||||
|
||||
// if (thrust_total != 0) {
|
||||
// float pitch_fraction = -thrust_north / thrust_total;
|
||||
// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
|
||||
|
||||
// if (roll_fraction < -1) {
|
||||
// roll_fraction = -1;
|
||||
|
||||
// } else if (roll_fraction > 1) {
|
||||
// roll_fraction = 1;
|
||||
// }
|
||||
|
||||
// // if(counter % 5 == 0)
|
||||
// // {
|
||||
// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
|
||||
// // fflush(stdout);
|
||||
// // }
|
||||
|
||||
// pitch_desired = asinf(pitch_fraction);
|
||||
// roll_desired = asinf(roll_fraction);
|
||||
// }
|
||||
|
||||
// att_sp.roll = roll_desired;
|
||||
// att_sp.pitch = pitch_desired;
|
||||
|
||||
// counter++;
|
||||
// }
|
||||
@@ -0,0 +1,50 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@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 multirotor_position_control.h
|
||||
* Definition of the position control for a multirotor VTOL
|
||||
*/
|
||||
|
||||
// #ifndef POSITION_CONTROL_H_
|
||||
// #define POSITION_CONTROL_H_
|
||||
|
||||
// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
|
||||
// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
|
||||
// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
|
||||
|
||||
// #endif /* POSITION_CONTROL_H_ */
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
@@ -0,0 +1,423 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user