rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Remove main.c's dependency on mw.h/board.h. 

Moved pidControllers out of mw.c into flight_common.c/h.
Moved appropriate code into rc_controls.c/h.
This commit is contained in:
Dominic Clifton 2014-04-22 01:58:23 +01:00
parent 2c80094b0e
commit fe89d40fa0
28 changed files with 333 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -73,6 +73,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
flight_mixer.c \
gps_common.c \
runtime_config.c \
rc_controls.c \
rx_common.c \
rx_pwm.c \
rx_sbus.c \

1
src/config.c
View File

@ -23,6 +23,7 @@
#include "boardalignment.h"
#include "battery.h"
#include "gimbal.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "gps_common.h"
#include "serial_common.h"

4
src/drivers/light_ledring.c
View File

@ -8,9 +8,13 @@
#include "common/maths.h"
#include "bus_i2c.h"
// FIXME there should be no dependencies on the main source code
#include "rc_controls.h"
#include "sensors_common.h"
#include "flight_common.h"
#include "light_ledring.h"
// Driver for DFRobot I2C Led Ring
#define LED_RING_ADDRESS 0x6D

6
src/failsafe.c
View File

@ -1,10 +1,10 @@
#include <stdbool.h>
#include <stdint.h>
#include "rx_common.h"
#include "common/axis.h"
#include "flight_common.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "runtime_config.h"
#include "failsafe.h"

173
src/flight_common.c
View File

@ -1,13 +1,31 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "common/axis.h"
#include "common/maths.h"
#include "runtime_config.h"
#include "rc_controls.h"
#include "flight_common.h"
#include "gps_common.h"
extern uint16_t cycleTime;
int16_t heading, magHold;
int16_t axisPID[3];
uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim); // pid controller function prototype
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void mwDisarm(void)
@ -15,3 +33,158 @@ void mwDisarm(void)
if (f.ARMED)
f.ARMED = 0;
}
void resetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
}
void resetErrorGyro(void)
{
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
{
int axis, prop;
int32_t error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int32_t delta1[3], delta2[3];
int32_t deltaSum;
int32_t delta;
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim[axis];
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 = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
}
if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (f.ANGLE_MODE && axis < 2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
}
}
#define GYRO_I_MAX 256
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
{
int32_t errorAngle = 0;
int axis;
int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// calculate error and limit the angle to max configured inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim[axis]; // 16 bits is ok here
}
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
}
}
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis]) >> 7;
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * pidProfile->D8[axis]) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
}
void setPIDController(int type)
{
switch (type) {
case 0:
default:
pid_controller = pidMultiWii;
break;
case 1:
pid_controller = pidRewrite;
break;
}
}

5
src/flight_common.h
View File

@ -43,8 +43,13 @@ 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];
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int16_t heading, magHold;
void mwDisarm(void);
void setPIDController(int type);
void resetErrorAngle(void);
void resetErrorGyro(void);

2
src/flight_imu.c
View File

@ -24,6 +24,7 @@
#include "boardalignment.h"
#include "battery.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "drivers/serial_common.h"
#include "serial_common.h"
@ -53,7 +54,6 @@ int32_t vario = 0; // variometer in cm/s
int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind,
float throttleAngleScale;
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
uint32_t baroPressureSum = 0;

2
src/flight_mixer.h
View File

@ -57,3 +57,5 @@ typedef struct servoParam_t {
} servoParam_t;
bool isMixerUsingServos(void);
void mixerInit(void);
void writeAllMotors(int16_t mc);

51
src/main.c
View File

@ -1,23 +1,51 @@
#include "board.h"
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/axis.h"
#include "drivers/system_common.h"
#include "drivers/gpio_common.h"
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "drivers/timer_common.h"
#include "drivers/serial_common.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart.h"
#include "drivers/accgyro_common.h"
#include "drivers/pwm_common.h"
#include "drivers/adc_common.h"
#include "flight_common.h"
#include "flight_mixer.h"
#include "serial_common.h"
#include "failsafe.h"
#include "mw.h"
#include "gps_common.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "gimbal.h"
#include "sensors_common.h"
#include "drivers/accgyro_common.h"
#include "drivers/serial_common.h"
#include "sensors_sonar.h"
#include "sensors_barometer.h"
#include "sensors_acceleration.h"
#include "sensors_gyro.h"
#include "telemetry_common.h"
#include "battery.h"
#include "boardalignment.h"
#include "runtime_config.h"
#include "config.h"
#include "config_profile.h"
#include "config_master.h"
#include "build_config.h"
extern rcReadRawDataPtr rcReadRawFunc;
extern uint32_t previousTime;
failsafe_t *failsafe;
void initTelemetry(serialPorts_t *serialPorts);
@ -27,6 +55,10 @@ void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void buzzerInit(failsafe_t *initialFailsafe);
void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void sensorsAutodetect(void);
void imuInit(void);
void loop(void);
int main(void)
{
@ -159,10 +191,13 @@ int main(void)
initTelemetry(&serialPorts);
previousTime = micros();
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {
ACC_SetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
GYRO_SetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
Baro_SetCalibrationCycles(CALIBRATING_BARO_CYCLES);
f.SMALL_ANGLE = 1;
// loopy

179
src/mw.c
View File

@ -4,6 +4,7 @@
#include "common/maths.h"
#include "common/typeconversion.h"
#include "sensors_sonar.h"
#include "sensors_gyro.h"
#include "flight_common.h"
#include "serial_cli.h"
@ -26,19 +27,15 @@ int16_t headFreeModeHold;
int16_t telemTemperature1; // gyro sensor temperature
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
uint16_t rssi; // range: [0;1023]
static void pidMultiWii(void);
static void pidRewrite(void);
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
uint8_t dynP8[3], dynI8[3], dynD8[3];
int16_t axisPID[3];
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
extern failsafe_t *failsafe;
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);
extern pidControllerFuncPtr pid_controller;
// Automatic ACC Offset Calibration
bool AccInflightCalibrationArmed = false;
bool AccInflightCalibrationMeasurementDone = false;
@ -46,11 +43,6 @@ bool AccInflightCalibrationSavetoEEProm = false;
bool AccInflightCalibrationActive = false;
uint16_t InflightcalibratingA = 0;
bool areSticksInApModePosition(uint16_t ap_mode) // FIXME should probably live in rc_sticks.h
{
return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
}
void annexCode(void)
{
static uint32_t calibratedAccTime;
@ -198,150 +190,6 @@ static void mwVario(void)
}
static int32_t errorGyroI[3] = { 0, 0, 0 };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(void)
{
int axis, prop;
int32_t error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int32_t delta1[3], delta2[3];
int32_t deltaSum;
int32_t delta;
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis];
PTermACC = errorAngle * currentProfile.pidProfile.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -currentProfile.pidProfile.D8[PIDLEVEL] * 5, +currentProfile.pidProfile.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * currentProfile.pidProfile.I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / currentProfile.pidProfile.P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * currentProfile.pidProfile.I8[axis]) >> 6;
}
if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (f.ANGLE_MODE && axis < 2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
}
}
#define GYRO_I_MAX 256
static void pidRewrite(void)
{
int32_t errorAngle = 0;
int axis;
int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// calculate error and limit the angle to max configured inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis]; // 16 bits is ok here
}
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(currentProfile.controlRateConfig.yawRate + 27) * rcCommand[2]) >> 5);
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (currentProfile.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * currentProfile.pidProfile.I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * currentProfile.pidProfile.P8[PIDLEVEL]) >> 4;
}
}
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component
PTerm = (RateError * currentProfile.pidProfile.P8[axis]) >> 7;
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * currentProfile.pidProfile.I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * currentProfile.pidProfile.D8[axis]) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
}
void setPIDController(int type)
{
switch (type) {
case 0:
default:
pid_controller = pidMultiWii;
break;
case 1:
pid_controller = pidRewrite;
break;
}
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@ -418,11 +266,8 @@ void loop(void)
else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck))
isThrottleLow = true;
if (isThrottleLow) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
resetErrorAngle();
resetErrorGyro();
if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
if (rcOptions[BOXARM] && f.OK_TO_ARM)
mwArm();
@ -540,8 +385,7 @@ void loop(void)
if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
resetErrorAngle();
f.ANGLE_MODE = 1;
}
} else {
@ -551,8 +395,7 @@ void loop(void)
if (rcOptions[BOXHORIZON]) {
f.ANGLE_MODE = 0;
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
resetErrorAngle();
f.HORIZON_MODE = 1;
}
} else {
@ -757,7 +600,7 @@ void loop(void)
}
// PID - note this is function pointer set by setPIDController()
pid_controller();
pid_controller(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, currentProfile.angleTrim);
mixTable();
writeServos();

19
src/mw.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "rc_controls.h"
#include "runtime_config.h"
#include "flight_common.h"
#include "failsafe.h"
@ -19,26 +20,19 @@ enum {
ALIGN_MAG = 2
};
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_CYCLES 200
#include "serial_common.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "gps_common.h"
#include "config.h"
#include "config_profile.h"
#include "config_master.h"
extern int16_t axisPID[3];
extern int16_t rcCommand[4];
extern int16_t debug[4];
extern uint16_t acc_1G;
extern uint32_t accTimeSum;
extern int accSumCount;
extern uint32_t currentTime;
extern uint32_t previousTime;
extern uint16_t cycleTime;
extern uint16_t calibratingA;
extern uint16_t calibratingB;
@ -65,28 +59,19 @@ extern uint8_t toggleBeep;
extern flags_t f;
// main
void setPIDController(int type);
void loop(void);
// IMU
void imuInit(void);
void annexCode(void);
void computeIMU(void);
int getEstimatedAltitude(void);
// Sensors
void sensorsAutodetect(void);
void ACC_getADC(void);
int Baro_update(void);
void Gyro_getADC(void);
void Mag_init(void);
int Mag_getADC(void);
void Sonar_init(void);
void Sonar_update(void);
// Output
void mixerInit(void);
void mixerResetMotors(void);
void mixerLoadMix(int index);
void writeServos(void);

15
src/rc_controls.c Normal file
View File

@ -0,0 +1,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "common/maths.h"
#include "rc_controls.h"
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
bool areSticksInApModePosition(uint16_t ap_mode)
{
return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
}

38
src/rc_controls.h Normal file
View File

@ -0,0 +1,38 @@
#pragma once
typedef enum rc_alias {
ROLL = 0,
PITCH,
YAW,
THROTTLE,
AUX1,
AUX2,
AUX3,
AUX4
} rc_alias_e;
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
#define PIT_LO (1 << (2 * PITCH))
#define PIT_CE (3 << (2 * PITCH))
#define PIT_HI (2 << (2 * PITCH))
#define YAW_LO (1 << (2 * YAW))
#define YAW_CE (3 << (2 * YAW))
#define YAW_HI (2 << (2 * YAW))
#define THR_LO (1 << (2 * THROTTLE))
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))
typedef struct controlRateConfig_s {
uint8_t rcRate8;
uint8_t rcExpo8;
uint8_t thrMid8;
uint8_t thrExpo8;
uint8_t rollPitchRate;
uint8_t yawRate;
} controlRateConfig_t;
extern int16_t rcCommand[4];
bool areSticksInApModePosition(uint16_t ap_mode);

2
src/runtime_config.h
View File

@ -47,8 +47,6 @@ typedef struct flags_t {
uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code
} flags_t;
typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype
extern flags_t f;
bool sensors(uint32_t mask);

1
src/rx_common.c
View File

@ -9,6 +9,7 @@
#include "config.h"
#include "failsafe.h"
#include "rc_controls.h"
#include "rx_pwm.h"
#include "rx_sbus.h"

33
src/rx_common.h
View File

@ -1,29 +1,5 @@
#pragma once
typedef enum rc_alias {
ROLL = 0,
PITCH,
YAW,
THROTTLE,
AUX1,
AUX2,
AUX3,
AUX4
} rc_alias_e;
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
#define PIT_LO (1 << (2 * PITCH))
#define PIT_CE (3 << (2 * PITCH))
#define PIT_HI (2 << (2 * PITCH))
#define YAW_LO (1 << (2 * YAW))
#define YAW_CE (3 << (2 * YAW))
#define YAW_HI (2 << (2 * YAW))
#define THR_LO (1 << (2 * THROTTLE))
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))
#define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN?
#define PWM_RANGE_MIN 1000
#define PWM_RANGE_MAX 2000
@ -52,15 +28,6 @@ typedef struct rxConfig_s {
uint16_t maxcheck; // maximum rc end
} rxConfig_t;
typedef struct controlRateConfig_s {
uint8_t rcRate8;
uint8_t rcExpo8;
uint8_t thrMid8;
uint8_t thrExpo8;
uint8_t rollPitchRate;
uint8_t yawRate;
} controlRateConfig_t;
typedef struct rxRuntimeConfig_s {
uint8_t channelCount; // number of rc channels as reported by current input driver
} rxRuntimeConfig_t;

1
src/rx_pwm.c
View File

@ -6,6 +6,7 @@
#include "platform.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "config.h"

1
src/rx_sbus.c
View File

@ -11,6 +11,7 @@
#include "failsafe.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "rx_sbus.h"

1
src/rx_spektrum.c
View File

@ -10,6 +10,7 @@
#include "serial_common.h"
#include "failsafe.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "rx_spektrum.h"

1
src/rx_sumd.c
View File

@ -10,6 +10,7 @@
#include "serial_common.h"
#include "failsafe.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "rx_sumd.h"

5
src/sensors_acceleration.c
View File

@ -18,6 +18,11 @@ extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
extern bool AccInflightCalibrationActive;
void ACC_SetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingA = calibrationCyclesRequired;
}
void ACC_Common(void)
{
static int32_t a[3];

2
src/sensors_acceleration.h
View File

@ -13,7 +13,9 @@ typedef enum AccelSensors {
extern uint8_t accHardware;
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t calibratingA;
void ACC_SetCalibrationCycles(uint16_t calibrationCyclesRequired);
void ACC_Common(void);
void ACC_getADC(void);

7
src/sensors_barometer.c
View File

@ -6,9 +6,16 @@
baro_t baro; // barometer access functions
#ifdef BARO
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
void Baro_SetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingB = calibrationCyclesRequired;
}
void Baro_Common(void)
{
static int32_t baroHistTab[BARO_TAB_SIZE_MAX];

1
src/sensors_barometer.h
View File

@ -1,6 +1,7 @@
#pragma once
#ifdef BARO
void Baro_SetCalibrationCycles(uint16_t calibrationCyclesRequired);
void Baro_Common(void);
int Baro_update(void);
int32_t Baro_calculateAltitude(void);

4
src/sensors_common.h
View File

@ -1,5 +1,9 @@
#pragma once
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
typedef enum {
SENSOR_GYRO = 1 << 0, // always present
SENSOR_ACC = 1 << 1,

5
src/sensors_gyro.c
View File

@ -11,6 +11,11 @@ uint16_t acc_1G = 256; // this is the 1G measured acceleration.
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void GYRO_SetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingG = calibrationCyclesRequired;
}
void GYRO_Common(void)
{
int axis;

1
src/sensors_gyro.h
View File

@ -4,6 +4,7 @@ extern uint16_t acc_1G;
extern gyro_t gyro;
extern sensor_align_e gyroAlign;
void GYRO_SetCalibrationCycles(uint16_t calibrationCyclesRequired);
void GYRO_Common(void);
void Gyro_getADC(void);

4
src/sensors_sonar.h Normal file
View File

@ -0,0 +1,4 @@
#pragma once
void Sonar_init(void);
void Sonar_update(void);