Browse Source

AP_AHRS : temporary mods to test use of flow sensor internal gyro data

mission-4.1.18
priseborough 11 years ago committed by Andrew Tridgell
parent
commit
12b012a00e
  1. 4
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
  2. 2
      libraries/AP_AHRS/AP_AHRS_NavEKF.h

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -318,9 +318,9 @@ bool AP_AHRS_NavEKF::initialised(void) const @@ -318,9 +318,9 @@ bool AP_AHRS_NavEKF::initialised(void) const
};
// write optical flow data to EKF
void AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas)
void AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas)
{
EKF.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawSonarRange, msecFlowMeas);
EKF.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawSonarRange, msecFlowMeas);
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

2
libraries/AP_AHRS/AP_AHRS_NavEKF.h

@ -102,7 +102,7 @@ public: @@ -102,7 +102,7 @@ public:
bool get_relative_position_NED(Vector3f &vec) const;
// write optical flow measurements to EKF
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas);
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas);
void set_ekf_use(bool setting) { _ekf_use.set(setting); }

Loading…
Cancel
Save