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

Merge remote-tracking branch 'upstream/master' into paul_mtecs

Browse Source
This commit is contained in:
Thomas Gubler
2014-04-06 10:49:53 +02:00
parent d3ca12f136 be6c0d2ece
commit be918cd6d0
30 changed files with 999 additions and 687 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+8 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2500;
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
src/modules/commander/commander.cpp
+2 -11
View File
@@ -118,8 +118,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
#define RC_TIMEOUT 100000
#define RC_TIMEOUT_HIL 500000
#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
@@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[])
}
}
/*
* XXX workaround:
* Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz)
* which can trigger RC loss if the computer/simulator lags.
*/
uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT;
/* start RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) {
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
src/modules/fw_att_pos_estimator/estimator.cpp
+79 -134
View File
@@ -2,85 +2,6 @@
#include <string.h>
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float posNED[3]; // North, East Down position (m)
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
// Baro input
float baroHgt;
bool statesInitialised = false;
bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode = true; ///< boolean true if no position feedback is fused
bool useAirspeed = true; ///< boolean true if airspeed data is being used
bool useCompass = true; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection = true;
static unsigned storeIndex = 0;
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
@@ -185,7 +106,31 @@ void swap_var(float &d1, float &d2)
d2 = tmp;
}
void UpdateStrapdownEquationsNED()
AttPosEKF::AttPosEKF() :
fusionModeGPS(0),
covSkipCount(0),
EAS2TAS(1.0f),
statesInitialised(false),
fuseVelData(false),
fusePosData(false),
fuseHgtData(false),
fuseMagData(false),
fuseVtasData(false),
onGround(true),
staticMode(true),
useAirspeed(true),
useCompass(true),
numericalProtection(true),
storeIndex(0)
{
}
AttPosEKF::~AttPosEKF()
{
}
void AttPosEKF::UpdateStrapdownEquationsNED()
{
Vector3f delVelNav;
float q00;
@@ -247,7 +192,7 @@ void UpdateStrapdownEquationsNED()
qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
// Normalise the quaternions and update the quaternion states
quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
if (quatMag > 1e-16f)
{
float quatMagInv = 1.0f/quatMag;
@@ -312,7 +257,7 @@ void UpdateStrapdownEquationsNED()
ConstrainStates();
}
void CovariancePrediction(float dt)
void AttPosEKF::CovariancePrediction(float dt)
{
// scalars
float windVelSigma;
@@ -953,7 +898,7 @@ void CovariancePrediction(float dt)
ConstrainVariances();
}
void FuseVelposNED()
void AttPosEKF::FuseVelposNED()
{
// declare variables used by fault isolation logic
@@ -999,8 +944,8 @@ void FuseVelposNED()
observation[5] = -(hgtMea);
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
R_OBS[0] = 0.04f + sq(velErr);
R_OBS[1] = R_OBS[0];
R_OBS[2] = 0.08f + sq(velErr);
@@ -1026,7 +971,7 @@ void FuseVelposNED()
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
}
// apply a 5-sigma threshold
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
{
@@ -1175,7 +1120,7 @@ void FuseVelposNED()
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
}
void FuseMagnetometer()
void AttPosEKF::FuseMagnetometer()
{
uint8_t obsIndex;
uint8_t indexLimit;
@@ -1482,7 +1427,7 @@ void FuseMagnetometer()
ConstrainVariances();
}
void FuseAirspeed()
void AttPosEKF::FuseAirspeed()
{
float vn;
float ve;
@@ -1503,14 +1448,14 @@ void FuseAirspeed()
// Need to check that it is flying before fusing airspeed data
// Calculate the predicted airspeed
VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
float H_TAS[21];
for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
@@ -1611,7 +1556,7 @@ void FuseAirspeed()
ConstrainVariances();
}
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
@@ -1624,7 +1569,7 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
}
}
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
@@ -1637,13 +1582,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
}
}
float sq(float valIn)
float AttPosEKF::sq(float valIn)
{
return valIn*valIn;
}
// Store states in a history array along with time stamp
void StoreStates(uint64_t timestamp_ms)
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{
for (unsigned i=0; i<n_states; i++)
storedStates[i][storeIndex] = states[i];
@@ -1653,7 +1598,7 @@ void StoreStates(uint64_t timestamp_ms)
storeIndex = 0;
}
void ResetStoredStates()
void AttPosEKF::ResetStoredStates()
{
// reset all stored states
memset(&storedStates[0][0], 0, sizeof(storedStates));
@@ -1674,7 +1619,7 @@ void ResetStoredStates()
}
// Output the state vector stored at the time that best matches that specified by msec
int RecallStates(float statesForFusion[n_states], uint64_t msec)
int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
{
int ret = 0;
@@ -1723,7 +1668,7 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
return ret;
}
void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
{
// Calculate the nav to body cosine matrix
float q00 = sq(quat[0]);
@@ -1748,7 +1693,7 @@ void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
Tnb.y.z = 2*(q23 + q01);
}
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
{
// Calculate the body to nav cosine matrix
float q00 = sq(quat[0]);
@@ -1773,7 +1718,7 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
Tbn.z.y = 2*(q23 + q01);
}
void eul2quat(float (&quat)[4], const float (&eul)[3])
void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
{
float u1 = cos(0.5f*eul[0]);
float u2 = cos(0.5f*eul[1]);
@@ -1787,35 +1732,35 @@ void eul2quat(float (&quat)[4], const float (&eul)[3])
quat[3] = u1*u2*u6-u4*u5*u3;
}
void quat2eul(float (&y)[3], const float (&u)[4])
void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
{
y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
}
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
{
velNED[0] = gpsGndSpd*cos(gpsCourse);
velNED[1] = gpsGndSpd*sin(gpsCourse);
velNED[0] = gpsGndSpd*cosf(gpsCourse);
velNED[1] = gpsGndSpd*sinf(gpsCourse);
velNED[2] = gpsVelD;
}
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
{
posNED[0] = earthRadius * (lat - latRef);
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
posNED[2] = -(hgt - hgtRef);
}
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
{
lat = latRef + posNED[0] * earthRadiusInv;
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNED[2];
}
void OnGroundCheck()
void AttPosEKF::OnGroundCheck()
{
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) {
@@ -1823,7 +1768,7 @@ void OnGroundCheck()
}
}
void calcEarthRateNED(Vector3f &omega, float latitude)
void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
{
//Define Earth rotation vector in the NED navigation frame
omega.x = earthRate*cosf(latitude);
@@ -1831,13 +1776,13 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
omega.z = -earthRate*sinf(latitude);
}
void CovarianceInit()
void AttPosEKF::CovarianceInit()
{
// Calculate the initial covariance matrix P
P[0][0] = 0.25f*sq(1.0f*deg2rad);
P[1][1] = 0.25f*sq(1.0f*deg2rad);
P[2][2] = 0.25f*sq(1.0f*deg2rad);
P[3][3] = 0.25f*sq(10.0f*deg2rad);
P[0][0] = 0.25f * sq(1.0f*deg2rad);
P[1][1] = 0.25f * sq(1.0f*deg2rad);
P[2][2] = 0.25f * sq(1.0f*deg2rad);
P[3][3] = 0.25f * sq(10.0f*deg2rad);
P[4][4] = sq(0.7);
P[5][5] = P[4][4];
P[6][6] = sq(0.7);
@@ -1857,12 +1802,12 @@ void CovarianceInit()
P[20][20] = P[18][18];
}
float ConstrainFloat(float val, float min, float max)
float AttPosEKF::ConstrainFloat(float val, float min, float max)
{
return (val > max) ? max : ((val < min) ? min : val);
}
void ConstrainVariances()
void AttPosEKF::ConstrainVariances()
{
if (!numericalProtection) {
return;
@@ -1914,7 +1859,7 @@ void ConstrainVariances()
}
void ConstrainStates()
void AttPosEKF::ConstrainStates()
{
if (!numericalProtection) {
return;
@@ -1971,7 +1916,7 @@ void ConstrainStates()
}
void ForceSymmetry()
void AttPosEKF::ForceSymmetry()
{
if (!numericalProtection) {
return;
@@ -1989,7 +1934,7 @@ void ForceSymmetry()
}
}
bool FilterHealthy()
bool AttPosEKF::FilterHealthy()
{
if (!statesInitialised) {
return false;
@@ -2012,7 +1957,7 @@ bool FilterHealthy()
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition(void)
void AttPosEKF::ResetPosition(void)
{
if (staticMode) {
states[7] = 0;
@@ -2030,7 +1975,7 @@ void ResetPosition(void)
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight(void)
void AttPosEKF::ResetHeight(void)
{
// write to the state vector
states[9] = -hgtMea;
@@ -2039,7 +1984,7 @@ void ResetHeight(void)
/**
* Reset the velocity state.
*/
void ResetVelocity(void)
void AttPosEKF::ResetVelocity(void)
{
if (staticMode) {
states[4] = 0.0f;
@@ -2054,7 +1999,7 @@ void ResetVelocity(void)
}
void FillErrorReport(struct ekf_status_report *err)
void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
{
for (int i = 0; i < n_states; i++)
{
@@ -2069,7 +2014,7 @@ void FillErrorReport(struct ekf_status_report *err)
err->hgtTimeout = current_ekf_state.hgtTimeout;
}
bool StatesNaN(struct ekf_status_report *err_report) {
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
bool err = false;
// check all states and covariance matrices
@@ -2122,7 +2067,7 @@ bool StatesNaN(struct ekf_status_report *err_report) {
* updated, but before any of the fusion steps are
* executed.
*/
int CheckAndBound()
int AttPosEKF::CheckAndBound()
{
// Store the old filter state
@@ -2164,7 +2109,7 @@ int CheckAndBound()
return 0;
}
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
@@ -2200,7 +2145,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
}
void InitializeDynamic(float (&initvelNED)[3])
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
{
// Clear the init flag
@@ -2254,7 +2199,7 @@ void InitializeDynamic(float (&initvelNED)[3])
summedDelVel.z = 0.0f;
}
void InitialiseFilter(float (&initvelNED)[3])
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
{
//store initial lat,long and height
latRef = gpsLat;
@@ -2266,7 +2211,7 @@ void InitialiseFilter(float (&initvelNED)[3])
InitializeDynamic(initvelNED);
}
void ZeroVariables()
void AttPosEKF::ZeroVariables()
{
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
@@ -2292,12 +2237,12 @@ void ZeroVariables()
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
}
void GetFilterState(struct ekf_status_report *state)
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
{
memcpy(state, &current_ekf_state, sizeof(state));
}
void GetLastErrorState(struct ekf_status_report *last_error)
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
{
memcpy(last_error, &last_ekf_error, sizeof(last_error));
}
src/modules/fw_att_pos_estimator/estimator.h
+107 -83
View File
@@ -48,85 +48,10 @@ void swap_var(float &d1, float &d2);
const unsigned int n_states = 21;
const unsigned int data_buffer_size = 50;
extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
extern Vector3f dVelIMU;
extern Vector3f dAngIMU;
extern float P[n_states][n_states]; // covariance matrix
extern float Kfusion[n_states]; // Kalman gains
extern float states[n_states]; // state matrix
extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
extern bool useAirspeed; // boolean true if airspeed data is being used
extern bool useCompass; // boolean true if magnetometer data is being used
extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
extern float innovVelPos[6]; // innovation output
extern float varInnovVelPos[6]; // innovation variance output
extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
extern bool fuseMagData; // boolean true when magnetometer data is to be fused
extern float velNED[3]; // North, East, Down velocity obs (m/s)
extern float posNE[2]; // North, East position obs (m)
extern float hgtMea; // measured height (m)
extern float posNED[3]; // North, East Down position (m)
extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
extern float innovMag[3]; // innovation output
extern float varInnovMag[3]; // innovation variance output
extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
extern float innovVtas; // innovation output
extern float varInnovVtas; // innovation variance output
extern bool fuseVtasData; // boolean true when airspeed data is to be fused
extern float VtasMeas; // true airspeed measurement (m/s)
extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
extern float latRef; // WGS-84 latitude of reference point (rad)
extern float lonRef; // WGS-84 longitude of reference point (rad)
extern float hgtRef; // WGS-84 height of reference point (m)
extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
extern float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
extern float gpsCourse;
extern float gpsVelD;
extern float gpsLat;
extern float gpsLon;
extern float gpsHgt;
extern uint8_t GPSstatus;
// Baro input
extern float baroHgt;
extern bool statesInitialised;
extern bool numericalProtection;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
extern bool staticMode;
// extern bool staticMode;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
@@ -150,6 +75,93 @@ struct ekf_status_report {
bool kalmanGainsNaN;
};
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
@@ -164,8 +176,6 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
float sq(float valIn);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
@@ -190,15 +200,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
void OnGroundCheck();
@@ -231,5 +245,15 @@ void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]);
protected:
bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
};
uint32_t millis();
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+199 -185
View File
@@ -124,6 +124,16 @@ public:
*/
int start();
/**
* Print the current status.
*/
void print_status();
/**
* Trip the filter by feeding it NaN values.
*/
int trip_nan();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
@@ -199,6 +209,7 @@ private:
param_t tas_delay_ms;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
/**
* Update our local parameter cache.
@@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_initialized(false),
_gps_initialized(false),
_mavlink_fd(-1)
_mavlink_fd(-1),
_ekf(nullptr)
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@@ -384,6 +396,12 @@ void
FixedwingEstimator::task_main()
{
_ekf = new AttPosEKF();
if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!");
}
/*
* do subscriptions
*/
@@ -414,23 +432,23 @@ FixedwingEstimator::task_main()
parameters_update();
/* set initial filter state */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
statesInitialised = false;
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
_ekf->fuseHgtData = false;
_ekf->fuseMagData = false;
_ekf->fuseVtasData = false;
_ekf->statesInitialised = false;
/* initialize measurement data */
VtasMeas = 0.0f;
_ekf->VtasMeas = 0.0f;
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
dVelIMU.x = 0.0f;
dVelIMU.y = 0.0f;
dVelIMU.z = 0.0f;
dAngIMU.x = 0.0f;
dAngIMU.y = 0.0f;
dAngIMU.z = 0.0f;
_ekf->dVelIMU.x = 0.0f;
_ekf->dVelIMU.y = 0.0f;
_ekf->dVelIMU.z = 0.0f;
_ekf->dAngIMU.x = 0.0f;
_ekf->dAngIMU.y = 0.0f;
_ekf->dAngIMU.z = 0.0f;
/* wakeup source(s) */
struct pollfd fds[2];
@@ -509,7 +527,7 @@ FixedwingEstimator::task_main()
}
last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
last_run = _gyro.timestamp;
@@ -521,20 +539,20 @@ FixedwingEstimator::task_main()
// Always store data, independent of init status
/* fill in last data set */
dtIMU = deltaT;
_ekf->dtIMU = deltaT;
angRate.x = _gyro.x;
angRate.y = _gyro.y;
angRate.z = _gyro.z;
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
accel.x = _accel.x;
accel.y = _accel.y;
accel.z = _accel.z;
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
lastAccel = accel;
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
_ekf->lastAccel = accel;
#else
@@ -563,20 +581,20 @@ FixedwingEstimator::task_main()
// Always store data, independent of init status
/* fill in last data set */
dtIMU = deltaT;
_ekf->dtIMU = deltaT;
angRate.x = _sensor_combined.gyro_rad_s[0];
angRate.y = _sensor_combined.gyro_rad_s[1];
angRate.z = _sensor_combined.gyro_rad_s[2];
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
accel.x = _sensor_combined.accelerometer_m_s2[0];
accel.y = _sensor_combined.accelerometer_m_s2[1];
accel.z = _sensor_combined.accelerometer_m_s2[2];
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
lastAccel = accel;
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
mag_updated = true;
@@ -597,7 +615,7 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
VtasMeas = _airspeed.true_airspeed_m_s;
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
newAdsData = true;
} else {
@@ -622,24 +640,24 @@ FixedwingEstimator::task_main()
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
ResetPosition();
ResetVelocity();
ResetStoredStates();
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
/* fuse GPS updates */
//_gps.timestamp / 1e3;
GPSstatus = _gps.fix_type;
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
gpsLat = math::radians(_gps.lat / (double)1e7);
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
gpsHgt = _gps.alt / 1e3f;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
newDataGps = true;
}
@@ -652,10 +670,10 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude - _baro_ref;
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
}
#ifndef SENSOR_COMBINED_SUB
@@ -671,27 +689,27 @@ FixedwingEstimator::task_main()
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
magData.x = _mag.x;
magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.x = _mag.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
magData.y = _mag.y;
magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.y = _mag.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
magData.z = _mag.z;
magBias.z = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _mag.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#else
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
magData.x = _sensor_combined.magnetometer_ga[0];
magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
magData.y = _sensor_combined.magnetometer_ga[1];
magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
magData.z = _sensor_combined.magnetometer_ga[2];
magBias.z = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#endif
@@ -705,7 +723,7 @@ FixedwingEstimator::task_main()
/**
* CHECK IF THE INPUT DATA IS SANE
*/
int check = CheckAndBound();
int check = _ekf->CheckAndBound();
switch (check) {
case 0:
@@ -739,7 +757,7 @@ FixedwingEstimator::task_main()
struct ekf_status_report ekf_report;
GetLastErrorState(&ekf_report);
_ekf->GetLastErrorState(&ekf_report);
struct estimator_status_report rep;
memset(&rep, 0, sizeof(rep));
@@ -779,16 +797,16 @@ FixedwingEstimator::task_main()
if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && (GPSstatus == 3)) {
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7;
double lon = _gps.lon * 1e-7;
float alt = _gps.alt * 1e-3;
InitialiseFilter(velNED);
_ekf->InitialiseFilter(_ekf->velNED);
// Initialize projection
_local_pos.ref_lat = _gps.lat;
@@ -799,7 +817,7 @@ FixedwingEstimator::task_main()
// Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude;
baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude - _baro_ref;
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
// XXX this is not multithreading safe
@@ -808,24 +826,24 @@ FixedwingEstimator::task_main()
_gps_initialized = true;
} else if (!statesInitialised) {
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
} else if (!_ekf->statesInitialised) {
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
InitialiseFilter(velNED);
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
}
}
// If valid IMU data and states initialised, predict states and covariances
if (statesInitialised) {
if (_ekf->statesInitialised) {
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
_ekf->UpdateStrapdownEquationsNED();
#if 0
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
@@ -842,20 +860,20 @@ FixedwingEstimator::task_main()
#endif
// store the predicted states for subsequent use by measurement fusion
StoreStates(IMUmsec);
_ekf->StoreStates(IMUmsec);
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
_ekf->OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + dVelIMU;
dt += dtIMU;
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
dt += _ekf->dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction(dt);
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
_ekf->CovariancePrediction(dt);
_ekf->summedDelAng = _ekf->summedDelAng.zero();
_ekf->summedDelVel = _ekf->summedDelVel.zero();
dt = 0.0f;
}
@@ -865,79 +883,79 @@ FixedwingEstimator::task_main()
// Fuse GPS Measurements
if (newDataGps && _gps_initialized) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else if (statesInitialised) {
} else if (_ekf->statesInitialised) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else {
fuseVelData = false;
fusePosData = false;
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
}
if (newAdsData && statesInitialised) {
if (newAdsData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
fuseHgtData = true;
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else {
fuseHgtData = false;
_ekf->fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised) {
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
if (newDataMag && _ekf->statesInitialised) {
_ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
} else {
fuseMagData = false;
_ekf->fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed();
} else {
fuseVtasData = false;
_ekf->fuseVtasData = false;
}
// Publish results
@@ -954,7 +972,7 @@ FixedwingEstimator::task_main()
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
math::Quaternion q(states[0], states[1], states[2], states[3]);
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@@ -962,10 +980,10 @@ FixedwingEstimator::task_main()
_att.R[i][j] = R(i, j);
_att.timestamp = last_sensor_timestamp;
_att.q[0] = states[0];
_att.q[1] = states[1];
_att.q[2] = states[2];
_att.q[3] = states[3];
_att.q[0] = _ekf->states[0];
_att.q[1] = _ekf->states[1];
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
@@ -974,13 +992,13 @@ FixedwingEstimator::task_main()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = angRate.x - states[10];
_att.pitchspeed = angRate.y - states[11];
_att.yawspeed = angRate.z - states[12];
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
// gyro offsets
_att.rate_offsets[0] = states[10];
_att.rate_offsets[1] = states[11];
_att.rate_offsets[2] = states[12];
_att.rate_offsets[0] = _ekf->states[10];
_att.rate_offsets[1] = _ekf->states[11];
_att.rate_offsets[2] = _ekf->states[12];
/* lazily publish the attitude only once available */
if (_att_pub > 0) {
@@ -993,20 +1011,15 @@ FixedwingEstimator::task_main()
}
}
if (!isfinite(states[0])) {
print_status();
_exit(0);
}
if (_gps_initialized) {
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = states[7];
_local_pos.y = states[8];
_local_pos.z = states[9];
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
_local_pos.z = _ekf->states[9];
_local_pos.vx = states[4];
_local_pos.vy = states[5];
_local_pos.vz = states[6];
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
_local_pos.vz = _ekf->states[6];
_local_pos.xy_valid = _gps_initialized;
_local_pos.z_valid = true;
@@ -1107,9 +1120,10 @@ FixedwingEstimator::start()
return OK;
}
void print_status()
void
FixedwingEstimator::print_status()
{
math::Quaternion q(states[0], states[1], states[2], states[3]);
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@@ -1125,30 +1139,30 @@ void print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(statesInitialised) ? "INITIALIZED" : "NON_INIT",
(onGround) ? "ON_GROUND" : "AIRBORNE",
(fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(fusePosData) ? "FUSE_POS" : "INH_POS",
(fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
}
int trip_nan() {
int FixedwingEstimator::trip_nan() {
int ret = 0;
@@ -1166,7 +1180,7 @@ int trip_nan() {
float nan_val = 0.0f / 0.0f;
warnx("system not armed, tripping state vector with NaN values");
states[5] = nan_val;
_ekf->states[5] = nan_val;
usleep(100000);
// warnx("tripping covariance #1 with NaN values");
@@ -1178,15 +1192,15 @@ int trip_nan() {
// usleep(100000);
warnx("tripping covariance #3 with NaN values");
P[3][3] = nan_val; // covariance matrix
_ekf->P[3][3] = nan_val; // covariance matrix
usleep(100000);
warnx("tripping Kalman gains with NaN values");
Kfusion[0] = nan_val; // Kalman gains
_ekf->Kfusion[0] = nan_val; // Kalman gains
usleep(100000);
warnx("tripping stored states[0] with NaN values");
storedStates[0][0] = nan_val;
_ekf->storedStates[0][0] = nan_val;
usleep(100000);
warnx("\nDONE - FILTER STATE:");
@@ -1234,7 +1248,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
if (estimator::g_estimator) {
warnx("running");
print_status();
estimator::g_estimator->print_status();
exit(0);
@@ -1245,7 +1259,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
if (!strcmp(argv[1], "trip")) {
if (estimator::g_estimator) {
int ret = trip_nan();
int ret = estimator::g_estimator->trip_nan();
exit(ret);
src/modules/mavlink/mavlink_orb_subscription.cpp
+1
View File
@@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
if (_updated) {
orb_copy(_topic, _fd, _data);
_published = true;
return true;
}
}
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached()
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}
src/modules/px4iofirmware/controls.c
+94 -84
View File
@@ -134,8 +134,6 @@ controls_tick() {
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
@@ -201,94 +199,104 @@ controls_tick() {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* do not command anything in failsafe, kick in the RC loss counter */
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
scaled = -scaled;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) {
/* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
if (mapped == 3 && r_setup_rc_thr_failsafe) {
/* throttle failsafe detection */
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i))) {
r_rc_values[i] = 0;
}
}
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/*
@@ -316,8 +324,13 @@ controls_tick() {
* Handle losing RC input
*/
/* this kicks in if the receiver is gone or the system went to failsafe */
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* if we are in failsafe, clear the override flag */
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
@@ -326,27 +339,24 @@ controls_tick() {
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set raw channel count to zero */
r_raw_rc_count = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* must have R/C input (NO FAILSAFE!).
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false;
@@ -369,10 +379,10 @@ controls_tick() {
mixer_tick();
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
}
src/modules/px4iofirmware/protocol.h
+15 -13
View File
@@ -164,10 +164,10 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
@@ -201,13 +201,15 @@ enum { /* DSM bind states */
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* 12 occupied by CRC */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
@@ -217,10 +219,10 @@ enum { /* DSM bind states */
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_VALID 64
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 52
src/modules/px4iofirmware/px4io.h
+1
View File
@@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#endif
#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
#define r_control_values (&r_page_controls[0])
src/modules/px4iofirmware/registers.c
+1
View File
@@ -169,6 +169,7 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
};
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
src/modules/sdlog2/sdlog2.c
+29 -1
View File
@@ -84,6 +84,8 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
int system_power_sub;
int servorail_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
thread_running = true;
@@ -1184,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
@@ -1226,6 +1236,24 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- SYSTEM POWER RAILS --- */
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
log_msg.msg_type = LOG_PWR_MSG;
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
/* copy servo rail status topic here too */
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
LOGBUFFER_WRITE_AND_COUNT(PWR);
}
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
@@ -1258,7 +1286,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
LOGBUFFER_WRITE_AND_COUNT(DIST);
LOGBUFFER_WRITE_AND_COUNT(ESTM);
}
/* signal the other thread new data, but not yet unlock */
src/modules/sdlog2/sdlog2_messages.h
+30 -15
View File
@@ -174,6 +174,7 @@ struct log_OUT0_s {
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
float air_temperature_celsius;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -277,6 +278,29 @@ struct log_TELE_s {
uint8_t txbuf;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 23
struct log_ESTM_s {
float s[10];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
struct log_PWR_s {
float peripherals_5v;
float servo_rail_5v;
float servo_rssi;
uint8_t usb_ok;
uint8_t brick_ok;
uint8_t servo_ok;
uint8_t low_power_rail_overcurrent;
uint8_t high_power_rail_overcurrent;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -299,16 +323,6 @@ struct log_PARM_s {
float value;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 132
struct log_ESTM_s {
float s[32];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
#pragma pack(pop)
/* construct list of all message formats */
@@ -325,7 +339,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
@@ -335,15 +349,16 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
/* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
LOG_FORMAT(PARM, "Nf", "Name,Value"),
LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PARM, "Nf", "Name,Value")
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
#endif /* SDLOG2_MESSAGES_H_ */
src/modules/sensors/sensor_params.c
+3 -25
View File
@@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
*/
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
/**
* Failsafe channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_CH, 0);
/**
* Failsafe channel mode.
*
* 0 = too low means signal loss,
* 1 = too high means signal loss
*
* @min 0
* @max 1
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_MODE, 0);
/**
* Failsafe channel PWM threshold.
*
* @min 0
* @max 1
* @min 800
* @max 2200
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
src/modules/sensors/sensors.cpp
+12 -23
View File
@@ -263,9 +263,7 @@ private:
float rc_scale_yaw;
float rc_scale_flaps;
int rc_fs_ch;
int rc_fs_mode;
float rc_fs_thr;
int32_t rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
@@ -313,8 +311,6 @@ private:
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t rc_fs_ch;
param_t rc_fs_mode;
param_t rc_fs_thr;
param_t battery_voltage_scaling;
@@ -531,9 +527,7 @@ Sensors::Sensors() :
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* RC failsafe */
_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -689,8 +683,6 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
/* update RC function mappings */
@@ -1033,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
@@ -1312,19 +1305,15 @@ Sensors::rc_poll()
manual_control.aux5 = NAN;
/* require at least four channels to consider the signal valid */
if (rc_input.channel_count < 4)
if (rc_input.channel_count < 4) {
return;
}
/* failsafe check */
if (_parameters.rc_fs_ch != 0) {
if (_parameters.rc_fs_mode == 0) {
if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
return;
} else if (_parameters.rc_fs_mode == 1) {
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
return;
}
/* check for failsafe */
if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
/* do not publish manual control setpoints when there are none */
return;
}
unsigned channel_limit = rc_input.channel_count;
src/modules/uORB/objects_common.cpp
+3
View File
@@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
#include "topics/servorail_status.h"
ORB_DEFINE(servorail_status, struct servorail_status_s);
#include "topics/system_power.h"
ORB_DEFINE(system_power, struct system_power_s);
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
src/modules/uORB/topics/airspeed.h
+3 -2
View File
@@ -52,9 +52,10 @@
* Airspeed
*/
struct airspeed_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */
};
/**
src/modules/uORB/topics/differential_pressure.h
+5 -4
View File
@@ -52,13 +52,14 @@
* Differential pressure.
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
};
src/modules/uORB/topics/system_power.h
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file system_power.h
*
* Definition of the system_power voltage and power status uORB topic.
*/
#ifndef SYSTEM_POWER_H_
#define SYSTEM_POWER_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* voltage and power supply status
*/
struct system_power_s {
uint64_t timestamp; /**< microseconds since system boot */
float voltage5V_v; /**< peripheral 5V rail voltage */
uint8_t usb_connected:1; /**< USB is connected when 1 */
uint8_t brick_valid:1; /**< brick power is good when 1 */
uint8_t servo_valid:1; /**< servo power is good when 1 */
uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */
uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(system_power);
#endif