From f988599f77f01a6d1f842d26e37b0ac485fc9ea2 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 9 Nov 2024 19:34:20 +1100 Subject: [PATCH] add suggestions from recent reviews --- src/main/common/axis.h | 3 +++ src/main/fc/rc.c | 1 - src/main/fc/rc_controls.h | 3 ++- src/main/flight/autopilot.c | 24 ++++++++++----------- src/main/flight/autopilot.h | 2 +- src/main/flight/gps_rescue.c | 2 +- src/main/flight/gps_rescue.h | 2 +- src/main/flight/pid.h | 6 +++--- src/main/io/gps.c | 9 ++++---- src/main/io/gps.h | 2 +- src/main/osd/osd_elements.c | 2 +- src/main/sensors/acceleration.h | 2 +- src/test/unit/althold_unittest.cc | 2 +- src/test/unit/arming_prevention_unittest.cc | 2 +- 14 files changed, 33 insertions(+), 29 deletions(-) diff --git a/src/main/common/axis.h b/src/main/common/axis.h index 8cc8054fa0..be3fc6b944 100644 --- a/src/main/common/axis.h +++ b/src/main/common/axis.h @@ -42,4 +42,7 @@ typedef enum { AI_PITCH } angle_index_t; +#define RP_AXIS_COUNT 2 +#define EF_AXIS_COUNT 2 + #define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 850299f3fa..3fa890addd 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -27,7 +27,6 @@ #include "build/debug.h" -#include "common/axis.h" #include "common/utils.h" #include "common/vector.h" diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index c2f8f9f78a..1af3349721 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -22,6 +22,7 @@ #include +#include "common/axis.h" #include "common/filter.h" #include "pg/pg.h" @@ -87,7 +88,7 @@ extern float rcCommand[4]; typedef struct rcSmoothingFilter_s { bool filterInitialized; pt3Filter_t filterSetpoint[4]; - pt3Filter_t filterRcDeflection[2]; + pt3Filter_t filterRcDeflection[RP_AXIS_COUNT]; pt3Filter_t filterFeedforward[3]; uint8_t setpointCutoffSetting; diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index df73e45dde..98f9779212 100644 --- a/src/main/flight/autopilot.c +++ b/src/main/flight/autopilot.c @@ -22,6 +22,7 @@ #include "platform.h" #include "build/debug.h" +#include "common/axis.h" #include "common/filter.h" #include "common/maths.h" #include "common/vector.h" @@ -43,7 +44,7 @@ #define POSITION_I_SCALE 0.0001f #define POSITION_D_SCALE 0.0015f #define POSITION_A_SCALE 0.0008f -#define UPSAMPLING_CUTOFF 5.0f +#define UPSAMPLING_CUTOFF_HZ 5.0f static pidCoefficient_t altitudePidCoeffs; static pidCoefficient_t positionPidCoeffs; @@ -75,9 +76,9 @@ typedef struct { float pt1Gain; bool sticksActive; float maxAngle; - float pidSumCraft[2]; - pt3Filter_t upsample[2]; - earthFrame_t efAxis[2]; + float pidSumCraft[EF_AXIS_COUNT]; + pt3Filter_t upsample[EF_AXIS_COUNT]; + earthFrame_t efAxis[EF_AXIS_COUNT]; } posHoldState; static posHoldState posHold = { @@ -93,7 +94,7 @@ static posHoldState posHold = { }; static gpsLocation_t currentTargetLocation = {0, 0, 0}; -float autopilotAngle[2]; +float autopilotAngle[RP_AXIS_COUNT]; void resetPositionControlEFParams(earthFrame_t *efAxis) { @@ -133,7 +134,7 @@ void autopilotInit(const autopilotConfig_t *config) positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE; positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration // initialise filters with approximate filter gain - float upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF, 0.01f); // 5Hz, assuming 100Hz task rate + float upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate pt3FilterInit(&posHold.upsample[AI_ROLL], upsampleCutoff); pt3FilterInit(&posHold.upsample[AI_PITCH], upsampleCutoff); // Initialise PT1 filters for earth frame axes NS and EW @@ -215,8 +216,8 @@ void resetLocation(earthFrame_t *efAxis, axisEF_t loopAxis) bool positionControl(void) { static uint16_t previousGpsStamp = ~0; - if (currentGpsStamp() != previousGpsStamp) { - previousGpsStamp = currentGpsStamp(); + if (getGpsStamp() != previousGpsStamp) { + previousGpsStamp = getGpsStamp(); posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS; @@ -302,7 +303,6 @@ bool positionControl(void) // ** PID Sum ** efAxis->pidSum = pidP + pidI + pidDA; - // Debugs... distances in cm, angles in degrees * 10, velocities cm/2 if (gyroConfig()->gyro_filter_debug_axis == loopAxis) { DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceCm)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10)); @@ -342,9 +342,9 @@ bool positionControl(void) // note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) { - DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[EW].distance)); - DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[EW].pidSum * 10)); - DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10)); + DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[EW].distance)); // cm + DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[EW].pidSum * 10)); // deg * 10 + DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10)); // deg * 10 } else { DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[NS].distance)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[NS].pidSum * 10)); diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index 9f8f522990..85277eef6f 100644 --- a/src/main/flight/autopilot.h +++ b/src/main/flight/autopilot.h @@ -21,7 +21,7 @@ #include "flight/pid.h" #include "io/gps.h" -extern float autopilotAngle[2]; // NOTE: ANGLES ARE IN CENTIDEGREES +extern float autopilotAngle[RP_AXIS_COUNT]; // Roll and pitch angles in degrees void autopilotInit(const autopilotConfig_t *config); void resetAltitudeControl(void); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index a738095f88..5a4e81e764 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -123,7 +123,7 @@ typedef struct { static const float taskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); // i.e. 0.01 s static float rescueThrottle; static float rescueYaw; -float gpsRescueAngle[2] = { 0, 0 }; +float gpsRescueAngle[RP_AXIS_COUNT] = { 0, 0 }; bool magForceDisable = false; static bool newGPSData = false; static pt1Filter_t velocityDLpf; diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 845bfc7472..0128ec887c 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -45,7 +45,7 @@ typedef enum { GPS_RESCUE_ALT_MODE_COUNT } gpsRescueAltitudeMode_e; -extern float gpsRescueAngle[2]; // NOTE: ANGLES ARE IN CENTIDEGREES +extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES void gpsRescueInit(void); void gpsRescueUpdate(void); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 932d45a3c9..f1d077bf6b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -462,7 +462,7 @@ typedef struct pidRuntime_s { uint8_t acroTrainerDebugAxis; float acroTrainerGain; bool acroTrainerActive; - int acroTrainerAxisState[2]; // only need roll and pitch + int acroTrainerAxisState[RP_AXIS_COUNT]; // only need roll and pitch #endif #ifdef USE_DYN_LPF @@ -508,12 +508,12 @@ typedef struct pidRuntime_s { #endif #ifdef USE_ACC - pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH + pt3Filter_t attitudeFilter[RP_AXIS_COUNT]; // Only for ROLL and PITCH pt1Filter_t horizonSmoothingPt1; uint16_t horizonDelayMs; float angleYawSetpoint; float angleEarthRef; - float angleTarget[2]; + float angleTarget[RP_AXIS_COUNT]; bool axisInAngleMode[3]; #endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 4b51596141..9691d2edda 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -2555,8 +2555,9 @@ void GPS_reset_home_position(void) //////////////////////////////////////////////////////////////////////////////////// #define EARTH_ANGLE_TO_CM (111.3195f * 1000 * 100 / GPS_DEGREES_DIVIDER) // latitude unit to cm at equator (111km/deg) -// Get distance between two points in cm -// Get bearing from pos1 to pos2, returns an 1deg = 100 precision +// Get distance between two points in cm using spherical to Cartesian transform +// One one latitude unit, or one longitude unit at the equator, equals 1.113195 cm. +// Get bearing from pos1 to pos2, returns values with 0.01 degree precision void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t* to, bool dist3d, uint32_t *pDist, int32_t *pBearing) { float dLat = (to->lat - from->lat) * EARTH_ANGLE_TO_CM; @@ -2607,7 +2608,7 @@ void onGpsNewData(void) return; } - gpsStamp ++; // increment the stamp + gpsStamp++; // increment the stamp gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f; @@ -2626,7 +2627,7 @@ void onGpsNewData(void) } -uint16_t currentGpsStamp(void) { +uint16_t getGpsStamp(void) { return gpsStamp; } diff --git a/src/main/io/gps.h b/src/main/io/gps.h index b9fe27c0b8..077c8f7e66 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -393,6 +393,6 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pE void gpsSetFixState(bool state); float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue -uint16_t currentGpsStamp(void); +uint16_t getGpsStamp(void); baudRate_e getGpsPortActualBaudRateIndex(void); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index b98522b236..a3a66334e2 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -769,7 +769,7 @@ static void osdElementUpDownReference(osdElementParms_t *element) int direction; if(attitude.values.pitch>0.0){ //nose down - thetaB = -earthUpinBodyFrame[2]; // get pitch w/re to nadir (use small angle approx for sine) + thetaB = -earthUpinBodyFrame[RP_AXIS_COUNT]; // get pitch w/re to nadir (use small angle approx for sine) psiB = -earthUpinBodyFrame[1]; // calculate the yaw w/re to nadir (use small angle approx for sine) direction = DOWN; } else { // nose up diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index cd2de0f585..602e861158 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -68,7 +68,7 @@ typedef struct rollAndPitchTrims_s { } rollAndPitchTrims_t_def; typedef union rollAndPitchTrims_u { - int16_t raw[2]; + int16_t raw[RP_AXIS_COUNT]; rollAndPitchTrims_t_def values; } rollAndPitchTrims_t; diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc index ee2c5b8b43..03b62b7bed 100644 --- a/src/test/unit/althold_unittest.cc +++ b/src/test/unit/althold_unittest.cc @@ -114,7 +114,7 @@ extern "C" { float getAltitudeDerivative(void) {return 0.0f;} float getCosTiltAngle(void) { return 0.0f; } float getGpsDataIntervalSeconds(void) { return 0.01f; }// gpsSolutionData_t gpsSol; - uint16_t currentGpsStamp(void){ return 0; } + uint16_t getGpsStamp(void){ return 0; } float rcCommand[4]; diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index d89796aadb..2011166ca5 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1196,5 +1196,5 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pE bool canUseGPSHeading; bool compassIsHealthy; - uint16_t currentGpsStamp(void){ return 0; } + uint16_t getGpsStamp(void){ return 0; } }