More relocation of variables.
This commit is contained in:
parent
53406a7ac7
commit
8cc9e8ca37
|
@ -25,3 +25,18 @@ typedef enum {
|
|||
|
||||
#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
|
||||
|
|
|
@ -22,6 +22,9 @@
|
|||
|
||||
#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 applyAltHold(airplaneConfig_t *airplaneConfig);
|
||||
void updateAltHoldState(void);
|
||||
|
|
|
@ -48,7 +48,7 @@
|
|||
extern uint16_t cycleTime;
|
||||
extern uint8_t motorCount;
|
||||
|
||||
int16_t heading, magHold;
|
||||
int16_t heading;
|
||||
int16_t axisPID[3];
|
||||
|
||||
#ifdef BLACKBOX
|
||||
|
|
|
@ -49,39 +49,12 @@ typedef struct pidProfile_s {
|
|||
uint8_t H_sensitivity;
|
||||
} 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 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 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 resetErrorAngle(void);
|
||||
void resetErrorGyro(void);
|
||||
|
|
|
@ -48,8 +48,9 @@
|
|||
|
||||
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];
|
||||
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
uint32_t accTimeSum = 0; // keep track for integration of acc
|
||||
int accSumCount = 0;
|
||||
|
@ -63,11 +64,6 @@ float fc_acc;
|
|||
float magneticDeclination = 0.0f; // calculated at startup from config
|
||||
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
|
||||
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
|
||||
|
|
|
@ -22,6 +22,11 @@ extern uint32_t accTimeSum;
|
|||
extern int accSumCount;
|
||||
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 {
|
||||
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
int16_t rollDeciDegrees;
|
||||
|
|
|
@ -57,6 +57,7 @@ extern int16_t debug[4];
|
|||
|
||||
bool areSticksInApModePosition(uint16_t ap_mode);
|
||||
|
||||
int16_t magHold;
|
||||
|
||||
// **********************
|
||||
// GPS
|
||||
|
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
extern int16_t magHold;
|
||||
|
||||
// navigation mode
|
||||
typedef enum {
|
||||
NAV_MODE_NONE = 0,
|
||||
|
|
|
@ -26,32 +26,36 @@
|
|||
#include "build_config.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/maths.h"
|
||||
#include "common/axis.h"
|
||||
|
||||
#ifdef DISPLAY
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/display_ug2864hsweg01.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/compass.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "flight/flight.h"
|
||||
#include "sensors/sensors.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
|
||||
#include "io/gps.h"
|
||||
#include "flight/navigation.h"
|
||||
#endif
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
#include "config/config.h"
|
||||
|
|
|
@ -33,6 +33,8 @@
|
|||
#include "sensors/gyro.h"
|
||||
|
||||
uint16_t calibratingG = 0;
|
||||
int16_t gyroADC[XYZ_AXIS_COUNT];
|
||||
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
static gyroConfig_t *gyroConfig;
|
||||
|
||||
|
|
|
@ -20,6 +20,9 @@
|
|||
extern gyro_t gyro;
|
||||
extern sensor_align_e gyroAlign;
|
||||
|
||||
extern int16_t gyroADC[XYZ_AXIS_COUNT];
|
||||
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
||||
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.
|
||||
} gyroConfig_t;
|
||||
|
|
|
@ -36,20 +36,28 @@
|
|||
#include "drivers/gpio.h"
|
||||
#include "drivers/timer.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/acceleration.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "flight/flight.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/rc_controls.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/frsky.h"
|
||||
|
||||
|
|
|
@ -50,6 +50,7 @@
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "flight/altitudehold.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/smartport.h"
|
||||
|
|
Loading…
Reference in New Issue