diff options
Diffstat (limited to 'src/modules')
24 files changed, 733 insertions, 651 deletions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a7..c8c7a42e7 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -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) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 06f7ae6cc..69a45a02f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f4..2f84dc963 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -660,18 +660,24 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 46e405526..c31217393 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -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(¤t_ekf_state, 0, sizeof(current_ekf_state)); } -void GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *state) { memcpy(state, ¤t_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)); } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index c5a5e9d8d..e62f1a98a 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -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]); + +static void quat2eul(float (&eul)[3], const float (&quat)[4]); -void quat2eul(float (&eul)[3], const float (&quat)[4]); +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); +static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); -void calcposNED(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); -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(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index c9d75bce4..840cd585e 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -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; - - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - InitialiseFilter(velNED); + } 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; + + _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; - - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + _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; + + _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); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 58393e013..3d897431a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1937,6 +1937,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); break; case MAVLINK_MODE_CAMERA: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1e6bd6f93..45b86a497 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,22 +262,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 4de722832..d432edd2b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t) if (_updated) { orb_copy(_topic, _fd, _data); + _published = true; return true; } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1581f30d3..c66350f5b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.chan[2].scaled = man.r / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f; - if (_rc_pub == 0) { + if (_rc_pub < 0) { _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { @@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; - if (_manual_pub == 0) { + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { @@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } @@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7..8ccb2a035 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -139,6 +138,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..5a94b6671 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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; } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d..56c5aa005 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -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; - - /* 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]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - 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]; - - /* - * 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 { - /* 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; + /* 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 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) { + + uint16_t raw = r_raw_rc_values[i]; + + 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]; + + /* + * 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 { + /* in the configured dead zone, output zero */ + scaled = 0; + } - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) { + scaled = -scaled; + } - /* 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) { - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* 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; + } + 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); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d48c6c529..a978d483a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 54c5663a5..4db948484 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 97d25bbfa..6752e7b4b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f07eea53..e62b0fafc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b41755de..a1a5856bc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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_ */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a..14f7ac812 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f..4083b8b4f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4b31cc8a4..c8a589bb7 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -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); diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758f..d2ee754cd 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -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 */ }; /** diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6..01e14cda9 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -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 */ }; diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h new file mode 100644 index 000000000..7763b6004 --- /dev/null +++ b/src/modules/uORB/topics/system_power.h @@ -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 |