Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Merge branch 'imu-cleanup' of https://github.com/avoid3d/cleanflight into avoid3d-imu-cleanup

Browse source
This commit is contained in:
Dominic Clifton 2015-01-25 01:00:14 +01:00
commit 3fc7f32324
19 changed files with 143 additions and 88 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/blackbox/blackbox.c
View file

@ -912,7 +912,7 @@ static void configureBlackboxPort(void)
*
* 9 / 1250 = 7200 / 1000000
*/
serialChunkSize = max((masterConfig.looptime * 9) / 1250, 4);
serialChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4);
}
static void releaseBlackboxPort(void)
@ -1163,7 +1163,7 @@ static bool blackboxWriteSysinfo()
}
// How many bytes can we afford to transmit this loop?
xmitState.u.serialBudget = min(xmitState.u.serialBudget + serialChunkSize, 64);
xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + serialChunkSize, 64);
// Most headers will consume at least 20 bytes so wait until we've built up that much link budget
if (xmitState.u.serialBudget < 20) {

2
src/main/common/maths.c
View file

@ -22,7 +22,7 @@
int32_t applyDeadband(int32_t value, int32_t deadband)
{
if (abs(value) < deadband) {
if (ABS(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;

6
src/main/common/maths.h
View file

@ -26,9 +26,9 @@
#define RAD (M_PIf / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define ABS(x) ((x) > 0 ? (x) : -(x))
typedef struct stdev_t
{

2
src/main/common/typeconversion.c
View file

@ -159,7 +159,7 @@ char *ftoa(float x, char *floatString)
value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer
itoa(abs(value), intString1, 10); // Create string from abs of integer value
itoa(ABS(value), intString1, 10); // Create string from abs of integer value
if (value >= 0)
intString2[0] = ' '; // Positive number, add a pad space

7
src/main/config/config.c
View file

@ -616,7 +616,9 @@ void activateConfig(void)
configureIMU(
&imuRuntimeConfig,
&currentProfile->pidProfile,
&currentProfile->accDeadband
&currentProfile->accDeadband,
currentProfile->accz_lpf_cutoff,
currentProfile->throttle_correction_angle
);
configureAltitudeHold(
@ -626,9 +628,6 @@ void activateConfig(void)
&masterConfig.escAndServoConfig
);
calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);
#ifdef BARO
useBarometerConfig(&currentProfile->barometerConfig);
#endif

4
src/main/drivers/compass_hmc5883l.c
View file

@ -177,7 +177,7 @@ void hmc5883lInit(void)
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
@ -197,7 +197,7 @@ void hmc5883lInit(void)
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}

8
src/main/flight/altitudehold.c
View file

@ -91,7 +91,7 @@ static void applyMultirotorAltHold(void)
// multirotor alt hold
if (rcControlsConfig->alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
@ -104,7 +104,7 @@ static void applyMultirotorAltHold(void)
}
} else {
// slow alt changes, mostly used for aerial photography
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
@ -172,12 +172,12 @@ void updateSonarAltHoldState(void)
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
{
return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
}
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
{
return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees));
}

10
src/main/flight/flight.c
View file

@ -117,11 +117,11 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
if(abs(stickPosAil) > abs(stickPosEle)){
mostDeflectedPos = abs(stickPosAil);
if(ABS(stickPosAil) > ABS(stickPosEle)){
mostDeflectedPos = ABS(stickPosAil);
}
else {
mostDeflectedPos = abs(stickPosEle);
mostDeflectedPos = ABS(stickPosEle);
}
// Progressively turn off the horizon self level strength as the stick is banged over
@ -220,7 +220,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
UNUSED(controlRateConfig);
// **** PITCH & ROLL & YAW PID ****
prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500]
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
@ -252,7 +252,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if ((abs(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && abs(rcCommand[axis]) > 100))
if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;

106
src/main/flight/imu.c
View file

@ -73,17 +73,25 @@ int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
static void getEstimatedAttitude(void);
imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile;
accDeadband_t *accDeadband;
void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
void configureIMU(
imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
float accz_lpf_cutoff,
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
uint16_t throttle_correction_angle
)
{
imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
}
void initIMU()
@ -93,39 +101,18 @@ void initIMU()
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
}
void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
/*
* Calculate RC time constant used in the accZ lpf.
*/
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
}
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
{
static int16_t gyroYawSmooth = 0;
gyroGetADC();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims);
getEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
if (mixerMode == MIXER_TRI) {
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
gyroYawSmooth = gyroData[FD_YAW];
} else {
gyroData[FD_YAW] = gyroADC[FD_YAW];
}
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
// **************************************************
@ -201,11 +188,33 @@ void acc_calc(uint32_t deltaT)
/*
* Baseflight calculation by Luggi09 originates from arducopter
* ============================================================
* This function turns a vector which is (usually) the direction
* of magnetic flux in the coordinate system of the craft into
* a compass heading in degrees, clockwise away from north. Note
* that the magnetic flux is not parrelell with the vector towards
* magnetic north it's self but rather is parrelell with the ground
* when near the equator.
*
* Calculate the heading of the craft (in degrees clockwise from North)
* when given a 3-vector representing the direction of North.
* First we consider it in 2D:
*
* An example, the vector <1, 1> would be turned into the heading
* 45 degrees, representing it's angle clockwise from north.
*
* ***************** *
* * | <1,1> *
* * | / *
* * | / *
* * |/ *
* * * *
* * *
* * *
* * *
* * *
* *******************
*
* //TODO: Add explanation for how it uses the Z dimension.
*/
static int16_t calculateHeading(t_fp_vector *vec)
int16_t calculateHeading(t_fp_vector *vec)
{
int16_t head;
@ -298,7 +307,32 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame
}
// Correction of throttle in lateral wind.
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
{
static int16_t gyroYawSmooth = 0;
gyroGetADC();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims);
getEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
if (mixerMode == MIXER_TRI) {
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
gyroYawSmooth = gyroData[FD_YAW];
} else {
gyroData[FD_YAW] = gyroADC[FD_YAW];
}
}
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);

14
src/main/flight/imu.h
View file

@ -30,12 +30,20 @@ typedef struct imuRuntimeConfig_s {
int8_t small_angle;
} imuRuntimeConfig_t;
void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
void configureIMU(
imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
float accz_lpf_cutoff,
uint16_t throttle_correction_angle
);
void calculateEstimatedAltitude(uint32_t currentTime);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
int16_t calculateHeading(t_fp_vector *vec);
void accSum_reset(void);

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

@ -509,7 +509,7 @@ void mixTable(void)
if (motorCount > 3) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW]));
}
// motors for non-servo mixes

16
src/main/flight/navigation.c
View file

@ -306,7 +306,7 @@ void onGpsNewData(void)
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0f);
dTnav = MIN(dTnav, 1.0f);
GPS_calculateDistanceAndDirectionToHome();
@ -413,7 +413,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
//
static void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (abs((float)lat) / 10000000.0f) * 0.0174532925f;
float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cosf(rads);
}
@ -442,7 +442,7 @@ static bool check_missed_wp(void)
int32_t temp;
temp = target_bearing - original_target_bearing;
temp = wrap_18000(temp);
return (abs(temp) > 10000); // we passed the waypoint by 100 degrees
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
}
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
@ -536,7 +536,7 @@ static void GPS_calc_poshold(void)
// get rid of noise
#if defined(GPS_LOW_SPEED_D_FILTER)
if (abs(actual_speed[axis]) < 50)
if (ABS(actual_speed[axis]) < 50)
d = 0;
#endif
@ -582,7 +582,7 @@ static void GPS_calc_nav_rate(uint16_t max_speed)
//
static void GPS_update_crosstrack(void)
{
if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
@ -607,10 +607,10 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
{
// max_speed is default 400 or 4m/s
if (_slow) {
max_speed = min(max_speed, wp_distance / 2);
max_speed = MIN(max_speed, wp_distance / 2);
} else {
max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s
max_speed = MIN(max_speed, wp_distance);
max_speed = MAX(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s
}
// limit the ramp up of the speed

4
src/main/io/display.c
View file

@ -292,12 +292,12 @@ void showProfilePage(void)
void showGpsPage() {
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
i2c_OLED_set_xy(max(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphValue = min(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue);
}

8
src/main/io/ledstrip.c
View file

@ -892,10 +892,10 @@ void updateLedStrip(void)
if (indicatorFlashNow) {
uint8_t rollScale = abs(rcCommand[ROLL]) / 50;
uint8_t pitchScale = abs(rcCommand[PITCH]) / 50;
uint8_t scale = max(rollScale, pitchScale);
nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale));
uint8_t rollScale = ABS(rcCommand[ROLL]) / 50;
uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50;
uint8_t scale = MAX(rollScale, pitchScale);
nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale));
if (indicatorFlashState == 0) {
indicatorFlashState = 1;

4
src/main/io/rc_controls.c
View file

@ -64,7 +64,7 @@ bool isUsingSticksForArming(void)
bool areSticksInApModePosition(uint16_t ap_mode)
{
return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@ -518,7 +518,7 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return min(abs(rcData[axis] - midrc), 500);
return MIN(ABS(rcData[axis] - midrc), 500);
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)

4
src/main/io/serial_msp.c
View file

@ -827,9 +827,9 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery
serialize16(rssi);
if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
} else
serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
break;
case MSP_RC_TUNING:
headSerialReply(7);

6
src/main/mw.c
View file

@ -182,7 +182,7 @@ void annexCode(void)
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
@ -205,7 +205,7 @@ void annexCode(void)
}
}
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * abs(tmp) / 500;
prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
@ -397,7 +397,7 @@ void updateInflightCalibrationState(void)
void updateMagHold(void)
{
if (abs(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
int16_t dif = heading - magHold;
if (dif <= -180)
dif += 360;

4
src/main/telemetry/frsky.c
View file

@ -161,7 +161,7 @@ static void sendBaro(void)
sendDataHead(ID_ALTITUDE_BP);
serialize16(BaroAlt / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16(abs(BaroAlt % 100));
serialize16(ABS(BaroAlt % 100));
}
static void sendGpsAltitude(void)
@ -247,7 +247,7 @@ static void sendTime(void)
static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
{
int32_t absgps, deg, min;
absgps = abs(mwiigps);
absgps = ABS(mwiigps);
deg = absgps / GPS_DEGREES_DIVIDER;
absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
min = absgps / GPS_DEGREES_DIVIDER; // minutes left

20
src/test/unit/flight_imu_unittest.cc
View file

@ -24,6 +24,7 @@
extern "C" {
#include "common/axis.h"
#include "common/maths.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
@ -48,10 +49,23 @@ extern "C" {
#define UPWARDS_THRUST false
TEST(FlightImuTest, Placeholder)
TEST(FlightImuTest, TestCalculateHeading)
{
// TODO test things
EXPECT_EQ(true, true);
//TODO: Add test cases using the Z dimension.
t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&north), 0);
t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&east), 90);
t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&south), 180);
t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&west), 270);
t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&north_east), 45);
}
// STUBS