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

Quaternion-based DCM IMU (original code my S.Madgwick)

Browse source 
Restore binaries from merge
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-10-22 13:36:17 +10:00 • committed by borisbstyle
parent 846b86f489
commit 5df8ca926c
21 changed files with 379 additions and 738 deletions
Download patch file Download diff file Expand all files Collapse all files

0
obj/betaflight_CC3D.hex Normal file → Executable file
View file

0
obj/betaflight_NAZE.hex Normal file → Executable file
View file

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

@ -397,8 +397,8 @@ static void resetConf(void)
// global settings // global settings
masterConfig.current_profile_index = 0; // default profile masterConfig.current_profile_index = 0; // default profile
masterConfig.gyro_cmpf_factor = 600; // default MWC masterConfig.dcm_kp = 10000; // 1.0 * 10000
masterConfig.gyro_cmpfm_factor = 250; // default MWC masterConfig.dcm_ki = 30; // 0.003 * 10000
resetAccelerometerTrims(&masterConfig.accZero); resetAccelerometerTrims(&masterConfig.accZero);
@ -484,15 +484,14 @@ static void resetConf(void)
resetRollAndPitchTrims(&currentProfile->accelerometerTrims); resetRollAndPitchTrims(&currentProfile->accelerometerTrims);
currentProfile->mag_declination = 0; currentProfile->mag_declination = 0;
currentProfile->acc_lpf_factor = 4; currentProfile->acc_cut_hz = 15;
currentProfile->accz_lpf_cutoff = 5.0f; currentProfile->accz_lpf_cutoff = 5.0f;
currentProfile->accDeadband.xy = 40; currentProfile->accDeadband.xy = 40;
currentProfile->accDeadband.z = 40; currentProfile->accDeadband.z = 40;
currentProfile->acc_unarmedcal = 1;
resetBarometerConfig(&currentProfile->barometerConfig); resetBarometerConfig(&currentProfile->barometerConfig);
currentProfile->acc_unarmedcal = 1;
// Radio // Radio
parseRcChannels("AETR1234", &masterConfig.rxConfig); parseRcChannels("AETR1234", &masterConfig.rxConfig);
@ -719,10 +718,10 @@ void activateConfig(void)
&masterConfig.rxConfig &masterConfig.rxConfig
); );
imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor; imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor; imuRuntimeConfig.acc_cut_hz = currentProfile->acc_cut_hz;
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;
imuRuntimeConfig.small_angle = masterConfig.small_angle; imuRuntimeConfig.small_angle = masterConfig.small_angle;
imuConfigure( imuConfigure(

4
src/main/config/config_master.h
View file

@ -48,8 +48,8 @@ typedef struct master_t {
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
gyroConfig_t gyroConfig; gyroConfig_t gyroConfig;

2
src/main/config/config_profile.h
View file

@ -28,7 +28,7 @@ typedef struct profile_s {
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
// sensor-related stuff // sensor-related stuff
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter uint8_t acc_cut_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
accDeadband_t accDeadband; accDeadband_t accDeadband;

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

@ -168,9 +168,9 @@ void updateSonarAltHoldState(void)
} }
} }
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude)
{ {
return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES;
} }
/* /*
@ -178,9 +178,9 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft * //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
* (my best interpretation of scalar 'tiltAngle') or rename the function. * (my best interpretation of scalar 'tiltAngle') or rename the function.
*/ */
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination) int16_t calculateTiltAngle(attitudeEulerAngles_t *attitude)
{ {
return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees)); return MAX(ABS(attitude->values.roll), ABS(attitude->values.pitch));
} }
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old) int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
@ -189,7 +189,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
int32_t error; int32_t error;
int32_t setVel; int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) { if (!isThrustFacingDownwards(&attitude)) {
return result; return result;
} }
@ -260,7 +260,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
#endif #endif
#ifdef SONAR #ifdef SONAR
tiltAngle = calculateTiltAngle(&inclination); tiltAngle = calculateTiltAngle(&attitude);
sonarAlt = sonarRead(); sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle); sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
#endif #endif

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

File diff suppressed because it is too large Load diff

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

@ -18,26 +18,27 @@
#pragma once #pragma once
extern int16_t throttleAngleCorrection; extern int16_t throttleAngleCorrection;
extern uint32_t accTimeSum; // altitudehold.c extern uint32_t accTimeSum;
extern int accSumCount; // altitudehold.c extern int accSumCount;
extern float accVelScale; // altitudehold.c extern float accVelScale;
extern t_fp_vector EstG; // display.c extern int16_t accSmooth[XYZ_AXIS_COUNT];
extern int16_t accSmooth[XYZ_AXIS_COUNT]; // blackbox.c, display.c, serial_msp.c, frsky.c, smartport.c extern int32_t accSum[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT]; // altitudehold.c
extern int16_t smallAngle; // display.c
typedef struct rollAndPitchInclination_s { #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 #define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
int16_t rollDeciDegrees; #define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
int16_t pitchDeciDegrees;
} rollAndPitchInclination_t_def;
typedef union { typedef union {
int16_t raw[ANGLE_INDEX_COUNT]; int16_t raw[XYZ_AXIS_COUNT];
rollAndPitchInclination_t_def values; struct {
} rollAndPitchInclination_t; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t roll;
int16_t pitch;
int16_t yaw;
} values;
} attitudeEulerAngles_t;
extern rollAndPitchInclination_t inclination; extern attitudeEulerAngles_t attitude;
typedef struct accDeadband_s { typedef struct accDeadband_s {
uint8_t xy; // set the acc deadband for xy-Axis uint8_t xy; // set the acc deadband for xy-Axis
@ -45,10 +46,10 @@ typedef struct accDeadband_s {
} accDeadband_t; } accDeadband_t;
typedef struct imuRuntimeConfig_s { typedef struct imuRuntimeConfig_s {
uint8_t acc_lpf_factor; uint8_t acc_cut_hz;
uint8_t acc_unarmedcal; uint8_t acc_unarmedcal;
float gyro_cmpf_factor; float dcm_ki;
float gyro_cmpfm_factor; float dcm_kp;
uint8_t small_angle; uint8_t small_angle;
} imuRuntimeConfig_t; } imuRuntimeConfig_t;

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

@ -684,8 +684,8 @@ STATIC_UNIT_TESTED void servoMixer(void)
} }
} }
input[INPUT_GIMBAL_PITCH] = scaleRange(inclination.values.pitchDeciDegrees, -1800, 1800, -500, +500); input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(inclination.values.rollDeciDegrees, -1800, 1800, -500, +500); input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
@ -841,8 +841,8 @@ void mixTable(void)
/* /*
case MIXER_GIMBAL: case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break; break;
*/ */
@ -858,11 +858,11 @@ void mixTable(void)
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50; servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else { } else {
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees / 50; servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} }
} }
} }

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

@ -33,8 +33,12 @@
#include "drivers/serial_uart.h" #include "drivers/serial_uart.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/serial.h" #include "io/serial.h"
@ -44,6 +48,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/gps_conversion.h" #include "flight/gps_conversion.h"
#include "flight/imu.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -359,7 +364,7 @@ void GPS_reset_home_position(void)
GPS_home[LAT] = GPS_coord[LAT]; GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON]; GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
nav_takeoff_bearing = heading; // save takeoff heading nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude // Set ground altitude
ENABLE_STATE(GPS_FIX_HOME); ENABLE_STATE(GPS_FIX_HOME);
} }
@ -644,8 +649,8 @@ static int32_t wrap_36000(int32_t angle)
void updateGpsStateForHomeAndHoldMode(void) void updateGpsStateForHomeAndHoldMode(void)
{ {
float sin_yaw_y = sin_approx(heading * 0.0174532925f); float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
float cos_yaw_x = cos_approx(heading * 0.0174532925f); float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
if (gpsProfile->nav_slew_rate) { if (gpsProfile->nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8 nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);

25
src/main/flight/pid.c
View file

@ -51,7 +51,6 @@ extern uint16_t cycleTime;
extern uint8_t motorCount; extern uint8_t motorCount;
extern float dT; extern float dT;
int16_t heading;
int16_t axisPID[3]; int16_t axisPID[3];
#ifdef BLACKBOX #ifdef BLACKBOX
@ -144,10 +143,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// calculate error and limit the angle to the max inclination // calculate error and limit the angle to the max inclination
#ifdef GPS #ifdef GPS
errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#else #else
errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination), errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
@ -238,10 +237,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// observe max inclination // observe max inclination
#ifdef GPS #ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
@ -341,10 +340,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// 50 degrees max inclination // 50 degrees max inclination
#ifdef GPS #ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here
@ -447,10 +446,10 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
// observe max inclination // observe max inclination
#ifdef GPS #ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
@ -578,9 +577,9 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS #ifdef GPS
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
@ -730,10 +729,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// calculate error and limit the angle to max configured inclination // calculate error and limit the angle to max configured inclination
#ifdef GPS #ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#else #else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#endif #endif
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID

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

@ -78,9 +78,6 @@ typedef struct pidProfile_s {
#endif #endif
} pidProfile_t; } pidProfile_t;
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

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

@ -481,11 +481,12 @@ void showSensorsPage(void)
} }
#endif #endif
tfp_sprintf(lineBuffer, format, "I&H", inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees, heading); tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
/*
uint8_t length; uint8_t length;
ftoa(EstG.A[X], lineBuffer); ftoa(EstG.A[X], lineBuffer);
@ -509,6 +510,7 @@ void showSensorsPage(void)
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
*/
} }

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

@ -216,9 +216,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif #endif
if (!sensors(SENSOR_MAG))
heading = 0; // reset heading to zero after gyro calibration
return; return;
} }

6
src/main/io/serial_cli.c
View file

@ -411,8 +411,8 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, 100, 900 }, { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, 100, 900 },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 },
{ "gyro_cmpf_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpf_factor, 100, 1000 }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, 0, 20000 },
{ "gyro_cmpfm_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpfm_factor, 100, 1000 }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, 0, 20000 },
{ "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, 1, 250 }, { "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, 0, 1 }, { "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, 0, 1 },
@ -459,7 +459,7 @@ const clivalue_t valueTable[] = {
#endif #endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX }, { "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX },
{ "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 }, { "acc_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_cut_hz, 0, 200 },
{ "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 }, { "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 },
{ "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 }, { "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 },
{ "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, 1, 20 }, { "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, 1, 20 },

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

@ -895,9 +895,9 @@ static bool processOutCommand(uint8_t cmdMSP)
break; break;
case MSP_ATTITUDE: case MSP_ATTITUDE:
headSerialReply(6); headSerialReply(6);
for (i = 0; i < 2; i++) serialize16(attitude.values.roll);
serialize16(inclination.raw[i]); serialize16(attitude.values.pitch);
serialize16(heading); serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
break; break;
case MSP_ALTITUDE: case MSP_ALTITUDE:
headSerialReply(6); headSerialReply(6);

5
src/main/main.c
View file

@ -116,13 +116,13 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig); void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
void loop(void); void loop(void);
void spektrumBind(rxConfig_t *rxConfig); void spektrumBind(rxConfig_t *rxConfig);
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
void sonarInit(const sonarHardware_t *sonarHardware); void sonarInit(const sonarHardware_t *sonarHardware);
void qimuInit(void);
#ifdef STM32F303xC #ifdef STM32F303xC
// from system_stm32f30x.c // from system_stm32f30x.c
@ -398,7 +398,8 @@ void init(void)
compassInit(); compassInit();
#endif #endif
qimuInit(); imuInit();
mspInit(&masterConfig.serialConfig); mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI #ifdef USE_CLI

12
src/main/mw.c
View file

@ -234,7 +234,7 @@ void annexCode(void)
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (FLIGHT_MODE(HEADFREE_MODE)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(heading - headFreeModeHold); float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff); float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff); float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
@ -338,7 +338,7 @@ void mwArm(void)
} }
if (!ARMING_FLAG(PREVENT_ARMING)) { if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading; headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX #ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) { if (feature(FEATURE_BLACKBOX)) {
@ -412,7 +412,7 @@ void updateInflightCalibrationState(void)
void updateMagHold(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; int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
if (dif <= -180) if (dif <= -180)
dif += 360; dif += 360;
if (dif >= +180) if (dif >= +180)
@ -421,7 +421,7 @@ void updateMagHold(void)
if (STATE(SMALL_ANGLE)) if (STATE(SMALL_ANGLE))
rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg
} else } else
magHold = heading; magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
} }
typedef enum { typedef enum {
@ -623,7 +623,7 @@ void processRx(void)
if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) { if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE); ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = heading; magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
} }
} else { } else {
DISABLE_FLIGHT_MODE(MAG_MODE); DISABLE_FLIGHT_MODE(MAG_MODE);
@ -636,7 +636,7 @@ void processRx(void)
DISABLE_FLIGHT_MODE(HEADFREE_MODE); DISABLE_FLIGHT_MODE(HEADFREE_MODE);
} }
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
headFreeModeHold = heading; // acquire new heading headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
} }
} }
#endif #endif

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

@ -70,5 +70,3 @@ typedef struct sensorAlignmentConfig_s {
sensor_align_e acc_align; // acc alignment sensor_align_e acc_align; // acc alignment
sensor_align_e mag_align; // mag alignment sensor_align_e mag_align; // mag alignment
} sensorAlignmentConfig_t; } sensorAlignmentConfig_t;
extern int16_t heading;

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

@ -432,7 +432,7 @@ static void sendFuelLevel(void)
static void sendHeading(void) static void sendHeading(void)
{ {
sendDataHead(ID_COURSE_BP); sendDataHead(ID_COURSE_BP);
serialize16(heading); serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
sendDataHead(ID_COURSE_AP); sendDataHead(ID_COURSE_AP);
serialize16(0); serialize16(0);
} }

2
src/main/telemetry/smartport.c
View file

@ -381,7 +381,7 @@ void handleSmartPortTelemetry(void)
} }
break; break;
case FSSP_DATAID_HEADING : case FSSP_DATAID_HEADING :
smartPortSendPackage(id, heading * 100); // given in deg, requested in 10000 = 100 deg smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
smartPortHasRequest = 0; smartPortHasRequest = 0;
break; break;
case FSSP_DATAID_ACCX : case FSSP_DATAID_ACCX :