From be03ed95fa7b1f3d2274fb202039d2399d32c566 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 22:40:00 -0800 Subject: [PATCH 1/6] Renamed min, max and abs macros to MIN MAX and ABS. --- src/main/blackbox/blackbox.c | 4 ++-- src/main/common/maths.c | 2 +- src/main/common/maths.h | 6 +++--- src/main/common/typeconversion.c | 2 +- src/main/drivers/compass_hmc5883l.c | 4 ++-- src/main/flight/altitudehold.c | 8 ++++---- src/main/flight/flight.c | 10 +++++----- src/main/flight/mixer.c | 2 +- src/main/flight/navigation.c | 16 ++++++++-------- src/main/io/display.c | 4 ++-- src/main/io/ledstrip.c | 8 ++++---- src/main/io/rc_controls.c | 4 ++-- src/main/io/serial_msp.c | 4 ++-- src/main/mw.c | 6 +++--- src/main/telemetry/frsky.c | 4 ++-- 15 files changed, 42 insertions(+), 42 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 447f791f19..251b42cda2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -894,7 +894,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) @@ -1133,7 +1133,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) { diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 5ed16f916d..15d02f8f66 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index d65b87df22..8dc281fee3 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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 { diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 7df5bb1370..4e3f8b4c2e 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -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 diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 63d0566c90..afa40d6c45 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -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. } diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index d2bb2b146c..f74a32c73e 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -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)); } diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index dbc049b3d3..7b994e5f6d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -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; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 324bdf12fc..18c9e380fc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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 diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index dd09430562..9c1e919cc8 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -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 diff --git a/src/main/io/display.c b/src/main/io/display.c index 001615e874..ef103fd44e 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -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); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 75eadc25da..1a8cd62608 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -775,10 +775,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; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 9bd4560085..64a70ba40e 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -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) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index e7b1d1a43b..5076986cbb 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -820,9 +820,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); diff --git a/src/main/mw.c b/src/main/mw.c index 9b2a45e0a6..f7081090b1 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index be9e0b830e..f30f961eb3 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -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 From d691f7284904c43c2e9a8bcdfd93da83307298fc Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:03:22 -0800 Subject: [PATCH 2/6] Moved configuring of IMU all into one function call. --- src/main/config/config.c | 7 +++---- src/main/flight/imu.c | 21 ++++++++++++++++----- src/main/flight/imu.h | 12 +++++++++--- 3 files changed, 28 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 17c656ddd9..010e686b08 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -616,7 +616,9 @@ void activateConfig(void) configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, - ¤tProfile->accDeadband + ¤tProfile->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(¤tProfile->barometerConfig); #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f0a4535a00..284d11ab0e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -79,11 +79,19 @@ 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, + uint16_t throttle_correction_angle +) { imuRuntimeConfig = initialImuRuntimeConfig; pidProfile = initialPidProfile; accDeadband = initialAccDeadband; + fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff); + throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } void initIMU() @@ -93,14 +101,17 @@ void initIMU() gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } -void calculateThrottleAngleScale(uint16_t throttle_correction_angle) +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 + return 0.5f / (M_PIf * accz_lpf_cutoff); } void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 552093508d..cf03c8e7f0 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,12 +30,18 @@ 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); void accSum_reset(void); From 8d994df45757fb6bd046a14e5e65842618943775 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:06:46 -0800 Subject: [PATCH 3/6] Move code around to avoid forward declaration. --- src/main/flight/imu.c | 52 +++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 27 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 284d11ab0e..4b4bbb47bb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -73,8 +73,6 @@ 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; @@ -114,31 +112,6 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) return 0.5f / (M_PIf * accz_lpf_cutoff); } -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]; - } -} - // ************************************************** // Simplified IMU based on "Complementary Filter" // Inspired by http://starlino.com/imu_guide.html @@ -309,6 +282,31 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame } +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]; + } +} + // Correction of throttle in lateral wind. int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { From f312636b9fef90977a52dcd721791be1f5508196 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:08:38 -0800 Subject: [PATCH 4/6] Add comments suggesting moving throttle angle correction code into own module. --- src/main/flight/imu.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 4b4bbb47bb..307b7205a2 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -82,6 +82,7 @@ void configureIMU( 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 ) { @@ -89,6 +90,7 @@ void configureIMU( 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); } @@ -99,6 +101,7 @@ void initIMU() gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } +//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); @@ -307,7 +310,7 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) } } -// Correction of throttle in lateral wind. +//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); From 6a0d1b84f2ca3d29fbddf18ca59993e2f7354f1d Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:27:53 -0800 Subject: [PATCH 5/6] Add tests for the calculateHeading method of the IMU. --- src/test/unit/flight_imu_unittest.cc | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 44aada0687..d82d12fb1d 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -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 From 67a2d5cd756b2b536447e12462e99e0a27a544b5 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:29:45 -0800 Subject: [PATCH 6/6] Add test for calculate heading. --- src/main/flight/imu.c | 28 +++++++++++++++++++++++++--- src/main/flight/imu.h | 2 ++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 307b7205a2..3428f73412 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -188,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; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index cf03c8e7f0..45949c4e1e 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -44,4 +44,6 @@ float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); +int16_t calculateHeading(t_fp_vector *vec); + void accSum_reset(void);