aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: move EKF objects in; this requires unconsting some methods

Browse Source
This commit is contained in:
Peter Barker 2019-12-10 20:32:52 +11:00 committed by Andrew Tridgell
parent d6dbdd58d3
commit 14fce28e1b
5 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS.h
View File

@ -465,13 +465,13 @@ public:
// return the amount of yaw angle change due to the last yaw angle reset in radians // return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
virtual uint32_t getLastYawResetAngle(float &yawAng) const { virtual uint32_t getLastYawResetAngle(float &yawAng) {
return 0; return 0;
}; };
// return the amount of NE position change in metres due to the last reset // return the amount of NE position change in metres due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) const WARN_IF_UNUSED { virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
return 0; return 0;
}; };
@ -483,7 +483,7 @@ public:
// return the amount of vertical position change due to the last reset in meters // return the amount of vertical position change due to the last reset in meters
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
virtual uint32_t getLastPosDownReset(float &posDelta) const WARN_IF_UNUSED { virtual uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
return 0; return 0;
}; };

12
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -35,12 +35,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// constructor // constructor
AP_AHRS_NavEKF::AP_AHRS_NavEKF(NavEKF2 &_EKF2, AP_AHRS_NavEKF::AP_AHRS_NavEKF(uint8_t flags) :
NavEKF3 &_EKF3,
uint8_t flags) :
AP_AHRS_DCM(), AP_AHRS_DCM(),
EKF2(_EKF2),
EKF3(_EKF3),
_ekf_flags(flags) _ekf_flags(flags)
{ {
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub) #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
@ -1447,7 +1443,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// return the amount of yaw angle change due to the last yaw angle reset in radians // return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) const uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
{ {
switch (ekf_type()) { switch (ekf_type()) {
@ -1470,7 +1466,7 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) const
// return the amount of NE position change in metres due to the last reset // return the amount of NE position change in metres due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) const uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos)
{ {
switch (ekf_type()) { switch (ekf_type()) {
@ -1517,7 +1513,7 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
// return the amount of vertical position change due to the last reset in meters // return the amount of vertical position change due to the last reset in meters
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) const uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta)
{ {
switch (ekf_type()) { switch (ekf_type()) {
case EKFType::NONE: case EKFType::NONE:

14
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -45,7 +45,7 @@ public:
}; };
// Constructor // Constructor
AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, uint8_t flags = FLAG_NONE); AP_AHRS_NavEKF(uint8_t flags = FLAG_NONE);
/* Do not allow copies */ /* Do not allow copies */
AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete; AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete;
@ -195,11 +195,11 @@ public:
// return the amount of yaw angle change due to the last yaw angle reset in radians // return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAng) const override; uint32_t getLastYawResetAngle(float &yawAng) override;
// return the amount of NE position change in meters due to the last reset // return the amount of NE position change in meters due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosNorthEastReset(Vector2f &pos) const override; uint32_t getLastPosNorthEastReset(Vector2f &pos) override;
// return the amount of NE velocity change in meters/sec due to the last reset // return the amount of NE velocity change in meters/sec due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
@ -207,7 +207,7 @@ public:
// return the amount of vertical position change due to the last reset in meters // return the amount of vertical position change due to the last reset in meters
// returns the time of the last reset or 0 if no reset has ever occurred // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosDownReset(float &posDelta) const override; uint32_t getLastPosDownReset(float &posDelta) override;
// Resets the baro so that it reads zero at the current height // Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero // Resets the EKF height to zero
@ -271,6 +271,10 @@ public:
// check whether compass can be bypassed for arming check in case when external navigation data is available // check whether compass can be bypassed for arming check in case when external navigation data is available
bool is_ext_nav_used_for_yaw(void) const; bool is_ext_nav_used_for_yaw(void) const;
// these are only out here so vehicles can reference them for parameters
NavEKF2 EKF2;
NavEKF3 EKF3;
private: private:
enum class EKFType { enum class EKFType {
NONE = 0, NONE = 0,
@ -286,8 +290,6 @@ private:
return _ekf_flags & FLAG_ALWAYS_USE_EKF; return _ekf_flags & FLAG_ALWAYS_USE_EKF;
} }
NavEKF2 &EKF2;
NavEKF3 &EKF3;
bool _ekf2_started; bool _ekf2_started;
bool _ekf3_started; bool _ekf3_started;
bool _force_ekf; bool _force_ekf;

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -144,11 +144,11 @@ public:
return ahrs.get_accel_ef_blended(); return ahrs.get_accel_ef_blended();
} }
uint32_t getLastPosNorthEastReset(Vector2f &pos) const WARN_IF_UNUSED { uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
return ahrs.getLastPosNorthEastReset(pos); return ahrs.getLastPosNorthEastReset(pos);
} }
uint32_t getLastPosDownReset(float &posDelta) const WARN_IF_UNUSED { uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
return ahrs.getLastPosDownReset(posDelta); return ahrs.getLastPosDownReset(posDelta);
} }

5
libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp
View File

@ -30,10 +30,7 @@ static AP_Logger logger{logger_bitmask};
class DummyVehicle { class DummyVehicle {
public: public:
NavEKF2 EKF2{&ahrs}; AP_AHRS_NavEKF ahrs{AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
NavEKF3 EKF3{&ahrs};
AP_AHRS_NavEKF ahrs{EKF2, EKF3,
AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
}; };
static DummyVehicle vehicle; static DummyVehicle vehicle;