Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

add suggestions from recent reviews

Browse source
This commit is contained in:
ctzsnooze 2024-11-09 19:34:20 +11:00
parent b07c781461
commit f988599f77
14 changed files with 33 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/common/axis.h
View file

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

1
src/main/fc/rc.c
View file

@ -27,7 +27,6 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/vector.h"

3
src/main/fc/rc_controls.h
View file

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

24
src/main/flight/autopilot.c
View file

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

2
src/main/flight/autopilot.h
View file

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

2
src/main/flight/gps_rescue.c
View file

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

2
src/main/flight/gps_rescue.h
View file

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

6
src/main/flight/pid.h
View file

@ -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

9
src/main/io/gps.c
View file

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

2
src/main/io/gps.h
View file

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

2
src/main/osd/osd_elements.c
View file

@ -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

2
src/main/sensors/acceleration.h
View file

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

2
src/test/unit/althold_unittest.cc
View file

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

2
src/test/unit/arming_prevention_unittest.cc
View file

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