add suggestions from recent reviews

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)

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"
 

src/main/fc/rc_controls.h

@@ -22,6 +22,7 @@
 
 #include <stdbool.h>
 
+#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;

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];
+    float pidSumCraft[EF_AXIS_COUNT];
-    pt3Filter_t upsample[2];
+    pt3Filter_t upsample[EF_AXIS_COUNT];
-    earthFrame_t efAxis[2];
+    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) {
+    if (getGpsStamp() != previousGpsStamp) {
-        previousGpsStamp = currentGpsStamp();
+        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, 1, lrintf(posHold.efAxis[EW].distance));    // cm
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[EW].pidSum * 10));
+        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));
+        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));

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);

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;

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);

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
 

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 distance between two points in cm using spherical to Cartesian transform
-// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
+// 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;
 }
 

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);

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

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;
 

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];
 

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; }
 }