diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 823b34a1a6..f73385f242 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -302,7 +302,7 @@ public: virtual bool get_position(struct Location &loc) const = 0; // return a wind estimation vector, in m/s - virtual Vector3f wind_estimate(void) = 0; + virtual Vector3f wind_estimate(void) const = 0; // return an airspeed estimate if available. return true // if we have an estimate @@ -448,17 +448,17 @@ public: static const struct AP_Param::GroupInfo var_info[]; // return secondary attitude solution if available, as eulers in radians - virtual bool get_secondary_attitude(Vector3f &eulers) { + virtual bool get_secondary_attitude(Vector3f &eulers) const { return false; } // return secondary attitude solution if available, as quaternion - virtual bool get_secondary_quaternion(Quaternion &quat) { + virtual bool get_secondary_quaternion(Quaternion &quat) const { return false; } // return secondary position solution if available - virtual bool get_secondary_position(struct Location &loc) { + virtual bool get_secondary_position(struct Location &loc) const { return false; } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 67418f351b..2c8bc2dd0f 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -99,7 +99,7 @@ public: } // return a wind estimation vector, in m/s - Vector3f wind_estimate() override { + Vector3f wind_estimate() const override { return _wind; } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index bbca2c9b54..1fb0dd813c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -434,7 +434,7 @@ float AP_AHRS_NavEKF::get_error_yaw(void) const } // return a wind estimation vector, in m/s -Vector3f AP_AHRS_NavEKF::wind_estimate(void) +Vector3f AP_AHRS_NavEKF::wind_estimate(void) const { Vector3f wind; switch (active_EKF_type()) { @@ -489,7 +489,7 @@ bool AP_AHRS_NavEKF::use_compass(void) // return secondary attitude solution if available, as eulers in radians -bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) +bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const { switch (active_EKF_type()) { case EKF_TYPE_NONE: @@ -510,7 +510,7 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) // return secondary attitude solution if available, as quaternion -bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) +bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const { switch (active_EKF_type()) { case EKF_TYPE_NONE: @@ -530,7 +530,7 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) } // return secondary position solution if available -bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) +bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const { switch (active_EKF_type()) { case EKF_TYPE_NONE: @@ -690,7 +690,7 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for. -bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) +bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const { switch (active_EKF_type()) { case EKF_TYPE_NONE: @@ -1175,7 +1175,7 @@ uint8_t AP_AHRS_NavEKF::setInhibitGPS(void) } // get speed limit -void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) +void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const { switch (ekf_type()) { case 0: @@ -1200,7 +1200,7 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel // get compass offset estimates // true if offsets are valid -bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) +bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const { switch (ekf_type()) { case 0: @@ -1381,7 +1381,7 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void) } // send a EKF_STATUS_REPORT for current EKF -void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) +void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const { switch (ekf_type()) { case EKF_TYPE_NONE: @@ -1556,7 +1556,7 @@ void AP_AHRS_NavEKF::setTouchdownExpected(bool val) } } -bool AP_AHRS_NavEKF::getGpsGlitchStatus() +bool AP_AHRS_NavEKF::getGpsGlitchStatus() const { nav_filter_status ekf_status {}; if (!get_filter_status(ekf_status)) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 0681902aa9..6b0a455b60 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -79,7 +79,7 @@ public: float get_error_yaw() const override; // return a wind estimation vector, in m/s - Vector3f wind_estimate() override; + Vector3f wind_estimate() const override; // return an airspeed estimate if available. return true // if we have an estimate @@ -104,13 +104,13 @@ public: } // return secondary attitude solution if available, as eulers in radians - bool get_secondary_attitude(Vector3f &eulers) override; + bool get_secondary_attitude(Vector3f &eulers) const override; // return secondary attitude solution if available, as quaternion - bool get_secondary_quaternion(Quaternion &quat) override; + bool get_secondary_quaternion(Quaternion &quat) const override; // return secondary position solution if available - bool get_secondary_position(struct Location &loc) override; + bool get_secondary_position(struct Location &loc) const override; // EKF has a better ground speed vector estimate Vector2f groundspeed_vector() override; @@ -156,7 +156,7 @@ public: // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate(float &velocity); + bool get_vert_pos_rate(float &velocity) const; // write optical flow measurements to EKF void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset); @@ -168,7 +168,7 @@ public: uint8_t setInhibitGPS(void); // get speed limit - void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler); + void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; void set_ekf_use(bool setting); @@ -183,7 +183,7 @@ public: // get compass offset estimates // true if offsets are valid - bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets); + bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; // report any reason for why the backend is refusing to initialise const char *prearm_failure_reason(void) const override; @@ -212,7 +212,7 @@ public: bool resetHeightDatum() override; // send a EKF_STATUS_REPORT for current EKF - void send_ekf_status_report(mavlink_channel_t chan); + void send_ekf_status_report(mavlink_channel_t chan) const; // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag // this is used to limit height during optical flow navigation @@ -240,7 +240,7 @@ public: void setTakeoffExpected(bool val); void setTouchdownExpected(bool val); - bool getGpsGlitchStatus(); + bool getGpsGlitchStatus() const; // used by Replay to force start at right timestamp void force_ekf_start(void) { _force_ekf = true; }