From 7e486a646d9f68799b993ce5c3f5ccb904d8869e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 28 Jul 2021 13:28:37 +1000 Subject: [PATCH 1/2] Plane: remove unimplemented function declaration --- ArduPlane/Plane.h | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 21670bb1ee37e..6a9c522f1c1ec 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -866,7 +866,6 @@ class Plane : public AP_Vehicle { void Log_Write_Status(); void Log_Write_RC(void); void Log_Write_Vehicle_Startup_Messages(); - void Log_Write_AOA_SSA(); void Log_Write_AETR(); void Log_Write_MavCmdI(const mavlink_command_int_t &packet); void log_init(); From 1147009fc3abb878eed800801e94f71d15eb9625 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Jul 2021 21:54:35 +1000 Subject: [PATCH 2/2] AP_AHRS: move AOA/SSA to AP_AHRS, don't recalc on fetch --- libraries/AP_AHRS/AP_AHRS.cpp | 3 +++ libraries/AP_AHRS/AP_AHRS.h | 12 ++++++++++++ libraries/AP_AHRS/AP_AHRS_Backend.cpp | 16 +--------------- libraries/AP_AHRS/AP_AHRS_Backend.h | 9 --------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 3 --- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 2 +- 6 files changed, 17 insertions(+), 28 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index d1e6d5599dd6a..c946325046568 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -320,6 +320,9 @@ void AP_AHRS::update(bool skip_ins_update) _view->update(skip_ins_update); } + // update AOA and SSA + update_AOA_SSA(); + #if HAL_NMEA_OUTPUT_ENABLED // update NMEA output if (_nmea_out != nullptr) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 1367cd3edd97f..6ac536a7e3b1a 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -324,6 +324,18 @@ class AP_AHRS : public AP_AHRS_DCM { // create a view AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0); + // write AOA and SSA information to dataflash logs: + void Write_AOA_SSA(void) const; + + // update AOA and SSA values + virtual void update_AOA_SSA(void); + + // return AOA + float getAOA(void) const { return _AOA; } + + // return SSA + float getSSA(void) const { return _SSA; } + protected: // optional view class AP_AHRS_View *_view; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 677670fae1def..028aa04703cd0 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -248,7 +248,7 @@ AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg) * "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by * JOSEPH E. ZEIS, JR., CAPTAIN, USAF */ -void AP_AHRS_Backend::update_AOA_SSA(void) +void AP_AHRS::update_AOA_SSA(void) { #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) const uint32_t now = AP_HAL::millis(); @@ -294,20 +294,6 @@ void AP_AHRS_Backend::update_AOA_SSA(void) #endif } -// return current AOA -float AP_AHRS_Backend::getAOA(void) -{ - update_AOA_SSA(); - return _AOA; -} - -// return calculated SSA -float AP_AHRS_Backend::getSSA(void) -{ - update_AOA_SSA(); - return _SSA; -} - // rotate a 2D vector from earth frame to body frame Vector2f AP_AHRS_Backend::earth_to_body2D(const Vector2f &ef) const { diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 8b3f127b78bc9..d0f3f3f7234aa 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -514,12 +514,6 @@ class AP_AHRS_Backend AP::ins().get_delta_velocity(ret, dt); } - // return calculated AOA - float getAOA(void); - - // return calculated SSA - float getSSA(void); - // rotate a 2D vector from earth frame to body frame // in result, x is forward, y is right Vector2f earth_to_body2D(const Vector2f &ef_vector) const; @@ -537,8 +531,6 @@ class AP_AHRS_Backend Vector3f earth_to_body(const Vector3f &v) const { return get_rotation_body_to_ned().mul_transpose(v); } - - virtual void update_AOA_SSA(void); // get_hgt_ctrl_limit - get maximum height to be observed by the // control loops in meters and a validity flag. It will return @@ -571,7 +563,6 @@ class AP_AHRS_Backend // Logging to disk functions void Write_AHRS2(void) const; - void Write_AOA_SSA(void); // should be const? but it calls update functions void Write_Attitude(const Vector3f &targets) const; void Write_Origin(uint8_t origin_type, const Location &loc) const; void Write_POS(void) const; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 658a8e28998fe..5b7e334dfc435 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -112,9 +112,6 @@ AP_AHRS_DCM::update(bool skip_ins_update) // update trig values including _cos_roll, cos_pitch update_trig(); - // update AOA and SSA - update_AOA_SSA(); - backup_attitude(); // update takeoff/touchdown flags diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 89b205f77ad40..92ac94a279477 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -32,7 +32,7 @@ void AP_AHRS_Backend::Write_AHRS2() const } // Write AOA and SSA -void AP_AHRS_Backend::Write_AOA_SSA(void) +void AP_AHRS::Write_AOA_SSA(void) const { const struct log_AOA_SSA aoa_ssa{ LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),