diff --git a/src/axis.h b/src/axis.h new file mode 100644 index 0000000000..d70268a467 --- /dev/null +++ b/src/axis.h @@ -0,0 +1,10 @@ +#pragma once + +typedef enum { + X = 0, + Y, + Z +} axis_e; + +#define XYZ_AXIS_COUNT 3 + diff --git a/src/board.h b/src/board.h index f898e22785..d501d4320a 100755 --- a/src/board.h +++ b/src/board.h @@ -21,6 +21,7 @@ #include "drivers/system_common.h" #include "drivers/altimeter_common.h" #include "sensors_common.h" +#include "axis.h" typedef enum { SENSOR_GYRO = 1 << 0, // always present diff --git a/src/drivers/accgyro_l3g4200d.c b/src/drivers/accgyro_l3g4200d.c index da3fe2a75f..dec54197a9 100644 --- a/src/drivers/accgyro_l3g4200d.c +++ b/src/drivers/accgyro_l3g4200d.c @@ -4,7 +4,9 @@ #include #include "accgyro_common.h" -#include +#include "axis.h" +#include "sensors_common.h" + #include "accgyro_l3g4200d.h" diff --git a/src/drivers/compass_hmc5883l.c b/src/drivers/compass_hmc5883l.c index 1c302e775f..9866a971b6 100755 --- a/src/drivers/compass_hmc5883l.c +++ b/src/drivers/compass_hmc5883l.c @@ -12,6 +12,7 @@ #include "light_led.h" #include "boardalignment.h" +#include "axis.h" #include "sensors_common.h" #include "maths.h" diff --git a/src/drivers/gpio_common.c b/src/drivers/gpio_common.c index 91021f2d72..01e8890449 100644 --- a/src/drivers/gpio_common.c +++ b/src/drivers/gpio_common.c @@ -1,4 +1,10 @@ -#include "board.h" + +#include +#include + +#include "platform.h" + +#include "gpio_common.h" void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config) { diff --git a/src/drivers/light_ledring.c b/src/drivers/light_ledring.c index dd44d7d8b1..d8388af2af 100644 --- a/src/drivers/light_ledring.c +++ b/src/drivers/light_ledring.c @@ -1,5 +1,13 @@ -#include "board.h" -#include "mw.h" + +#include +#include + +#include "platform.h" + +#include "bus_i2c.h" +#include "sensors_common.h" +#include "axis.h" +#include "flight_common.h" #include "maths.h" @@ -17,7 +25,8 @@ bool ledringDetect(void) return true; } -void ledringState(void) +// pitch/roll are absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 +void ledringState(bool armed, int16_t pitch, int16_t roll) { uint8_t b[10]; static uint8_t state; @@ -29,14 +38,14 @@ void ledringState(void) state = 1; } else if (state == 1) { b[0] = 'y'; - b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180); - b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180); + b[1] = constrain(roll / 10 + 90, 0, 180); + b[2] = constrain(pitch / 10 + 90, 0, 180); i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); state = 2; } else if (state == 2) { b[0] = 'd'; // all unicolor GREEN b[1] = 1; - if (f.ARMED) + if (armed) b[2] = 1; else b[2] = 0; diff --git a/src/drivers/light_ledring.h b/src/drivers/light_ledring.h index 579f0ba250..bf247fe8b4 100644 --- a/src/drivers/light_ledring.h +++ b/src/drivers/light_ledring.h @@ -1,5 +1,5 @@ #pragma once bool ledringDetect(void); -void ledringState(void); +void ledringState(bool armed, int16_t pitch, int16_t roll); void ledringBlink(void); diff --git a/src/flight_common.h b/src/flight_common.h new file mode 100644 index 0000000000..09ea0c9c9f --- /dev/null +++ b/src/flight_common.h @@ -0,0 +1,24 @@ +#pragma once + +enum { + AI_ROLL = 0, + AI_PITCH, +} angle_index_t; + +#define ANGLE_INDEX_COUNT 2 + +extern int16_t angle[ANGLE_INDEX_COUNT]; // see angle_index_t + +enum { + GI_ROLL = 0, + GI_PITCH, + GI_YAW +} gyro_index_t; + +#define GYRO_INDEX_COUNT 3 + +extern int16_t gyroData[GYRO_INDEX_COUNT]; // see gyro_index_t +extern int16_t gyroZero[GYRO_INDEX_COUNT]; // see gyro_index_t + +extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; +extern int32_t accSum[XYZ_AXIS_COUNT]; diff --git a/src/flight_imu.c b/src/flight_imu.c index 6d03f9bb2e..f41d280bbf 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -3,8 +3,12 @@ #include "maths.h" -int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; -int32_t accSum[3]; +#include "sensors_compass.h" +#include "flight_common.h" + +int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; +int32_t accSum[XYZ_AXIS_COUNT]; + uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; int16_t accZ_25deg = 0; @@ -26,8 +30,9 @@ float throttleAngleScale; // ************** // gyro+acc IMU // ************** -int16_t gyroData[3] = { 0, 0, 0 }; -int16_t gyroZero[3] = { 0, 0, 0 }; +int16_t gyroData[GYRO_INDEX_COUNT] = { 0, 0, 0 }; +int16_t gyroZero[GYRO_INDEX_COUNT] = { 0, 0, 0 }; + int16_t angle[2] = { 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 @@ -62,10 +67,10 @@ void computeIMU(void) } if (mcfg.mixerConfiguration == MULTITYPE_TRI) { - gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3; - gyroYawSmooth = gyroData[YAW]; - gyroData[ROLL] = gyroADC[ROLL]; - gyroData[PITCH] = gyroADC[PITCH]; + gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3; + gyroYawSmooth = gyroData[GI_YAW]; + gyroData[GI_ROLL] = gyroADC[GI_ROLL]; + gyroData[GI_PITCH] = gyroADC[GI_PITCH]; } else { for (axis = 0; axis < 3; axis++) gyroData[axis] = gyroADC[axis]; @@ -102,6 +107,12 @@ typedef union { t_fp_vector EstG; +typedef struct fp_angles { + float roll; + float pitch; + float yaw; +} fp_angles_t; + // Normalize a vector void normalizeV(struct fp_vector *src, struct fp_vector *dest) { @@ -116,7 +127,7 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest) } // Rotate Estimated vector(s) with small angle approximation, according to the gyro data -void rotateV(struct fp_vector *v, float *delta) +void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta) { struct fp_vector v_tmp = *v; @@ -125,12 +136,12 @@ void rotateV(struct fp_vector *v, float *delta) float cosx, sinx, cosy, siny, cosz, sinz; float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx; - cosx = cosf(delta[ROLL]); - sinx = sinf(delta[ROLL]); - cosy = cosf(delta[PITCH]); - siny = sinf(delta[PITCH]); - cosz = cosf(delta[YAW]); - sinz = sinf(delta[YAW]); + cosx = cosf(delta->roll); + sinx = sinf(delta->roll); + cosy = cosf(delta->pitch); + siny = sinf(delta->pitch); + cosz = cosf(delta->yaw); + sinz = sinf(delta->yaw); coszcosx = cosz * cosx; coszcosy = cosz * cosy; @@ -153,6 +164,16 @@ void rotateV(struct fp_vector *v, float *delta) v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; } +// deprecated - it uses legacy indices for ROLL/PITCH/YAW, see rc_alias_e - use rotateAnglesV instead +void rotateV(struct fp_vector *v, float *delta) { + fp_angles_t temp; + temp.roll = delta[ROLL]; + temp.pitch = delta[PITCH]; + temp.yaw = delta[YAW]; + + rotateAnglesV(v, &temp); +} + int32_t applyDeadband(int32_t value, int32_t deadband) { if (abs(value) < deadband) { @@ -173,19 +194,19 @@ void acc_calc(uint32_t deltaT) { static int32_t accZoffset = 0; static float accz_smooth; - float rpy[3]; + fp_angles_t rpy; t_fp_vector accel_ned; // the accel values have to be rotated into the earth frame - rpy[0] = -(float)anglerad[ROLL]; - rpy[1] = -(float)anglerad[PITCH]; - rpy[2] = -(float)heading * RAD; + rpy.roll = -(float)anglerad[AI_ROLL]; + rpy.pitch = -(float)anglerad[AI_PITCH]; + rpy.yaw = -(float)heading * RAD; accel_ned.V.X = accSmooth[0]; accel_ned.V.Y = accSmooth[1]; accel_ned.V.Z = accSmooth[2]; - rotateV(&accel_ned.V, rpy); + rotateAnglesV(&accel_ned.V, &rpy); if (cfg.acc_unarmedcal == 1) { if (!f.ARMED) { @@ -226,10 +247,10 @@ static int16_t calculateHeading(t_fp_vector *vec) { int16_t head; - float cosineRoll = cosf(anglerad[ROLL]); - float sineRoll = sinf(anglerad[ROLL]); - float cosinePitch = cosf(anglerad[PITCH]); - float sinePitch = sinf(anglerad[PITCH]); + float cosineRoll = cosf(anglerad[AI_ROLL]); + float sineRoll = sinf(anglerad[AI_ROLL]); + float cosinePitch = cosf(anglerad[AI_PITCH]); + float sinePitch = sinf(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; @@ -295,10 +316,10 @@ static void getEstimatedAttitude(void) f.SMALL_ANGLES_25 = 0; // Attitude of the estimated vector - anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z); - anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); - angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI)); - angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI)); + anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); + anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); + angle[AI_ROLL] = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); + angle[AI_PITCH] = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); if (sensors(SENSOR_MAG)) heading = calculateHeading(&EstM); diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 2464c82a0e..ac95129b3f 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +#include "flight_common.h" + #include "maths.h" static uint8_t numberMotor = 0; diff --git a/src/mw.c b/src/mw.c index 0db01d7779..821daa54f3 100755 --- a/src/mw.c +++ b/src/mw.c @@ -3,6 +3,7 @@ #include "serial_cli.h" #include "telemetry_common.h" +#include "flight_common.h" #include "typeconversion.h" #include "maths.h" @@ -185,7 +186,7 @@ void annexCode(void) static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; - ledringState(); + ledringState(f.ARMED, angle[AI_PITCH], angle[AI_ROLL]); } } #endif diff --git a/src/mw.h b/src/mw.h index 6344b1d496..1c04b8def4 100755 --- a/src/mw.h +++ b/src/mw.h @@ -57,7 +57,7 @@ typedef enum GimbalFlags { } GimbalFlags; /*********** RC alias *****************/ -enum { +typedef enum rc_alias { ROLL = 0, PITCH, YAW, @@ -66,7 +66,7 @@ enum { AUX2, AUX3, AUX4 -}; +} rc_alias_e; enum { PIDROLL, @@ -315,17 +315,12 @@ typedef struct flags_t { uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code } flags_t; -extern int16_t gyroZero[3]; -extern int16_t gyroData[3]; -extern int16_t angle[2]; extern int16_t axisPID[3]; extern int16_t rcCommand[4]; extern uint8_t rcOptions[CHECKBOXITEMS]; extern int16_t failsafeCnt; extern int16_t debug[4]; -extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; -extern int32_t accSum[3]; extern uint16_t acc_1G; extern uint32_t accTimeSum; extern int accSumCount; diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index 27c71e170e..b5e006770e 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +#include "flight_common.h" + uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; diff --git a/src/sensors_common.h b/src/sensors_common.h index 607ada19da..8ada5066e3 100644 --- a/src/sensors_common.h +++ b/src/sensors_common.h @@ -23,8 +23,4 @@ typedef struct sensor_t float scale; // scalefactor (currently used for gyro only, todo for accel) } sensor_t; -typedef enum { - X = 0, - Y, - Z -} sensor_axis_e; +extern int16_t heading; diff --git a/src/sensors_compass.c b/src/sensors_compass.c index 9c39ec81bb..4790f91719 100644 --- a/src/sensors_compass.c +++ b/src/sensors_compass.c @@ -1,6 +1,10 @@ #include "board.h" #include "mw.h" +#include "axis.h" + +int16_t magADC[XYZ_AXIS_COUNT]; + #ifdef MAG static uint8_t magInit = 0; diff --git a/src/sensors_compass.h b/src/sensors_compass.h index a673bc7798..49fa047569 100644 --- a/src/sensors_compass.h +++ b/src/sensors_compass.h @@ -4,3 +4,5 @@ void Mag_init(void); int Mag_getADC(void); #endif + +extern int16_t magADC[XYZ_AXIS_COUNT]; diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index fecc24b80c..55fb840011 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +#include "flight_common.h" + #include "maths.h" void GYRO_Common(void) diff --git a/src/serial_msp.c b/src/serial_msp.c index d9b090f0ce..ea18b7b7f3 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -3,6 +3,8 @@ #include "serial_cli.h" #include "telemetry_common.h" +#include "flight_common.h" +#include "sensors_compass.h" // Multiwii Serial Protocol 0 #define MSP_VERSION 0 diff --git a/src/telemetry_frsky.c b/src/telemetry_frsky.c index 8d737d9f42..8ff0ea390a 100644 --- a/src/telemetry_frsky.c +++ b/src/telemetry_frsky.c @@ -4,6 +4,8 @@ #include "board.h" #include "mw.h" +#include "flight_common.h" + #include "telemetry_common.h" #include "telemetry_frsky.h"