forked from Archive/PX4-Autopilot
Merge pull request #156 from jgoppert/sensor_hil_rebase
Rebase of changes to sensor_hil_fixedwing branch.
This commit is contained in:
px4dev
commit
9faf348cf5
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -134,7 +134,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
|||
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
|
||||
|| current_status->flag_hil_enabled) {
|
||||
invalid_state = false;
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = false;
|
||||
|
|
@ -708,7 +709,9 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
|
|||
case SYSTEM_STATE_REBOOT:
|
||||
printf("try to reboot\n");
|
||||
|
||||
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
|
||||
if (current_system_state == SYSTEM_STATE_STANDBY
|
||||
|| current_system_state == SYSTEM_STATE_PREFLIGHT
|
||||
|| current_status->flag_hil_enabled) {
|
||||
printf("system will reboot\n");
|
||||
mavlink_log_critical(mavlink_fd, "Rebooting..");
|
||||
usleep(200000);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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; // earth 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) {
|
||||
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 (ret > 0) {
|
||||
updateSubscriptions();
|
||||
|
||||
if (_gps.fix_type > 2) break;
|
||||
|
||||
} else if (ret == 0) {
|
||||
printf("[kalman_demo] waiting for gps lock\n");
|
||||
usleep(1000000);
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
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(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) * dt;
|
||||
F(1, 2) = (omega * cosL + vE / R) * dt;
|
||||
F(1, 3) = (-1.0f / R) * dt;
|
||||
F(1, 8) = (vN / RSq) * dt;
|
||||
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) * 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(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) * 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(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) * 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(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) * 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(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) * dt;
|
||||
F(6, 8) = (-vN / RSq) * dt;
|
||||
F(6, 3) = 1 / R;
|
||||
F(6, 8) = -vN / RSq;
|
||||
|
||||
F(7, 4) = (1 / (R * cosL)) * dt;
|
||||
F(7, 6) = (vE * tanL / (R * cosL)) * dt;
|
||||
F(7, 8) = (-vE / (cosL * RSq)) * dt;
|
||||
F(7, 4) = 1 / (R * cosL);
|
||||
F(7, 6) = vE * tanL / (R * cosL);
|
||||
F(7, 8) = -vE / (cosL * RSq);
|
||||
|
||||
F(8, 5) = (-1) * dt;
|
||||
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(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) * 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;
|
||||
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);
|
||||
|
||||
// predict equations for kalman filter
|
||||
P = F * P * F.transpose() + G * V * G.transpose();
|
||||
// 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
|
||||
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);
|
||||
if (beta > 1000.0f) {
|
||||
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 > 1000.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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,18 +133,30 @@ Dcm::~Dcm()
|
|||
int __EXPORT dcmTest()
|
||||
{
|
||||
printf("Test DCM\t\t: ");
|
||||
// default ctor
|
||||
ASSERT(matrixEqual(Dcm(),
|
||||
Matrix::identity(3)));
|
||||
// quaternion ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
|
||||
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
|
||||
0.2896295f, 0.9564251f, -0.0369570f,
|
||||
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||
// euler angle ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
|
||||
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
|
||||
0.2896295f, 0.9564251f, -0.0369570f,
|
||||
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||
// rotations
|
||||
Vector3 vB(1, 2, 3);
|
||||
ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
|
||||
Matrix::identity(3)));
|
||||
ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
|
||||
Matrix::identity(3)));
|
||||
ASSERT(vectorEqual(Vector3(-2, 1, 3),
|
||||
Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
||||
Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
|
||||
ASSERT(vectorEqual(Vector3(1, -3, 2),
|
||||
Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
||||
ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
|
||||
Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||
Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
|
||||
ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
|
||||
Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||
Dcm(EulerAngles(
|
||||
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
|
||||
printf("PASS\n");
|
||||
|
|
|
|||
|
|
@ -64,6 +64,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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -97,23 +97,27 @@ EulerAngles::~EulerAngles()
|
|||
int __EXPORT eulerAnglesTest()
|
||||
{
|
||||
printf("Test EulerAngles\t: ");
|
||||
EulerAngles euler(1, 2, 3);
|
||||
EulerAngles euler(0.1f, 0.2f, 0.3f);
|
||||
|
||||
// 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.1f, 0.2f, 0.3f), euler));
|
||||
ASSERT(equal(euler.getPhi(), 0.1f));
|
||||
ASSERT(equal(euler.getTheta(), 0.2f));
|
||||
ASSERT(equal(euler.getPsi(), 0.3f));
|
||||
|
||||
// test dcm ctor
|
||||
euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
|
||||
// test quat ctor
|
||||
euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), 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.4f);
|
||||
euler.setTheta(0.5f);
|
||||
euler.setPsi(0.6f);
|
||||
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
|
||||
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -142,38 +144,29 @@ int __EXPORT quaternionTest()
|
|||
printf("Test Quaternion\t\t: ");
|
||||
// test default ctor
|
||||
Quaternion q;
|
||||
ASSERT(equal(q.getA(), 1));
|
||||
ASSERT(equal(q.getB(), 0));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
ASSERT(equal(q.getA(), 1.0f));
|
||||
ASSERT(equal(q.getB(), 0.0f));
|
||||
ASSERT(equal(q.getC(), 0.0f));
|
||||
ASSERT(equal(q.getD(), 0.0f));
|
||||
// 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.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
|
||||
ASSERT(equal(q.getA(), 0.1825742f));
|
||||
ASSERT(equal(q.getB(), 0.3651484f));
|
||||
ASSERT(equal(q.getC(), 0.5477226f));
|
||||
ASSERT(equal(q.getD(), 0.7302967f));
|
||||
// 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.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
|
||||
// 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.0f, 0.0f, 0.0f, 0.0f)));
|
||||
// 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));
|
||||
q.setA(0.1f);
|
||||
q.setB(0.2f);
|
||||
q.setC(0.3f);
|
||||
q.setD(0.4f);
|
||||
ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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)
|
||||
|
|
@ -77,7 +77,7 @@
|
|||
/* define MAVLink specific parameters */
|
||||
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
|
||||
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
|
||||
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR);
|
||||
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
|
||||
|
||||
__EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
||||
|
|
@ -98,7 +98,7 @@ static bool mavlink_link_termination_allowed = false;
|
|||
mavlink_system_t mavlink_system = {
|
||||
100,
|
||||
50,
|
||||
MAV_TYPE_QUADROTOR,
|
||||
MAV_TYPE_FIXED_WING,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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,161 @@ 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;
|
||||
|
||||
/* 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 +571,7 @@ receive_thread(void *arg)
|
|||
int uart_fd = *((int *)arg);
|
||||
|
||||
const int timeout = 1000;
|
||||
uint8_t ch;
|
||||
uint8_t buf[32];
|
||||
|
||||
mavlink_message_t msg;
|
||||
|
||||
|
|
@ -423,13 +582,12 @@ 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));
|
||||
ASSERT(nread > 0)
|
||||
|
||||
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 +597,7 @@ receive_thread(void *arg)
|
|||
/* Handle packet with parameter component */
|
||||
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
|
||||
}
|
||||
} while (nread > 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -452,6 +610,10 @@ 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, ¶m);
|
||||
|
|
|
|||
Loading…
Reference in New Issue