More relocation of variables.

This commit is contained in:
Dominic Clifton 2015-02-01 00:03:46 +01:00
parent 53406a7ac7
commit 8cc9e8ca37
13 changed files with 65 additions and 52 deletions

View File

@ -25,3 +25,18 @@ typedef enum {
#define XYZ_AXIS_COUNT 3 #define XYZ_AXIS_COUNT 3
// See http://en.wikipedia.org/wiki/Flight_dynamics
typedef enum {
FD_ROLL = 0,
FD_PITCH,
FD_YAW
} flight_dynamics_index_t;
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
typedef enum {
AI_ROLL = 0,
AI_PITCH,
} angle_index_t;
#define ANGLE_INDEX_COUNT 2

View File

@ -22,6 +22,9 @@
#include "sensors/barometer.h" #include "sensors/barometer.h"
extern int32_t AltHold;
extern int32_t vario;
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
void applyAltHold(airplaneConfig_t *airplaneConfig); void applyAltHold(airplaneConfig_t *airplaneConfig);
void updateAltHoldState(void); void updateAltHoldState(void);

View File

@ -48,7 +48,7 @@
extern uint16_t cycleTime; extern uint16_t cycleTime;
extern uint8_t motorCount; extern uint8_t motorCount;
int16_t heading, magHold; int16_t heading;
int16_t axisPID[3]; int16_t axisPID[3];
#ifdef BLACKBOX #ifdef BLACKBOX

View File

@ -49,39 +49,12 @@ typedef struct pidProfile_s {
uint8_t H_sensitivity; uint8_t H_sensitivity;
} pidProfile_t; } pidProfile_t;
typedef enum {
AI_ROLL = 0,
AI_PITCH,
} angle_index_t;
#define ANGLE_INDEX_COUNT 2
// See http://en.wikipedia.org/wiki/Flight_dynamics
typedef enum {
FD_ROLL = 0,
FD_PITCH,
FD_YAW
} flight_dynamics_index_t;
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f) #define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
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];
extern int16_t heading, magHold;
extern int32_t AltHold;
extern int32_t vario;
void setPIDController(int type); void setPIDController(int type);
void resetErrorAngle(void); void resetErrorAngle(void);
void resetErrorGyro(void); void resetErrorGyro(void);

View File

@ -48,8 +48,9 @@
extern int16_t debug[4]; extern int16_t debug[4];
int16_t gyroADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT];
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
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;
@ -63,11 +64,6 @@ float fc_acc;
float magneticDeclination = 0.0f; // calculated at startup from config float magneticDeclination = 0.0f; // calculated at startup from config
float gyroScaleRad; float gyroScaleRad;
// **************
// gyro+acc IMU
// **************
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
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 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 float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians

View File

@ -22,6 +22,11 @@ extern uint32_t accTimeSum;
extern int accSumCount; extern int accSumCount;
extern float accVelScale; extern float accVelScale;
extern int16_t accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
typedef struct rollAndPitchInclination_s { typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t rollDeciDegrees; int16_t rollDeciDegrees;

View File

@ -57,6 +57,7 @@ extern int16_t debug[4];
bool areSticksInApModePosition(uint16_t ap_mode); bool areSticksInApModePosition(uint16_t ap_mode);
int16_t magHold;
// ********************** // **********************
// GPS // GPS

View File

@ -17,6 +17,8 @@
#pragma once #pragma once
extern int16_t magHold;
// navigation mode // navigation mode
typedef enum { typedef enum {
NAV_MODE_NONE = 0, NAV_MODE_NONE = 0,

View File

@ -26,32 +26,36 @@
#include "build_config.h" #include "build_config.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/system.h"
#include "drivers/display_ug2864hsweg01.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "common/printf.h" #include "common/printf.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h"
#ifdef DISPLAY #ifdef DISPLAY
#include "drivers/system.h"
#include "drivers/display_ug2864hsweg01.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/compass.h" #include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/flight.h"
#include "flight/imu.h"
#ifdef GPS #ifdef GPS
#include "io/gps.h" #include "io/gps.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#endif #endif
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
#include "config/config.h" #include "config/config.h"

View File

@ -33,6 +33,8 @@
#include "sensors/gyro.h" #include "sensors/gyro.h"
uint16_t calibratingG = 0; uint16_t calibratingG = 0;
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig; static gyroConfig_t *gyroConfig;

View File

@ -20,6 +20,9 @@
extern gyro_t gyro; extern gyro_t gyro;
extern sensor_align_e gyroAlign; extern sensor_align_e gyroAlign;
extern int16_t gyroADC[XYZ_AXIS_COUNT];
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
typedef struct gyroConfig_s { typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t; } gyroConfig_t;

View File

@ -36,20 +36,28 @@
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "io/serial.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/barometer.h" #include "sensors/barometer.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "flight/flight.h"
#include "io/serial.h"
#include "io/rc_controls.h"
#include "io/gps.h" #include "io/gps.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/flight.h"
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "telemetry/frsky.h" #include "telemetry/frsky.h"

View File

@ -50,6 +50,7 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "telemetry/smartport.h" #include "telemetry/smartport.h"