aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--.gitignore5
-rw-r--r--apps/examples/control_demo/control_demo.cpp2
-rw-r--r--apps/examples/control_demo/params.c42
-rw-r--r--apps/examples/kalman_demo/KalmanNav.cpp393
-rw-r--r--apps/examples/kalman_demo/KalmanNav.hpp17
-rw-r--r--apps/examples/kalman_demo/params.c10
-rw-r--r--apps/mathlib/math/Dcm.cpp77
-rw-r--r--apps/mathlib/math/Dcm.hpp7
-rw-r--r--apps/mathlib/math/EulerAngles.cpp26
-rw-r--r--apps/mathlib/math/Quaternion.cpp59
-rw-r--r--apps/mathlib/math/nasa_rotation_def.pdfbin0 -> 709235 bytes
-rw-r--r--apps/mathlib/math/test_math.sce63
-rw-r--r--apps/mavlink/mavlink.c4
-rw-r--r--apps/mavlink/mavlink_hil.h6
-rw-r--r--apps/mavlink/mavlink_receiver.c184
-rw-r--r--apps/uORB/topics/sensor_combined.h1
16 files changed, 629 insertions, 267 deletions
diff --git a/.gitignore b/.gitignore
index f072889f0..0861e7a52 100644
--- a/.gitignore
+++ b/.gitignore
@@ -8,6 +8,7 @@
apps/namedapp/namedapp_list.h
apps/namedapp/namedapp_proto.h
Make.dep
+*.pyc
*.o
*.a
*.d
@@ -43,3 +44,7 @@ cscope.out
.configX-e
nuttx-export.zip
dot.gdbinit
+mavlink/include/mavlink/v0.9/
+.*.swp
+.swp
+core
diff --git a/apps/examples/control_demo/control_demo.cpp b/apps/examples/control_demo/control_demo.cpp
index d9c773c05..e609f2f4b 100644
--- a/apps/examples/control_demo/control_demo.cpp
+++ b/apps/examples/control_demo/control_demo.cpp
@@ -108,7 +108,7 @@ int control_demo_main(int argc, char *argv[])
deamon_task = task_spawn("control_demo",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
- 4096,
+ 5120,
control_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 4eec456fb..8f923f5a1 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -6,31 +6,31 @@
// 16 is max name length
// gyro low pass filter
-PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
-PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
-PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
+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.1f); // 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
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
// psi -> phi -> p
-PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f); // heading 2 roll
-PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f); // roll to roll rate
-PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f); // roll limit
+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.5f); // roll limit, 28 deg
// velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
// theta -> q
@@ -41,15 +41,15 @@ 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.005f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
+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.0f);
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
// speed command
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
@@ -58,6 +58,6 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index c2faa89de..742bfc9c1 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -42,10 +42,12 @@
#include "KalmanNav.hpp"
// constants
+// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
-static const float R = 6.371000e6f; // earth radius, m
-static const float RSq = 4.0589641e13f; // radius squared
-static const float g = 9.8f; // gravitational accel. m/s^2
+static const float R0 = 6378137.0f; // earth radius, m
+
+static const float RSq = 4.0680631591e+13; // radius squared
+static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
@@ -57,15 +59,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// attitude measurement ekf matrices
HAtt(6, 9),
RAtt(6, 6),
- // gps measurement ekf matrices
- HGps(6, 9),
- RGps(6, 6),
+ // position measurement ekf matrices
+ HPos(5, 9),
+ RPos(5, 5),
// attitude representations
C_nb(),
q(),
// subscriptions
- _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
- _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
+ _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 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)),
@@ -78,6 +80,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
+ // miss counts
+ _missFast(0),
+ _missSlow(0),
// state
fN(0), fE(0), fD(0),
phi(0), theta(0), psi(0),
@@ -87,8 +92,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
- _rGpsV(this, "R_GPS_V"),
- _rGpsGeo(this, "R_GPS_GEO"),
+ _rGpsVel(this, "R_GPS_VEL"),
+ _rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL")
{
@@ -99,12 +104,21 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// wait for gps lock
while (1) {
- updateSubscriptions();
+ struct pollfd fds[1];
+ fds[0].fd = _gps.getHandle();
+ fds[0].events = POLLIN;
+
+ // poll 10 seconds for new data
+ int ret = poll(fds, 1, 10000);
- if (_gps.fix_type > 2) break;
+ if (ret > 0) {
+ updateSubscriptions();
- printf("[kalman_demo] waiting for gps lock\n");
- usleep(1000000);
+ if (_gps.fix_type > 2) break;
+
+ } else if (ret == 0) {
+ printf("[kalman_demo] waiting for gps lock\n");
+ }
}
// initial state
@@ -124,16 +138,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// initialize dcm
C_nb = Dcm(q);
- // initialize F to identity
- F = Matrix::identity(9);
-
- // HGps is constant
- HGps(0, 3) = 1.0f;
- HGps(1, 4) = 1.0f;
- HGps(2, 5) = 1.0f;
- HGps(3, 6) = 1.0f;
- HGps(4, 7) = 1.0f;
- HGps(5, 8) = 1.0f;
+ // HPos is constant
+ HPos(0, 3) = 1.0f;
+ HPos(1, 4) = 1.0f;
+ HPos(2, 6) = 1.0f;
+ HPos(3, 7) = 1.0f;
+ HPos(4, 8) = 1.0f;
// initialize all parameters
updateParams();
@@ -143,11 +153,13 @@ void KalmanNav::update()
{
using namespace math;
- struct pollfd fds[2];
+ struct pollfd fds[3];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
fds[1].fd = _param_update.getHandle();
fds[1].events = POLLIN;
+ fds[2].fd = _gps.getHandle();
+ fds[2].events = POLLIN;
// poll 20 milliseconds for new data
int ret = poll(fds, 2, 20);
@@ -158,53 +170,55 @@ void KalmanNav::update()
return;
} else if (ret == 0) { // timeout
- // run anyway
- } else if (ret > 0) {
- // update params when requested
- if (fds[1].revents & POLLIN) {
- printf("updating params\n");
- updateParams();
- }
-
- // if no new sensor data, return
- if (!(fds[0].revents & POLLIN)) return;
+ 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();
- // count fast frames
- _navFrames += 1;
+ // abort update if no new data
+ if (!(sensorsUpdate || gpsUpdate)) return;
// fast prediciton step
// note, using sensors timestamp so we can account
// for packet lag
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
_fastTimeStamp = _sensors.timestamp;
- predictFast(dtFast);
+
+ if (dtFast < 1.0f / 50) {
+ predictFast(dtFast);
+ // count fast frames
+ _navFrames += 1;
+
+ } else _missFast++;
// slow prediction step
float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
- if (dtSlow > 1.0f / 200) { // 200 Hz
+ if (dtSlow > 1.0f / 100) { // 100 Hz
_slowTimeStamp = _sensors.timestamp;
- predictSlow(dtSlow);
+
+ if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
+ else _missSlow ++;
}
// gps correction step
if (gpsUpdate) {
- correctGps();
+ correctPos();
}
// attitude correction step
- if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
+ if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
@@ -218,8 +232,10 @@ void KalmanNav::update()
// output
if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
_outTimeStamp = newTimeStamp;
- printf("nav: %4d Hz\n", _navFrames);
+ printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
_navFrames = 0;
+ _missFast = 0;
+ _missSlow = 0;
}
}
@@ -298,21 +314,28 @@ void KalmanNav::predictFast(float dt)
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
+ float cosLSing = cosf(lat);
+
+ if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
// position update
// neglects angular deflections in local gravity
// see Titerton pg. 70
- float LDot = vN / (R + float(alt));
- float lDot = vE / (cosL * (R + float(alt)));
- float vNDot = fN - vE * (2 * omega +
- lDot) * sinL +
+ float R = R0 + float(alt);
+ float LDot = vN / R;
+ float lDot = vE / (cosLSing * R);
+ float rotRate = 2 * omega + lDot;
+ float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
- float vDDot = fD - vE * (2 * omega + lDot) * cosL -
+ float vDDot = fD - vE * rotRate * cosL -
vN * LDot + g;
- float vEDot = fE + vN * (2 * omega + lDot) * sinL +
- vDDot * (2 * omega * cosL);
+ float vEDot = fE + vN * rotRate * sinL +
+ vDDot * rotRate * cosL;
// rectangular integration
+ //printf("dt: %8.4f\n", double(dt));
+ //printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
+ //printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
@@ -331,87 +354,88 @@ void KalmanNav::predictSlow(float dt)
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
+ // prepare for matrix
+ float R = R0 + float(alt);
+
// F Matrix
// Titterton pg. 291
- //
- // difference from Jacobian
- // multiplity by dt for all elements
- // add 1.0 to diagonal elements
-
- F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
- F(0, 2) = (vN / R) * dt;
- F(0, 4) = (1.0f / R) * dt;
- F(0, 6) = (-omega * sinL) * dt;
- F(0, 8) = (-vE / RSq) * dt;
-
- F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
- F(1, 2) = (omega * cosL + vE / R) * dt;
- F(1, 3) = (-1.0f / R) * dt;
- F(1, 8) = (vN / RSq) * dt;
-
- F(2, 0) = (-vN / R) * dt;
- F(2, 1) = (-omega * cosL - vE / R) * dt;
- F(2, 4) = (-tanL / R) * dt;
- F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
- F(2, 8) = (vE * tanL / RSq) * dt;
-
- F(3, 1) = (-fD) * dt;
- F(3, 2) = (fE) * dt;
- F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
- F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
- F(3, 5) = (vN / R) * dt;
- F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
- F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
-
- F(4, 0) = (fD) * dt;
- F(4, 2) = (-fN) * dt;
- F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
- F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
- F(4, 5) = (2 * omega * cosL + vE / R) * dt;
- F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
- vN * vE / (R * cosLSq)) * dt;
- F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
-
- F(5, 0) = (-fE) * dt;
- F(5, 1) = (fN) * dt;
- F(5, 3) = (-2 * vN / R) * dt;
- F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
- F(5, 6) = (2 * omega * vE * sinL) * dt;
- F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
-
- F(6, 3) = (1 / R) * dt;
- F(6, 8) = (-vN / RSq) * dt;
-
- F(7, 4) = (1 / (R * cosL)) * dt;
- F(7, 6) = (vE * tanL / (R * cosL)) * dt;
- F(7, 8) = (-vE / (cosL * RSq)) * dt;
-
- F(8, 5) = (-1) * dt;
+
+ 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) * dt;
- G(0, 1) = -C_nb(0, 1) * dt;
- G(0, 2) = -C_nb(0, 2) * dt;
- G(1, 0) = -C_nb(1, 0) * dt;
- G(1, 1) = -C_nb(1, 1) * dt;
- G(1, 2) = -C_nb(1, 2) * dt;
- G(2, 0) = -C_nb(2, 0) * dt;
- G(2, 1) = -C_nb(2, 1) * dt;
- G(2, 2) = -C_nb(2, 2) * dt;
-
- G(3, 3) = C_nb(0, 0) * dt;
- G(3, 4) = C_nb(0, 1) * dt;
- G(3, 5) = C_nb(0, 2) * dt;
- G(4, 3) = C_nb(1, 0) * dt;
- G(4, 4) = C_nb(1, 1) * dt;
- G(4, 5) = C_nb(1, 2) * dt;
- G(5, 3) = C_nb(2, 0) * dt;
- G(5, 4) = C_nb(2, 1) * dt;
- G(5, 5) = C_nb(2, 2) * dt;
-
- // predict equations for kalman filter
- P = F * P * F.transpose() + G * V * G.transpose();
+ 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;
}
void KalmanNav::correctAtt()
@@ -428,12 +452,11 @@ void KalmanNav::correctAtt()
// mag measurement
Vector3 zMag(_sensors.magnetometer_ga);
- zMag = zMag.unit();
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
- static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
+ static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level
static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
float bN = cosf(dip) * cosf(dec);
float bE = cosf(dip) * sinf(dec);
@@ -443,10 +466,29 @@ void KalmanNav::correctAtt()
// 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) > 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");
+ }
+
+ // account for banked turn
+ // this would only work for fixed wing, so try to avoid
+ //Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi);
+
// accel predicted measurement
- Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit();
+ Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit();
// combined measurement
Vector zAtt(6);
@@ -498,8 +540,9 @@ void KalmanNav::correctAtt()
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() + RAtt; // residual covariance
+ Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
@@ -510,24 +553,34 @@ void KalmanNav::correctAtt()
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in att correction\n");
+ // reset P matrix to identity
+ P = Matrix::identity(9) * 1.0f;
return;
}
}
// correct state
- phi += xCorrect(PHI);
- theta += xCorrect(THETA);
+ if (!ignoreAccel) {
+ phi += xCorrect(PHI);
+ theta += xCorrect(THETA);
+ }
+
psi += xCorrect(PSI);
+ // attitude also affects nav velocities
+ 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);
- printf("attitude: beta = %8.4f\n", (double)beta);
if (beta > 10.0f) {
- //printf("fault in attitude: beta = %8.4f\n", (double)beta);
+ printf("fault in attitude: beta = %8.4f\n", (double)beta);
//printf("y:\n"); y.print();
}
@@ -536,20 +589,32 @@ void KalmanNav::correctAtt()
q = Quaternion(EulerAngles(phi, theta, psi));
}
-void KalmanNav::correctGps()
+void KalmanNav::correctPos()
{
using namespace math;
Vector y(6);
y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE;
- y(2) = _gps.vel_d - vD;
- y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
- y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
- y(5) = double(_gps.alt) / 1.0e3 - alt;
+ y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
+ y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
+ y(4) = double(_gps.alt) / 1.0e3 - alt;
+
+ // update gps noise
+ float cosLSing = cosf(lat);
+ float R = R0 + float(alt);
+
+ // prevent singularity
+ if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+
+ float noiseLat = _rGpsPos.get() / R;
+ float noiseLon = _rGpsPos.get() / (R * cosLSing);
+ RPos(2, 2) = noiseLat * noiseLat;
+ RPos(3, 3) = noiseLon * noiseLon;
// compute correction
- Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
- Matrix K = P * HGps.transpose() * S.inverse();
+ // 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
@@ -566,6 +631,8 @@ void KalmanNav::correctGps()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
+ // reset P matrix to identity
+ P = Matrix::identity(9) * 1.0f;
return;
}
}
@@ -579,14 +646,14 @@ void KalmanNav::correctGps()
alt += double(xCorrect(ALT));
// update state covariance
- P = P - K * HGps * P;
+ // http://en.wikipedia.org/wiki/Extended_Kalman_filter
+ P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse() * y);
- printf("gps: beta = %8.4f\n", (double)beta);
- if (beta > 100.0f) {
- //printf("fault in gps: beta = %8.4f\n", (double)beta);
+ if (beta > 10.0f) {
+ printf("fault in gps: beta = %8.4f\n", (double)beta);
//printf("y:\n"); y.print();
}
}
@@ -597,6 +664,7 @@ void KalmanNav::updateParams()
using namespace control;
SuperBlock::updateParams();
+
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
@@ -608,20 +676,31 @@ void KalmanNav::updateParams()
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
- RAtt(0, 0) = _rMag.get(); // normalized direction
- RAtt(1, 1) = _rMag.get();
- RAtt(2, 2) = _rMag.get();
+ float noiseMagSq = _rMag.get() * _rMag.get();
+ RAtt(0, 0) = noiseMagSq; // normalized direction
+ RAtt(1, 1) = noiseMagSq;
+ RAtt(2, 2) = noiseMagSq;
// accelerometer noise
- RAtt(3, 3) = _rAccel.get(); // normalized direction
- RAtt(4, 4) = _rAccel.get();
- RAtt(5, 5) = _rAccel.get();
+ float noiseAccelSq = _rAccel.get() * _rAccel.get();
+ RAtt(3, 3) = noiseAccelSq; // normalized direction
+ RAtt(4, 4) = noiseAccelSq;
+ RAtt(5, 5) = noiseAccelSq;
// gps noise
- RGps(0, 0) = _rGpsV.get(); // vn, m/s
- RGps(1, 1) = _rGpsV.get(); // ve
- RGps(2, 2) = _rGpsV.get(); // vd
- RGps(3, 3) = _rGpsGeo.get(); // L, rad
- RGps(4, 4) = _rGpsGeo.get(); // l, rad
- RGps(5, 5) = _rGpsAlt.get(); // h, m
+ float cosLSing = cosf(lat);
+ float R = R0 + float(alt);
+
+ // prevent singularity
+ if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+
+ float noiseVel = _rGpsVel.get();
+ float noiseLat = _rGpsPos.get() / R;
+ float noiseLon = _rGpsPos.get() / (R * cosLSing);
+ float noiseAlt = _rGpsAlt.get();
+ RPos(0, 0) = noiseVel * noiseVel; // vn
+ RPos(1, 1) = noiseVel * noiseVel; // ve
+ RPos(2, 2) = noiseLat * noiseLat; // lat
+ RPos(3, 3) = noiseLon * noiseLon; // lon
+ RPos(4, 4) = noiseAlt * noiseAlt; // h
}
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index dc4a81f4a..1ea46d40f 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -60,6 +60,11 @@
#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:
@@ -70,7 +75,7 @@ public:
void predictFast(float dt);
void predictSlow(float dt);
void correctAtt();
- void correctGps();
+ void correctPos();
virtual void updateParams();
protected:
math::Matrix F;
@@ -79,8 +84,8 @@ protected:
math::Matrix V;
math::Matrix HAtt;
math::Matrix RAtt;
- math::Matrix HGps;
- math::Matrix RGps;
+ math::Matrix HPos;
+ math::Matrix RPos;
math::Dcm C_nb;
math::Quaternion q;
control::UOrbSubscription<sensor_combined_s> _sensors;
@@ -94,6 +99,8 @@ protected:
uint64_t _attTimeStamp;
uint64_t _outTimeStamp;
uint16_t _navFrames;
+ uint16_t _missFast;
+ uint16_t _missSlow;
float fN, fE, fD;
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
@@ -103,8 +110,8 @@ protected:
control::BlockParam<float> _vGyro;
control::BlockParam<float> _vAccel;
control::BlockParam<float> _rMag;
- control::BlockParam<float> _rGpsV;
- control::BlockParam<float> _rGpsGeo;
+ control::BlockParam<float> _rGpsVel;
+ control::BlockParam<float> _rGpsPos;
control::BlockParam<float> _rGpsAlt;
control::BlockParam<float> _rAccel;
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 327e2cda6..03cdca5ed 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -3,8 +3,8 @@
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
diff --git a/apps/mathlib/math/Dcm.cpp b/apps/mathlib/math/Dcm.cpp
index 59f3d88ab..467c6f34e 100644
--- a/apps/mathlib/math/Dcm.cpp
+++ b/apps/mathlib/math/Dcm.cpp
@@ -52,6 +52,23 @@ Dcm::Dcm() :
{
}
+Dcm::Dcm(float c00, float c01, float c02,
+ float c10, float c11, float c12,
+ float c20, float c21, float c22) :
+ Matrix(3, 3)
+{
+ Dcm &dcm = *this;
+ dcm(0,0) = c00;
+ dcm(0,1) = c01;
+ dcm(0,2) = c02;
+ dcm(1,0) = c10;
+ dcm(1,1) = c11;
+ dcm(1,2) = c12;
+ dcm(2,0) = c20;
+ dcm(2,1) = c21;
+ dcm(2,2) = c22;
+}
+
Dcm::Dcm(const float *data) :
Matrix(3, 3, data)
{
@@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) :
Matrix(3, 3)
{
Dcm &dcm = *this;
- float a = q.getA();
- float b = q.getB();
- float c = q.getC();
- float d = q.getD();
- float aSq = a * a;
- float bSq = b * b;
- float cSq = c * c;
- float dSq = d * d;
+ double a = q.getA();
+ double b = q.getB();
+ double c = q.getC();
+ double d = q.getD();
+ double aSq = a * a;
+ double bSq = b * b;
+ double cSq = c * c;
+ double dSq = d * d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
- dcm(0, 1) = 2 * (b * c - a * d);
- dcm(0, 2) = 2 * (a * c + b * d);
- dcm(1, 0) = 2 * (b * c + a * d);
+ dcm(0, 1) = 2.0 * (b * c - a * d);
+ dcm(0, 2) = 2.0 * (a * c + b * d);
+ dcm(1, 0) = 2.0 * (b * c + a * d);
dcm(1, 1) = aSq - bSq + cSq - dSq;
- dcm(1, 2) = 2 * (c * d - a * b);
- dcm(2, 0) = 2 * (b * d - a * c);
- dcm(2, 1) = 2 * (a * b + c * d);
+ dcm(1, 2) = 2.0 * (c * d - a * b);
+ dcm(2, 0) = 2.0 * (b * d - a * c);
+ dcm(2, 1) = 2.0 * (a * b + c * d);
dcm(2, 2) = aSq - bSq - cSq + dSq;
}
@@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) :
Matrix(3, 3)
{
Dcm &dcm = *this;
- float cosPhi = cosf(euler.getPhi());
- float sinPhi = sinf(euler.getPhi());
- float cosThe = cosf(euler.getTheta());
- float sinThe = sinf(euler.getTheta());
- float cosPsi = cosf(euler.getPsi());
- float sinPsi = sinf(euler.getPsi());
+ double cosPhi = cos(euler.getPhi());
+ double sinPhi = sin(euler.getPhi());
+ double cosThe = cos(euler.getTheta());
+ double sinThe = sin(euler.getTheta());
+ double cosPsi = cos(euler.getPsi());
+ double sinPsi = sin(euler.getPsi());
dcm(0, 0) = cosThe * cosPsi;
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
@@ -116,11 +133,23 @@ Dcm::~Dcm()
int __EXPORT dcmTest()
{
printf("Test DCM\t\t: ");
- Vector3 vB(1, 2, 3);
- ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
- Matrix::identity(3)));
- ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
+ // default ctor
+ ASSERT(matrixEqual(Dcm(),
Matrix::identity(3)));
+ // quaternion ctor
+ ASSERT(matrixEqual(
+ Dcm(Quaternion(0.983347, 0.034271, 0.106021, 0.143572)),
+ Dcm( 0.9362934, -0.2750958, 0.2183507,
+ 0.2896295, 0.9564251, -0.0369570,
+ -0.1986693, 0.0978434, 0.9751703)));
+ // euler angle ctor
+ ASSERT(matrixEqual(
+ Dcm(EulerAngles(0.1, 0.2, 0.3)),
+ Dcm( 0.9362934, -0.2750958, 0.2183507,
+ 0.2896295, 0.9564251, -0.0369570,
+ -0.1986693, 0.0978434, 0.9751703)));
+ // rotations
+ Vector3 vB(1, 2, 3);
ASSERT(vectorEqual(Vector3(-2, 1, 3),
Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
ASSERT(vectorEqual(Vector3(3, 2, -1),
diff --git a/apps/mathlib/math/Dcm.hpp b/apps/mathlib/math/Dcm.hpp
index de69a7aa4..781933e9e 100644
--- a/apps/mathlib/math/Dcm.hpp
+++ b/apps/mathlib/math/Dcm.hpp
@@ -65,6 +65,13 @@ public:
Dcm();
/**
+ * scalar ctor
+ */
+ Dcm(float c00, float c01, float c02,
+ float c10, float c11, float c12,
+ float c20, float c21, float c22);
+
+ /**
* data ctor
*/
Dcm(const float *data);
diff --git a/apps/mathlib/math/EulerAngles.cpp b/apps/mathlib/math/EulerAngles.cpp
index 8991d5623..27ebbf8b3 100644
--- a/apps/mathlib/math/EulerAngles.cpp
+++ b/apps/mathlib/math/EulerAngles.cpp
@@ -97,23 +97,27 @@ EulerAngles::~EulerAngles()
int __EXPORT eulerAnglesTest()
{
printf("Test EulerAngles\t: ");
- EulerAngles euler(1, 2, 3);
+ EulerAngles euler(0.1, 0.2, 0.3);
// test ctor
- ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
- ASSERT(equal(euler.getPhi(), 1));
- ASSERT(equal(euler.getTheta(), 2));
- ASSERT(equal(euler.getPsi(), 3));
+ ASSERT(vectorEqual(Vector3(0.1, 0.2, 0.3), euler));
+ ASSERT(equal(euler.getPhi(), 0.1));
+ ASSERT(equal(euler.getTheta(), 0.2));
+ ASSERT(equal(euler.getPsi(), 0.3));
// test dcm ctor
+ euler = Dcm(EulerAngles(0.1,0.2,0.3));
+ ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
+
+ // test quat ctor
+ euler = Quaternion(EulerAngles(0.1,0.2,0.3));
+ ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
// test assignment
- euler.setPhi(4);
- ASSERT(equal(euler.getPhi(), 4));
- euler.setTheta(5);
- ASSERT(equal(euler.getTheta(), 5));
- euler.setPsi(6);
- ASSERT(equal(euler.getPsi(), 6));
+ euler.setPhi(0.4);
+ euler.setTheta(0.5);
+ euler.setPsi(0.6);
+ ASSERT(vectorEqual(Vector3(0.4,0.5,0.6),euler));
printf("PASS\n");
return 0;
diff --git a/apps/mathlib/math/Quaternion.cpp b/apps/mathlib/math/Quaternion.cpp
index 68fe85300..4f07840c3 100644
--- a/apps/mathlib/math/Quaternion.cpp
+++ b/apps/mathlib/math/Quaternion.cpp
@@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) :
Quaternion::Quaternion(const Dcm &dcm) :
Vector(4)
{
- setA(0.5f * sqrtf(1 + dcm(0, 0) +
- dcm(1, 1) + dcm(2, 2)));
- setB((dcm(2, 1) - dcm(1, 2)) /
- (4 * getA()));
- setC((dcm(0, 2) - dcm(2, 0)) /
- (4 * getA()));
- setD((dcm(1, 0) - dcm(0, 1)) /
- (4 * getA()));
+ // avoiding singularities by not using
+ // division equations
+ setA(0.5 * sqrt(1.0 +
+ double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
+ setB(0.5 * sqrt(1.0 +
+ double( dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
+ setC(0.5 * sqrt(1.0 +
+ double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
+ setD(0.5 * sqrt(1.0 +
+ double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
}
Quaternion::Quaternion(const EulerAngles &euler) :
Vector(4)
{
- float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
- float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
- float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
- float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
- float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
- float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
+ double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
+ double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
+ double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
+ double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
+ double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
+ double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
- setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
+ setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2);
}
@@ -147,33 +149,24 @@ int __EXPORT quaternionTest()
ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(), 0));
// test float ctor
- q = Quaternion(0, 1, 0, 0);
- ASSERT(equal(q.getA(), 0));
- ASSERT(equal(q.getB(), 1));
- ASSERT(equal(q.getC(), 0));
- ASSERT(equal(q.getD(), 0));
+ q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967);
+ ASSERT(equal(q.getA(), 0.1825742));
+ ASSERT(equal(q.getB(), 0.3651484));
+ ASSERT(equal(q.getC(), 0.5477226));
+ ASSERT(equal(q.getD(), 0.7302967));
// test euler ctor
- q = Quaternion(EulerAngles(0, 0, 0));
- ASSERT(equal(q.getA(), 1));
- ASSERT(equal(q.getB(), 0));
- ASSERT(equal(q.getC(), 0));
- ASSERT(equal(q.getD(), 0));
+ q = Quaternion(EulerAngles(0.1, 0.2, 0.3));
+ ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572)));
// test dcm ctor
q = Quaternion(Dcm());
- ASSERT(equal(q.getA(), 1));
- ASSERT(equal(q.getB(), 0));
- ASSERT(equal(q.getC(), 0));
- ASSERT(equal(q.getD(), 0));
+ ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0)));
// TODO test derivative
// test accessors
q.setA(0.1);
q.setB(0.2);
q.setC(0.3);
q.setD(0.4);
- ASSERT(equal(q.getA(), 0.1));
- ASSERT(equal(q.getB(), 0.2));
- ASSERT(equal(q.getC(), 0.3));
- ASSERT(equal(q.getD(), 0.4));
+ ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4)));
printf("PASS\n");
return 0;
}
diff --git a/apps/mathlib/math/nasa_rotation_def.pdf b/apps/mathlib/math/nasa_rotation_def.pdf
new file mode 100644
index 000000000..eb67a4bfc
--- /dev/null
+++ b/apps/mathlib/math/nasa_rotation_def.pdf
Binary files differ
diff --git a/apps/mathlib/math/test_math.sce b/apps/mathlib/math/test_math.sce
new file mode 100644
index 000000000..c3fba4729
--- /dev/null
+++ b/apps/mathlib/math/test_math.sce
@@ -0,0 +1,63 @@
+clc
+clear
+function out = float_truncate(in, digits)
+ out = round(in*10^digits)
+ out = out/10^digits
+endfunction
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
+ cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
+ -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
+
+disp(C_nb)
+//C_nb = float_truncate(C_nb,3)
+//disp(C_nb)
+
+theta = asin(-C_nb(3,1))
+phi = atan(C_nb(3,2), C_nb(3,3))
+psi = atan(C_nb(2,1), C_nb(1,1))
+printf('phi %f\n', phi)
+printf('theta %f\n', theta)
+printf('psi %f\n', psi)
+
+q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
+ sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
+ cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
+ cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
+
+//q = float_truncate(q,3)
+
+a = q(1)
+b = q(2)
+c = q(3)
+d = q(4)
+printf('q: %f %f %f %f\n', a, b, c, d)
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
+ 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
+ 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
+
+disp(C2_nb)
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 2ec459ddc..da704d929 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -148,6 +148,10 @@ set_hil_on_off(bool hil_enabled)
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+ /* sensore level hil */
+ pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+ pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+
mavlink_hil_enabled = true;
/* ramp up some HIL-related subscriptions */
diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h
index 1eb8c32e9..8c7a5b514 100644
--- a/apps/mavlink/mavlink_hil.h
+++ b/apps/mavlink/mavlink_hil.h
@@ -43,8 +43,12 @@ extern bool mavlink_hil_enabled;
extern struct vehicle_global_position_s hil_global_pos;
extern struct vehicle_attitude_s hil_attitude;
+extern struct sensor_combined_s hil_sensors;
+extern struct vehicle_gps_position_s hil_gps;
extern orb_advert_t pub_hil_global_pos;
extern orb_advert_t pub_hil_attitude;
+extern orb_advert_t pub_hil_sensors;
+extern orb_advert_t pub_hil_gps;
/**
* Enable / disable Hardware in the Loop simulation mode.
@@ -54,4 +58,4 @@ extern orb_advert_t pub_hil_attitude;
* requested change could not be made or was
* redundant.
*/
-extern int set_hil_on_off(bool hil_enabled); \ No newline at end of file
+extern int set_hil_on_off(bool hil_enabled);
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 79452f515..0eb0b39d1 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -86,8 +86,12 @@ static struct offboard_control_setpoint_s offboard_control_sp;
struct vehicle_global_position_s hil_global_pos;
struct vehicle_attitude_s hil_attitude;
+struct vehicle_gps_position_s hil_gps;
+struct sensor_combined_s hil_sensors;
orb_advert_t pub_hil_global_pos = -1;
orb_advert_t pub_hil_attitude = -1;
+orb_advert_t pub_hil_gps = -1;
+orb_advert_t pub_hil_sensors = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
@@ -302,6 +306,166 @@ handle_message(mavlink_message_t *msg)
if (mavlink_hil_enabled) {
+ uint64_t timestamp = hrt_absolute_time();
+
+ if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
+
+ mavlink_raw_imu_t imu;
+ mavlink_msg_raw_imu_decode(msg, &imu);
+
+ /* packet counter */
+ static uint16_t hil_counter = 0;
+ static uint16_t hil_frames = 0;
+ static uint64_t old_timestamp = 0;
+
+ /* hil gyro */
+ static const float mrad2rad = 1.0e-3f;
+ hil_sensors.timestamp = timestamp;
+ hil_sensors.gyro_counter = hil_counter;
+ hil_sensors.gyro_raw[0] = imu.xgyro;
+ hil_sensors.gyro_raw[1] = imu.ygyro;
+ hil_sensors.gyro_raw[2] = imu.zgyro;
+ hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
+ hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
+ hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
+
+ /* accelerometer */
+ hil_sensors.accelerometer_counter = hil_counter;
+ static const float mg2ms2 = 9.8f / 1000.0f;
+ hil_sensors.accelerometer_raw[0] = imu.xacc;
+ hil_sensors.accelerometer_raw[1] = imu.yacc;
+ hil_sensors.accelerometer_raw[2] = imu.zacc;
+ hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
+ hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
+ hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
+ hil_sensors.accelerometer_mode = 0; // TODO what is this?
+ hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+
+ /* adc */
+ hil_sensors.adc_voltage_v[0] = 0;
+ hil_sensors.adc_voltage_v[1] = 0;
+ hil_sensors.adc_voltage_v[2] = 0;
+
+ /* battery */
+ hil_sensors.battery_voltage_counter = hil_counter;
+ hil_sensors.battery_voltage_v = 11.1f;
+ hil_sensors.battery_voltage_valid = true;
+
+ /* magnetometer */
+ float mga2ga = 1.0e-3f;
+ hil_sensors.magnetometer_counter = hil_counter;
+ hil_sensors.magnetometer_raw[0] = imu.xmag;
+ hil_sensors.magnetometer_raw[1] = imu.ymag;
+ hil_sensors.magnetometer_raw[2] = imu.zmag;
+ hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
+ hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
+ hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
+ hil_sensors.magnetometer_range_ga = 32.7f; // int16
+ hil_sensors.magnetometer_mode = 0; // TODO what is this
+ hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+
+ /* publish */
+ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
+ // increment counters
+ hil_counter += 1 ;
+ hil_frames += 1 ;
+
+ // output
+ if ((timestamp - old_timestamp) > 1000000) {
+ printf("receiving hil imu at %d hz\n", hil_frames);
+ old_timestamp = timestamp;
+ hil_frames = 0;
+ }
+ }
+
+ if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
+
+ mavlink_gps_raw_int_t gps;
+ mavlink_msg_gps_raw_int_decode(msg, &gps);
+
+ /* packet counter */
+ static uint16_t hil_counter = 0;
+ static uint16_t hil_frames = 0;
+ static uint64_t old_timestamp = 0;
+
+ /* gps */
+ hil_gps.timestamp = gps.time_usec;
+ hil_gps.counter = hil_counter++;
+ hil_gps.time_gps_usec = gps.time_usec;
+ hil_gps.lat = gps.lat;
+ hil_gps.lon = gps.lon;
+ hil_gps.alt = gps.alt;
+ hil_gps.counter_pos_valid = hil_counter++;
+ hil_gps.eph = gps.eph;
+ hil_gps.epv = gps.epv;
+ hil_gps.s_variance = 100;
+ hil_gps.p_variance = 100;
+ hil_gps.vel = gps.vel;
+ hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
+ hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
+ hil_gps.vel_d = 0.0f;
+ hil_gps.cog = gps.cog;
+ hil_gps.fix_type = gps.fix_type;
+ hil_gps.satellites_visible = gps.satellites_visible;
+
+ /* publish */
+ orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+
+ // increment counters
+ hil_counter += 1 ;
+ hil_frames += 1 ;
+
+ // output
+ if ((timestamp - old_timestamp) > 1000000) {
+ printf("receiving hil gps at %d hz\n", hil_frames);
+ old_timestamp = timestamp;
+ hil_frames = 0;
+ }
+ }
+
+ if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
+
+ mavlink_raw_pressure_t press;
+ mavlink_msg_raw_pressure_decode(msg, &press);
+
+ /* packet counter */
+ static uint16_t hil_counter = 0;
+ static uint16_t hil_frames = 0;
+ static uint64_t old_timestamp = 0;
+
+ /* baro */
+ /* TODO, set ground_press/ temp during calib */
+ static const float ground_press = 1013.25f; // mbar
+ static const float ground_tempC = 21.0f;
+ static const float ground_alt = 0.0f;
+ static const float T0 = 273.15;
+ static const float R = 287.05f;
+ static const float g = 9.806f;
+
+ float tempC = press.temperature / 100.0f;
+ float tempAvgK = T0 + (tempC + ground_tempC)/2.0f;
+ float h = ground_alt + (R/g)*tempAvgK*logf(ground_press / press.press_abs);
+ hil_sensors.baro_counter = hil_counter;
+ hil_sensors.baro_pres_mbar = press.press_abs;
+ hil_sensors.baro_alt_meter = h;
+ hil_sensors.baro_temp_celcius = tempC;
+
+ /* publish */
+ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
+ // increment counters
+ hil_counter += 1 ;
+ hil_frames += 1 ;
+
+ // output
+ if ((timestamp - old_timestamp) > 1000000) {
+ printf("receiving hil pressure at %d hz\n", hil_frames);
+ old_timestamp = timestamp;
+ hil_frames = 0;
+ }
+ }
+
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
mavlink_hil_state_t hil_state;
@@ -412,7 +576,7 @@ receive_thread(void *arg)
int uart_fd = *((int *)arg);
const int timeout = 1000;
- uint8_t ch;
+ uint8_t buf[512];
mavlink_message_t msg;
@@ -423,13 +587,11 @@ receive_thread(void *arg)
struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
if (poll(fds, 1, timeout) > 0) {
- /* non-blocking read until buffer is empty */
- int nread = 0;
+ /* non-blocking read */
+ size_t nread = read(uart_fd, buf, sizeof(buf));
- do {
- nread = read(uart_fd, &ch, 1);
-
- if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
+ for (size_t i = 0; i < nread; i++) {
+ if (mavlink_parse_char(chan, buf[i], &msg, &status)) { //parse the char
/* handle generic messages and commands */
handle_message(&msg);
@@ -439,7 +601,7 @@ receive_thread(void *arg)
/* Handle packet with parameter component */
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
}
- } while (nread > 0);
+ }
}
}
@@ -452,11 +614,15 @@ receive_start(int uart)
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
+ // set to non-blocking read
+ int flags = fcntl(uart, F_GETFL, 0);
+ fcntl(uart, F_SETFL, flags | O_NONBLOCK);
+
struct sched_param param;
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
- pthread_attr_setstacksize(&receiveloop_attr, 2048);
+ pthread_attr_setstacksize(&receiveloop_attr, 4096);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h
index 1d25af35a..fe84aab63 100644
--- a/apps/uORB/topics/sensor_combined.h
+++ b/apps/uORB/topics/sensor_combined.h
@@ -99,6 +99,7 @@ struct sensor_combined_s {
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */
+ float battery_voltage_v; /**< Battery voltage in volts, filtered */
float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
uint32_t baro_counter; /**< Number of raw baro measurements taken */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 09:25:54 +0000