1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00

Update gpio/ledring drivers so they do not include "board.h". It is now

clear what all gpio/ledring drivers need to compile and what was
unnecessarily included before.
 
In attempting this it was clear that ledring had a dependency on the
multiwii code, this was removed by passing the led status update method
the values it needs.

It also turned out that the ROLL/PITCH defines were coming from
rc_alias_e and much unrelated code is coupled to these defines.  This
commit also includes some cleanups relating to that problem.
This commit is contained in:
Dominic Clifton 2014-04-17 13:52:57 +01:00
parent b6232573c0
commit 38ec0b6080
19 changed files with 132 additions and 50 deletions

10
src/axis.h Normal file
View file

@ -0,0 +1,10 @@
#pragma once
typedef enum {
X = 0,
Y,
Z
} axis_e;
#define XYZ_AXIS_COUNT 3

View file

@ -21,6 +21,7 @@
#include "drivers/system_common.h" #include "drivers/system_common.h"
#include "drivers/altimeter_common.h" #include "drivers/altimeter_common.h"
#include "sensors_common.h" #include "sensors_common.h"
#include "axis.h"
typedef enum { typedef enum {
SENSOR_GYRO = 1 << 0, // always present SENSOR_GYRO = 1 << 0, // always present

View file

@ -4,7 +4,9 @@
#include <platform.h> #include <platform.h>
#include "accgyro_common.h" #include "accgyro_common.h"
#include <sensors_common.h> #include "axis.h"
#include "sensors_common.h"
#include "accgyro_l3g4200d.h" #include "accgyro_l3g4200d.h"

View file

@ -12,6 +12,7 @@
#include "light_led.h" #include "light_led.h"
#include "boardalignment.h" #include "boardalignment.h"
#include "axis.h"
#include "sensors_common.h" #include "sensors_common.h"
#include "maths.h" #include "maths.h"

View file

@ -1,4 +1,10 @@
#include "board.h"
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "gpio_common.h"
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config) void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
{ {

View file

@ -1,5 +1,13 @@
#include "board.h"
#include "mw.h" #include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "bus_i2c.h"
#include "sensors_common.h"
#include "axis.h"
#include "flight_common.h"
#include "maths.h" #include "maths.h"
@ -17,7 +25,8 @@ bool ledringDetect(void)
return true; 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]; uint8_t b[10];
static uint8_t state; static uint8_t state;
@ -29,14 +38,14 @@ void ledringState(void)
state = 1; state = 1;
} else if (state == 1) { } else if (state == 1) {
b[0] = 'y'; b[0] = 'y';
b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180); b[1] = constrain(roll / 10 + 90, 0, 180);
b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180); b[2] = constrain(pitch / 10 + 90, 0, 180);
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
state = 2; state = 2;
} else if (state == 2) { } else if (state == 2) {
b[0] = 'd'; // all unicolor GREEN b[0] = 'd'; // all unicolor GREEN
b[1] = 1; b[1] = 1;
if (f.ARMED) if (armed)
b[2] = 1; b[2] = 1;
else else
b[2] = 0; b[2] = 0;

View file

@ -1,5 +1,5 @@
#pragma once #pragma once
bool ledringDetect(void); bool ledringDetect(void);
void ledringState(void); void ledringState(bool armed, int16_t pitch, int16_t roll);
void ledringBlink(void); void ledringBlink(void);

24
src/flight_common.h Normal file
View file

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

View file

@ -3,8 +3,12 @@
#include "maths.h" #include "maths.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; #include "sensors_compass.h"
int32_t accSum[3]; #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 uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0; int accSumCount = 0;
int16_t accZ_25deg = 0; int16_t accZ_25deg = 0;
@ -26,8 +30,9 @@ float throttleAngleScale;
// ************** // **************
// gyro+acc IMU // gyro+acc IMU
// ************** // **************
int16_t gyroData[3] = { 0, 0, 0 }; int16_t gyroData[GYRO_INDEX_COUNT] = { 0, 0, 0 };
int16_t gyroZero[3] = { 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 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 float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
@ -62,10 +67,10 @@ void computeIMU(void)
} }
if (mcfg.mixerConfiguration == MULTITYPE_TRI) { if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3; gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3;
gyroYawSmooth = gyroData[YAW]; gyroYawSmooth = gyroData[GI_YAW];
gyroData[ROLL] = gyroADC[ROLL]; gyroData[GI_ROLL] = gyroADC[GI_ROLL];
gyroData[PITCH] = gyroADC[PITCH]; gyroData[GI_PITCH] = gyroADC[GI_PITCH];
} else { } else {
for (axis = 0; axis < 3; axis++) for (axis = 0; axis < 3; axis++)
gyroData[axis] = gyroADC[axis]; gyroData[axis] = gyroADC[axis];
@ -102,6 +107,12 @@ typedef union {
t_fp_vector EstG; t_fp_vector EstG;
typedef struct fp_angles {
float roll;
float pitch;
float yaw;
} fp_angles_t;
// Normalize a vector // Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest) 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 // 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; 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 cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx; float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta[ROLL]); cosx = cosf(delta->roll);
sinx = sinf(delta[ROLL]); sinx = sinf(delta->roll);
cosy = cosf(delta[PITCH]); cosy = cosf(delta->pitch);
siny = sinf(delta[PITCH]); siny = sinf(delta->pitch);
cosz = cosf(delta[YAW]); cosz = cosf(delta->yaw);
sinz = sinf(delta[YAW]); sinz = sinf(delta->yaw);
coszcosx = cosz * cosx; coszcosx = cosz * cosx;
coszcosy = cosz * cosy; 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]; 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) int32_t applyDeadband(int32_t value, int32_t deadband)
{ {
if (abs(value) < deadband) { if (abs(value) < deadband) {
@ -173,19 +194,19 @@ void acc_calc(uint32_t deltaT)
{ {
static int32_t accZoffset = 0; static int32_t accZoffset = 0;
static float accz_smooth; static float accz_smooth;
float rpy[3]; fp_angles_t rpy;
t_fp_vector accel_ned; t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame // the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL]; rpy.roll = -(float)anglerad[AI_ROLL];
rpy[1] = -(float)anglerad[PITCH]; rpy.pitch = -(float)anglerad[AI_PITCH];
rpy[2] = -(float)heading * RAD; rpy.yaw = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0]; accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1]; accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2]; accel_ned.V.Z = accSmooth[2];
rotateV(&accel_ned.V, rpy); rotateAnglesV(&accel_ned.V, &rpy);
if (cfg.acc_unarmedcal == 1) { if (cfg.acc_unarmedcal == 1) {
if (!f.ARMED) { if (!f.ARMED) {
@ -226,10 +247,10 @@ static int16_t calculateHeading(t_fp_vector *vec)
{ {
int16_t head; int16_t head;
float cosineRoll = cosf(anglerad[ROLL]); float cosineRoll = cosf(anglerad[AI_ROLL]);
float sineRoll = sinf(anglerad[ROLL]); float sineRoll = sinf(anglerad[AI_ROLL]);
float cosinePitch = cosf(anglerad[PITCH]); float cosinePitch = cosf(anglerad[AI_PITCH]);
float sinePitch = sinf(anglerad[PITCH]); float sinePitch = sinf(anglerad[AI_PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; 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 Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; 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; f.SMALL_ANGLES_25 = 0;
// Attitude of the estimated vector // Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[AI_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)); anglerad[AI_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[AI_ROLL] = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI)); angle[AI_PITCH] = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG)) if (sensors(SENSOR_MAG))
heading = calculateHeading(&EstM); heading = calculateHeading(&EstM);

View file

@ -1,6 +1,8 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
#include "flight_common.h"
#include "maths.h" #include "maths.h"
static uint8_t numberMotor = 0; static uint8_t numberMotor = 0;

View file

@ -3,6 +3,7 @@
#include "serial_cli.h" #include "serial_cli.h"
#include "telemetry_common.h" #include "telemetry_common.h"
#include "flight_common.h"
#include "typeconversion.h" #include "typeconversion.h"
#include "maths.h" #include "maths.h"
@ -185,7 +186,7 @@ void annexCode(void)
static uint32_t LEDTime; static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) { if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000; LEDTime = currentTime + 50000;
ledringState(); ledringState(f.ARMED, angle[AI_PITCH], angle[AI_ROLL]);
} }
} }
#endif #endif

View file

@ -57,7 +57,7 @@ typedef enum GimbalFlags {
} GimbalFlags; } GimbalFlags;
/*********** RC alias *****************/ /*********** RC alias *****************/
enum { typedef enum rc_alias {
ROLL = 0, ROLL = 0,
PITCH, PITCH,
YAW, YAW,
@ -66,7 +66,7 @@ enum {
AUX2, AUX2,
AUX3, AUX3,
AUX4 AUX4
}; } rc_alias_e;
enum { enum {
PIDROLL, 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 uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code
} flags_t; } 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 axisPID[3];
extern int16_t rcCommand[4]; extern int16_t rcCommand[4];
extern uint8_t rcOptions[CHECKBOXITEMS]; extern uint8_t rcOptions[CHECKBOXITEMS];
extern int16_t failsafeCnt; extern int16_t failsafeCnt;
extern int16_t debug[4]; 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 uint16_t acc_1G;
extern uint32_t accTimeSum; extern uint32_t accTimeSum;
extern int accSumCount; extern int accSumCount;

View file

@ -1,6 +1,8 @@
#include "board.h" #include "board.h"
#include "mw.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. 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; extern uint16_t InflightcalibratingA;

View file

@ -23,8 +23,4 @@ typedef struct sensor_t
float scale; // scalefactor (currently used for gyro only, todo for accel) float scale; // scalefactor (currently used for gyro only, todo for accel)
} sensor_t; } sensor_t;
typedef enum { extern int16_t heading;
X = 0,
Y,
Z
} sensor_axis_e;

View file

@ -1,6 +1,10 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
#include "axis.h"
int16_t magADC[XYZ_AXIS_COUNT];
#ifdef MAG #ifdef MAG
static uint8_t magInit = 0; static uint8_t magInit = 0;

View file

@ -4,3 +4,5 @@
void Mag_init(void); void Mag_init(void);
int Mag_getADC(void); int Mag_getADC(void);
#endif #endif
extern int16_t magADC[XYZ_AXIS_COUNT];

View file

@ -1,6 +1,8 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
#include "flight_common.h"
#include "maths.h" #include "maths.h"
void GYRO_Common(void) void GYRO_Common(void)

View file

@ -3,6 +3,8 @@
#include "serial_cli.h" #include "serial_cli.h"
#include "telemetry_common.h" #include "telemetry_common.h"
#include "flight_common.h"
#include "sensors_compass.h"
// Multiwii Serial Protocol 0 // Multiwii Serial Protocol 0
#define MSP_VERSION 0 #define MSP_VERSION 0

View file

@ -4,6 +4,8 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
#include "flight_common.h"
#include "telemetry_common.h" #include "telemetry_common.h"
#include "telemetry_frsky.h" #include "telemetry_frsky.h"