Browse Source

Working on IMU

master
Englebert 3 years ago
parent
commit
7a2b512c85
  1. 50
      src/Motors.cpp
  2. 1
      src/Motors.h
  3. 20
      src/config.h
  4. 24
      src/gyro.cpp
  5. 1
      src/gyro.h
  6. 549
      src/main.cpp
  7. 18
      src/main.h
  8. 325
      src/task.cpp
  9. 164
      src/task.h

50
src/Motors.cpp

@ -28,3 +28,53 @@ void Motors::initialize(void) {
void Motors::write_motor(uint16_t val1, uint16_t val2, uint16_t val3, uint16_t val4) {
d.send_cmd(val1, val2, val3, val4, false);
}
void Motors::mix_table() {
int16_t maxMotor;
uint8_t i;
#if defined(DYNBALANCE)
return;
#endif
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + YAW_DIRECTION * axisPID[YAW] * Z
#define SERVODIR(n,b) ((conf.servoConf[n].rate & b) ? -1 : 1)
// Main Mix Table
#if defined(MY_PRIVATE_MIXING)
#include MY_PRIVATE_MIXING
#elif defined(QUADX)
motor[0] = PIDMIX(-1,+1,-1); // REAR_R
motor[1] = PIDMIX(-1,-1,+1); // FRONT_R
motor[2] = PIDMIX(+1,+1,+1); // REAR_L
motor[3] = PIDMIX(+1,-1,-1); // FRONT_L
#else
#error "Missing quad copter mixtable entry. Please define it."
#endif
// Normalize the motors values
maxMotor = motor[0];
for(i = 1; i < NUMBER_MOTOR; i++)
if(motor[i] > maxMotor)
maxMotor = motor[i];
for(i = 0; i < NUMBER_MOTOR; i++) {
if(maxMotor > MAXTHROTTLE)
motor[i] -= maxMotor - MAXTHROTTLE; // This is a way to still have good gyro corrections if at least one of the motor reaches its max.
motor[i] = constrain(motor[i], conf.minthrottle, MAXTHROTTLE);
if(
(rcData[THROTTLE] < MINCHECK) &&
!flags.BARO_MODE
)
#ifndef MOTOR_STOP
motor[i] -= conf.minthrottle;
#else
motor[i] = MINCOMMAND;
#endif
if(!flags.ARMED)
motor[i] = MINCOMMAND;
}
}

1
src/Motors.h

@ -10,6 +10,7 @@ class Motors {
Motors(uint8_t motor1_pin, uint8_t motor2_pin, uint8_t motor3_pin, uint8_t motor4_pin);
void initialize(void);
void write_motor(uint16_t val1, uint16_t val2, uint16_t val3, uint16_t val4);
void mix_table(void);
private:
uint8_t _motor1_pin, _motor2_pin, _motor3_pin, _motor4_pin;

20
src/config.h

@ -1,13 +1,27 @@
#ifndef CONFIG_H
#define CONFIG_H
/* SECTION 1 - BASIC SETUP
*/
#define QUADX
// this is the maximum value for the ESCs at full power, this value can be increased up to 2000
#define MAXTHROTTLE 2000
/**************** SECTION 2 - COPTER TYPE SPECIFIC OPTIONS *******/
/******************************** PID Controller *********************************/
/* choose one of the alternate PID control algorithms
// this is the value for the ESCs when they are not armed
// in some cases, this value must be lowered down to 900 for some specific ESCs, otherwise they failed to initiate
#define MINCOMMAND 1000
/* SECTION 2 - COPTER TYPE SPECIFIC OPTIONS
* === PID Controller ===
* choose one of the alternate PID control algorithms
* 1 = evolved oldschool algorithm (similar to v2.2)
* 2 = new experimental algorithm from Alex Khoroshko - unsupported - http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671&start=10#p37387
*/
#define PID_CONTROLLER 2
#define YAW_DIRECTION 1
//#define YAW_DIRECTION -1 // if you want to reverse the yaw correction direction
#define FAILSAFE
/*
* GPS configuration. For now only serial GPS that I have. Set GPS 1 if exits and GPS 0 if not exists

24
src/gyro.cpp

@ -1,5 +1,9 @@
#include "baro.h"
int16_t gyroADCprevious[3] = {0, 0, 0};
int16_t gyroADCinter[3];
uint16_t timeInterleave = 0;
GYRO::GYRO(void) {
// Switch the baro to SPI
// If CSB is pulled down, the SPI interface is activated. After
@ -72,14 +76,17 @@ void GYRO::begin(void) {
Serial.println("GYRO initialized and unselected");
}
uint16_t GYRO::get_readings(void) {
}
uint8_t GYRO::get_id(void) {
uint8_t status = read_register(WHO_AM_I);
Serial.print("Status SPI Gyro (WHO_AM_I): "); Serial.println(status);
}
inline void GYRO::beginTransaction() {
//_SPI.beginTransaction(SPISettings(10000000, MSBFIRST, SPI_MODE0));
_SPI.beginTransaction(SPISettings(10000000, MSBFIRST, SPI_MODE0));
@ -88,6 +95,7 @@ inline void GYRO::beginTransaction() {
// delayMicroseconds(10);
}
inline void GYRO::endTransaction() {
// digitalWrite(GYRO_CS, HIGH);
GPIO.out_w1ts = ((uint32_t)1 << GYRO_CS);
@ -95,6 +103,7 @@ inline void GYRO::endTransaction() {
_SPI.endTransaction();
}
uint8_t GYRO::read_register(uint8_t addr) {
int8_t status;
uint8_t result;
@ -106,3 +115,18 @@ uint8_t GYRO::read_register(uint8_t addr) {
return result;
}
void GYRO::compute_imu(void) {
uint8_t axis;
timeInterleave = 0;
#if ACC
ACC_getADC();
getEstimatedAttitude();
#endif
#if GYRO
Gyro_getADB();
#endif
}

1
src/gyro.h

@ -122,6 +122,7 @@ class GYRO {
void beginTransaction(void);
void endTransaction(void);
uint8_t read_register(uint8_t addr);
void compute_imu(void);
private:
};

549
src/main.cpp

@ -8,15 +8,169 @@ NRF24L01 rx(RX_CE_PIN, RX_CSN_PIN); // Setting up receiver module
GYRO gyro;
WIFI wifi;
WEB web;
TASK task;
// TASK task;
// Seetting up Motors
Motors motors = Motors(MOTOR1, MOTOR2, MOTOR3, MOTOR4);
RXMessage rxMessage;
// Global Variables
uint16_t rcCommand[RC_CHANS];
uint16_t rcCommand[RC_CHANS]; // Signal to ESC
uint16_t rcData[RC_CHANS]; // Signal from nRF24L01+
int16_t rcSerial[8]; // Signal from Serial Port
uint16_t failsafeCnt = 0; // To trigger failsave if any issues
flags_struct_t flags;
// Common variables
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
uint8_t rcSticks; // This hold sticks position for command combos
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta;
int16_t PTerm = 0, ITerm = 0, DTerm, PTermACC, ITermACC;
int16_t lastGyro[2] = {0, 0};
int16_t errorAngleI[2] = {0, 0};
#if PID_CONTROLLER == 1
int32_t errorGyroI_YAW;
int16_t delta1[2], delta2[2];
int16_t errorGyroI[2] = {0, 0};
#elif PID_CONTROLLER == 2
int16_t delta1[3], delta2[3];
int32_t errorGyroI[3] = {0, 0, 0};
int16_t lastError[3] = {0, 0, 0};
int16_t deltaSum;
int16_t AngleRateTmp, RateError;
#endif
uint32_t rcTime = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
int16_t initialThrottleHold;
int16_t rc;
int32_t prop = 0;
int16_t magHold, headFreeModeHold; // [-180; +180]
uint16_t cycleTime = 0; // This is the number in micro second to achieve a full loop, it can differ a little and is taken
uint16_t calibratingA = 0; // The calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then w
uint16_t calibratingB = 0; // Baro calibration = get new ground pressure value
uint16_t calibratingG;
uint8_t vbatMin = VBATNOMINAL; // Lowest battery voltage in 0.1V steps
uint8_t rcOptions[CHECKBOXITEMS];
int16_t sonarAlt;
int16_t BaroPID = 0;
int16_t errorAltitudeI = 0;
// begin GPS common variables and define
#if GPS
gps_conf_struct GPS_conf;
#endif
int16_t GPS_angle[2] = {0, 0}; // the angles that must be applied for GPS correction
int32_t GPS_coord[2];
int32_t GPS_home[2];
int32_t GPS_hold[2];
int32_t GPS_prev[2];
int32_t GPS_poi[2]; // Coordinates of the current poi
int32_t GPS_directionToPoi; // direction to the actual poi (used to set heading to poi)
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // distance to home - unit: meter
int16_t GPS_directionToHome; // direction to home - unit: degree
uint16_t GPS_altitude; // GPS altitude - unit: meter
uint16_t GPS_speed; // GPS speed - unit: cm/s
uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update
uint16_t GPS_ground_course = 0; // - unit: degree*10
uint32_t GPS_time;
uint8_t GPS_mode; // contains the current selected gps flight mode
uint8_t NAV_error = 0; // Last error situation of the nav engine
uint8_t NAV_state = 0; // State of the nav engine
uint8_t GPS_saved_mission_state; // The mission state saved when poshold invoked during mission
uint8_t prv_gps_modes = 0; // GPS_checkbox items packed into 1 byte for checking GPS mode changes
uint32_t nav_timer_stop = 0; // common timer used in navigation (contains the desired stop time in millis()
uint16_t nav_hold_time; // time in seconds to hold position
uint8_t NAV_paused_at = 0; // This contains the mission step where poshold paused the runing mission.
uint8_t next_step = 1; // The mission step which is upcoming it equals with the mission_step stored in EEPROM
int16_t jump_times = -10; // How many loops do we have to do (alt/100 from mission step) -10 means not used jet, -1 unlimited
uint8_t isAltHoldChanged = 0;
uint16_t AltHoldCorr = 0;
uint32_t AltHold = 0; // in cm
// Altitude control state
#define ASCENDING 1
#define DESCENDING -1
#define REACHED_ALT 0
// The orginal altitude used as base our new altitude during nav
int32_t original_altitude;
// This is the target what we want to reach
int32_t target_altitude;
// This is the interim value which is feeded into the althold controller
int32_t alt_to_hold;
uint32_t alt_change_timer;
int8_t alt_change_flag;
uint32_t alt_change;
uint8_t land_detect; // land detector variable
uint8_t alarmArray[ALRM_FAC_SIZE]; // array
#if GPS
// Mission step structure
mission_step_struct mission_step;
#endif
// Possible action codes for a mission step
#define MISSION_WAYPOINT 1 //Set waypoint
#define MISSION_HOLD_UNLIM 2 //Poshold unlimited
#define MISSION_HOLD_TIME 3 //Hold for a predetermined time
#define MISSION_RTH 4 //Return to HOME
#define MISSION_SET_POI 5 //Set POINT of interest
#define MISSION_JUMP 6 //Jump to the given step (#times)
#define MISSION_SET_HEADING 7 //Set heading to a given orientation (parameter 1 is the waym 0-359 degree
#define MISSION_LAND 8 //Land at the given position
#define MISSION_FLAG_END 0xA5 //Flags that this is the last step
#define MISSION_FLAG_CRC_ERROR 0xFE //Returned WP had an EEPROM CRC error
#define MISSION_FLAG_HOME 0x01 //Returned WP is the home position
#define MISSION_FLAG_HOLD 0x02 //Returned WP is the hold position
#define MISSION_FLAG_DO_LAND 0x20 //Land when reached desired point (used in RTH)
#define MISSION_FLAG_NAV_IN_PROG 0xff //Navigation is in progress, returned wp is home
#define LAT 0
#define LON 1
// The desired bank towards North (Positive) or South (Negative) : latitude
// The desired bank towards East (Positive) or West (Negative) : longitude
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
#if BARO
int32_t baroPressure;
int16_t baroTemperature;
int32_t baroPressureSum;
#endif
alt_t alt;
att_t att;
imu_t imu;
conf_t conf;
static uint8_t dynP8[2], dynD8[2];
#if defined(THROTTLE_ANGLE_CORRECTION)
int16_t throttleAngleCorrection = 0; // Correction of throttle in lateral wind,
int8_t cosZ = 100; // cos(angleZ)*100
#endif
int16_t axisPID[3];
int16_t motor[8];
// Normal Setup Loop
void setup() {
Serial.begin(115200);
@ -36,8 +190,6 @@ void setup() {
}
gyro.get_id();
rx.begin();
task.begin();
// Serial.print("FREE RAM: "); Serial.print(ESP.getFreeHeap()); Serial.println(" MB");
@ -306,11 +458,394 @@ void taskDebug(void *pvParameters) {
}
// Main Task of the system
void taskMain(void *pvParameters) {
(void) pvParameters;
rx.stopListening();
rx.setAutoAck(false);
rx.setChannel(8); // ************* Temporary on 8 ... later make it auto ***************************
rx.setPayloadSize(sizeof(RXMessage));
rx.setDataRate(RF24_250KBPS);
rx.setPALevel(RF24_PA_MAX);
rx.openReadingPipe(1, pipeIn);
rx.startListening();
for(;;) {
vTaskDelay(1); // Theoritically 1mS delay ~ 1000Hz. <--- will this sufficient? Need to try
run_tasks();
// vTaskDelay(1); // Theoritically 1mS delay ~ 1000Hz. <--- will this sufficient? Need to try
}
}
// ********* Main Program
void run_tasks(void) {
if((uint32_t)(currentTime - rcTime) > 0) {
// RC Loop
rcTime = currentTime + 200000;
process_rc();
} else {
// Not in RC Loop
process_modules();
}
// Loop time
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
// Compute IMU
gyro.compute_imu();
// Experimental FlightModes
#if defined(ACROTRAINER_MODE)
if(flags.ANGLE_MODE) {
if(abs(rcCommand[ROLL] + ads(rcCommand[PITCH]) >= ACROTRAINER_MODE) {
flags.ANGLE_MODE = 0;
flags.HORIZON_MODE = 0;
flags.MAG_MODE = 0;
flags.BARO_MODE = 0;
GPS_mode = GPS_MODE_NONE;
}
}
#endif
// THROTTLE sticks during mission and RTH
#if GPS
if(GPS_conf.ignore_throttle == 1) {
if(
flags.GPS_mode == GPS_MODE_NAV ||
flags.GPS_mode == GPS_MODE_RTH
) {
// rcCommand[ROLL] = 0;
// rcCommand[PITCH] = 0;
// rcCommand[YAW] = 0;
flags.THROTTLE_IGNORED = 1;
} else {
flags.THROTTLE_IGNORED = 0;
}
}
// Heading manipulation
// TODO: Do heading manipulation
#endif
if(
abs(rcCommand[YAW]) < 70 &&
flags.MAG_MODE
){
int16_t dif = att.heading - magHold;
if(dif <= -180) dif += 360;
if(dif >= +180) dif -= 360;
if(
flags.SMALL_ANGLES_25 ||
(flags.GPS_mode != 0)
) {
rcCommand[YAW] -= dif * conf.pid[PIDMAG].P8 >> 5; // Always correct maghold in GPS MODE
} else {
magHold = att.heading;
}
}
#if BARO && (!defined(SURPRESS_BARO_ALTHOLD))
/* Smooth alt change routing for slow auto aerophoto modes. It is slowly increase/decrease altitude
* proportional to stick movement (+/-100 throttle gives about +/-50cm in 1 second with cycle time about 3-4ms)
*/
if(flags.BARO_MODE) {
isAltHoldChanged = 0;
AltHoldCorr = 0;
#if GPS
// If autoland is in progress then take over and decrease alt slowly
if(flags.LAND_IN_PROGRESS) {
AltHoldCorr -= GPS_conf.land_speed;
if(abs(AltHoldCorr) > 512) {
AltHold += AltHoldCorr / 512;
AltHoldCorr %= 512;
}
}
#endif
// If throttle not ignored then allow change altitude with the stick
if(
(abs(rcCommand[THROTTLE] - initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) &&
!flags.THROTTLE_IGNORED)
{
// Slowly increase/decrease AltHold proportional to stick movement (+100 throttle gives ~ +50cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
if(abs(AltHoldCorr) > 512) {
AltHold += AltHoldCorr / 512;
AltHoldCorr %= 512;
}
isAltHoldChanged = 1;
} else {
AltHold = alt.EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
#endif // BARO
#if defined(THROTTLE_ANGLE_CORRECTION)
if(
flags.ANGLE_MODE ||
flags.HORIZON_MODE
) {
rcCommand[THROTTLE] += throttleAngleCorrection;
}
#endif
#if GPS
// TODO: Split cos_yaw calculations into two phases (X and Y)
if(
(flags.GPS_mode != GPS_MODE_NONE) &&
flags.GPS_FIX_HOME
){
float sin_yaw_y = sin(att.heading * 0.0174532925f);
float cos_yaw_x = cos(att.heading * 0.0174532925f);
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
}
// Used to communicate back nav angles to the GPS simulator (for HIL testing)
#if defined(GPS_SIMULATOR)
// SerialWrite(2,0xa5);
// SerialWrite16(2,nav[LAT]+rcCommand[PITCH]);
// SerialWrite16(2,nav[LON]+rcCommand[ROLL]);
// SerialWrite16(2,(nav[LAT]+rcCommand[PITCH])-(nav[LON]+rcCommand[ROLL])); //check
#endif
#endif //GPS
// **** PITCH & ROLL & YAW PID ****
#if PID_CONTROLLER == 1 // evolved oldschool
if(flags.HORIZON_MODE)
prop = _min( max( abs(rcCommand[PITCH]), abs(rcCommand[ROLL]) ), 512);
// PITCH & ROLL
for(axis = 0; axis < 2; axis++){
rc = rcCommand[axis] << 1;
error = rc - imu.gyroData[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error, -16000, +16000); // WindUp 16 bits is ok here
if(abs(imu.gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] >> 7) * conf.pid[axis].I8 >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250
// PTerm = mul(rc, conf.pid[axis].P8) >> 6;
PTerm = (rc * conf.pid[axis].P8) >> 6;
if(
flags.ANGLE_MODE ||
flags.HORIZON_MODE
){
// axis relying on ACC
// 50 degrees max inclination
errorAngle = constrain(rc + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; // 16 bits is ok
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);
// PTermACC = mul(errorAngle, conf.pid[PIDLEVEL].P8) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could
PTermACC = (errorAngle * conf.pid[PIDLEVEL].P8) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could
int16_t limit = conf.pid[PIDLEVEL].D8 * 5;
PTermACC = constrain(PTermACC, -limit, +limit);
// ITermACC = mul(errorAngleI[axis], conf.pid[PIDLEVEL].I8) >> 12; // 32 bits is needed for calculation:10000*I8 c
ITermACC = (errorAngleI[axis] * conf.pid[PIDLEVEL].I8) >> 12; // 32 bits is needed for calculation:10000*I8 c
ITerm = ITermACC + ((ITerm-ITermACC) * prop >> 9);
PTerm = PTermACC + ((PTerm-PTermACC) * prop >> 9);
}
// PTerm -= mul(imu.gyroData[axis], dynP8[axis]) >> 6; // 32 bits is needed for calculation
PTerm -= (imu.gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation
delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is
lastGyro[axis] = imu.gyroData[axis];
DTerm = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// DTerm = mul(DTerm,dynD8[axis]) >> 5; // 32 bits is needed for calculation
DTerm = (DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
} // End of for(axis...
// YAW
#define GYRO_P_MAX 300
#define GYRO_I_MAX 250
// rc = mul(rcCommand[YAW] , (2 * conf.yawRate + 30)) >> 5;
rc = (rcCommand[YAW] * (2 * conf.yawRate + 30)) >> 5;
error = rc - imu.gyroData[YAW];
// errorGyroI_YAW += mul(error,conf.pid[YAW].I8);
errorGyroI_YAW += (error * conf.pid[YAW].I8);
errorGyroI_YAW = constrain(errorGyroI_YAW, 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
if(abs(rc) > 50)
errorGyroI_YAW = 0;
// PTerm = mul(error,conf.pid[YAW].P8) >> 6;
PTerm = (error * conf.pid[YAW].P8) >> 6;
ITerm = constrain((int16_t)(errorGyroI_YAW >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
axisPID[YAW] = PTerm + ITerm;
#elif PID_CONTROLLER == 2 // alexK
#define GYRO_I_MAX 256
#define ACC_I_MAX 256
prop = _min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500]
// ----------PID controller----------
for(axis = 0; axis < 3; axis++){
//-----Get the desired angle rate depending on flight mode
if(
(flags.ANGLE_MODE || flags.HORIZON_MODE) &&
axis < 2
){
// MODE relying on ACC
// Calculate error and limit the angle to 50 degrees max inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; // 16
}
if(axis == 2){
// YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t) (conf.yawRate + 27) * rcCommand[2]) >> 5);
} else {
if(!flags.ANGLE_MODE){
// Control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if(flags.HORIZON_MODE){
// Mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += ((int32_t) errorAngle * conf.pid[PIDLEVEL].I8) >> 8;
}
} else {
//it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = ((int32_t) errorAngle * conf.pid[PIDLEVEL].P8) >> 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 - imu.gyroData[axis];
// -----calculate P component
PTerm = ((int32_t) RateError * conf.pid[axis].P8) >> 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] += (((int32_t) RateError * cycleTime) >> 11) * conf.pid[axis].I8;
// 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
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 = ((int32_t) 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*conf.pid[axis].D8)>>8;
// Solve overflow in calculation above...
DTerm = ((int32_t)deltaSum*conf.pid[axis].D8) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
#else
#error "*** you must set PID_CONTROLLER to one existing implementation"
#endif
motors.mix_table();
// Do not update servos during unarmed calibration of sensors which are sensitive to vibration
/********************
#if defined(DISABLE_SERVOS_WHEN_UNARMED)
if(flags.ARMED)
writeServos();
#else
if(
(flags.ARMED) ||
((!calibratingG) && (!calibratingA)))
writeServos();
#endif
********************/
///// writeMotors();
}
void process_rc(void) {
if(rx.available()) {
// Getting signals from Receiver
rx.read(&rxMessage, sizeof(RXMessage));
/****
Serial.print(rxMessage.Byte00);
Serial.print(" ");
Serial.print(rxMessage.Byte01);
Serial.print(" ");
Serial.print(rxMessage.Byte02);
Serial.print(" ");
Serial.print(rxMessage.Byte03);
Serial.print(" ");
Serial.print(rxMessage.Byte04);
Serial.print(" ");
Serial.print(rxMessage.Byte05);
Serial.println(" ");
***/
// static bool firstload = true;
// 2021 New Data format for saving 2 bytes:
// 1234 5678 9TTT TTTT TTTT -YYY YYYY YYYY -PPP PPPP PPPP -RRR RRRR RRRR (7 Bytes)
// |-------| |-------| |-------| |-------| |-------| |-------| |-------|
// Byte 00 Byte 01 Byte 02 Byte 03 Byte 04 Byte 05 Byte 06
// Extracting data
// Processing switches channel....
rcData[AUX1] = ((rxMessage.Byte00 >> 7) & 0x01) ? 2000 : 1000; // 1
rcData[AUX2] = ((rxMessage.Byte00 >> 6) & 0x01) ? 2000 : 1000; // 2
rcData[AUX3] = ((rxMessage.Byte00 >> 5) & 0x01) ? 2000 : 1000; // 3
rcData[AUX4] = ((rxMessage.Byte00 >> 4) & 0x01) ? 2000 : 1000; // 4
rcData[AUX5] = ((rxMessage.Byte00 >> 3) & 0x01) ? 2000 : 1000; // 5
rcData[AUX6] = ((rxMessage.Byte00 >> 2) & 0x01) ? 2000 : 1000; // 6
rcData[AUX7] = ((rxMessage.Byte00 >> 1) & 0x01) ? 2000 : 1000; // 7
rcData[AUX8] = ((rxMessage.Byte00 >> 0) & 0x01) ? 2000 : 1000; // 8
rcData[AUX9] = ((rxMessage.Byte01 >> 7) & 0x01) ? 2000 : 1000; // 9
rcData[THROTTLE] = ((rxMessage.Byte01 & 0X7F) << 4) | (rxMessage.Byte02 & 0xF0) >> 4; // Throttle
rcData[YAW] = ((rxMessage.Byte02 & 0x07) << 8) | rxMessage.Byte03; // Yaw
rcData[PITCH] = ((rxMessage.Byte04 & 0x7F) << 4) | (rxMessage.Byte05 & 0xF0) >> 4; // Pitch
rcData[ROLL] = ((rxMessage.Byte05 & 0x07) << 8) | rxMessage.Byte06; // Roll
#if defined(FAILSAFE)
failsafeCnt = 0;
#endif
} else {
#if defined(FAILSAFE)
failsafeCnt++;
#endif
}
}
void process_modules(void) {
}

18
src/main.h

@ -44,7 +44,12 @@ const uint64_t pipeIn = 0xE8E8F0F0E1LL; // Must be same as the transmission
#define RC_CHANS 18
#define VBAT_CELLS_NUM 1 // Set this to the number of cells you monitor via analog pins
#if defined(QUADX) || defined(QUADP)
#define NUMBER_MOTOR 4
#endif
#define MINCHECK 1100
#define MAXCHECK 1900
// For RC Functions - Migrated from Multiwii
@ -418,7 +423,7 @@ typedef struct {
#include "baro.h"
#include "gyro.h"
#include "web.h"
#include "task.h"
// #include "task.h"
// Task scheduler
#define APP_CPU 1
@ -433,6 +438,11 @@ void taskMeasurements(void *pvParameters);
void taskMain(void *pvParameters); // Main task
void taskDebug(void *pvParameters); // <-- Only for debugging purposes. Will be disabled when done.
void process_rc(void);
void process_modules(void);
void run_tasks(void);
/***
void taskReceiver(void *pvParameters);
void taskGyro(void *pvParameters);
@ -449,5 +459,9 @@ void taskBaro(void *pvParameters);
extern flags_struct_t flags;
extern uint16_t rcCommand[RC_CHANS];
extern uint16_t rcData[RC_CHANS]; // Signal from nRF24L01+
extern int16_t motor[8];
extern int16_t axisPID[3];
extern conf_t conf;
extern uint16_t failsafeCnt;
#endif

325
src/task.cpp

@ -1,325 +0,0 @@
#include "task.h"
TASK::TASK(void) {
}
void TASK::begin(void) {
}
void TASK::run(void) {
if((uint32_t)(currentTime - rcTime) > 0) {
// RC Loop
process_rc();
} else {
// Not in RC Loop
process_modules();
}
// Loop time
currentTime = micros();
// Compute IMU
// Experimental FlightModes
#if defined(ACROTRAINER_MODE)
if(flags.ANGLE_MODE) {
if(abs(rcCommand[ROLL] + ads(rcCommand[PITCH]) >= ACROTRAINER_MODE) {
flags.ANGLE_MODE = 0;
flags.HORIZON_MODE = 0;
flags.MAG_MODE = 0;
flags.BARO_MODE = 0;
GPS_mode = GPS_MODE_NONE;
}
}
#endif
// THROTTLE sticks during mission and RTH
#if GPS
if(GPS_conf.ignore_throttle == 1) {
if(
flags.GPS_mode == GPS_MODE_NAV ||
flags.GPS_mode == GPS_MODE_RTH
) {
// rcCommand[ROLL] = 0;
// rcCommand[PITCH] = 0;
// rcCommand[YAW] = 0;
flags.THROTTLE_IGNORED = 1;
} else {
flags.THROTTLE_IGNORED = 0;
}
}
// Heading manipulation
// TODO: Do heading manipulation
#endif
if(
abs(rcCommand[YAW]) < 70 &&
flags.MAG_MODE
){
int16_t dif = att.heading - magHold;
if(dif <= -180) dif += 360;
if(dif >= +180) dif -= 360;
if(
flags.SMALL_ANGLES_25 ||
(flags.GPS_mode != 0)
) {
rcCommand[YAW] -= dif * conf.pid[PIDMAG].P8 >> 5; // Always correct maghold in GPS MODE
} else {
magHold = att.heading;
}
}
#if BARO && (!defined(SURPRESS_BARO_ALTHOLD))
/* Smooth alt change routing for slow auto aerophoto modes. It is slowly increase/decrease altitude
* proportional to stick movement (+/-100 throttle gives about +/-50cm in 1 second with cycle time about 3-4ms)
*/
if(flags.BARO_MODE) {
isAltHoldChanged = 0;
AltHoldCorr = 0;
#if GPS
// If autoland is in progress then take over and decrease alt slowly
if(flags.LAND_IN_PROGRESS) {
AltHoldCorr -= GPS_conf.land_speed;
if(abs(AltHoldCorr) > 512) {
AltHold += AltHoldCorr / 512;
AltHoldCorr %= 512;
}
}
#endif
// If throttle not ignored then allow change altitude with the stick
if(
(abs(rcCommand[THROTTLE] - initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) &&
!flags.THROTTLE_IGNORED)
{
// Slowly increase/decrease AltHold proportional to stick movement (+100 throttle gives ~ +50cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
if(abs(AltHoldCorr) > 512) {
AltHold += AltHoldCorr / 512;
AltHoldCorr %= 512;
}
isAltHoldChanged = 1;
} else {
AltHold = alt.EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
#endif // BARO
#if defined(THROTTLE_ANGLE_CORRECTION)
if(
flags.ANGLE_MODE ||
flags.HORIZON_MODE
) {
rcCommand[THROTTLE] += throttleAngleCorrection;
}
#endif
#if GPS
// TODO: Split cos_yaw calculations into two phases (X and Y)
if(
(flags.GPS_mode != GPS_MODE_NONE) &&
flags.GPS_FIX_HOME
){
float sin_yaw_y = sin(att.heading * 0.0174532925f);
float cos_yaw_x = cos(att.heading * 0.0174532925f);
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
}
// Used to communicate back nav angles to the GPS simulator (for HIL testing)
#if defined(GPS_SIMULATOR)
// SerialWrite(2,0xa5);
// SerialWrite16(2,nav[LAT]+rcCommand[PITCH]);
// SerialWrite16(2,nav[LON]+rcCommand[ROLL]);
// SerialWrite16(2,(nav[LAT]+rcCommand[PITCH])-(nav[LON]+rcCommand[ROLL])); //check
#endif
#endif //GPS
// **** PITCH & ROLL & YAW PID ****
#if PID_CONTROLLER == 1 // evolved oldschool
if(flags.HORIZON_MODE)
prop = _min( max( abs(rcCommand[PITCH]), abs(rcCommand[ROLL]) ), 512);
// PITCH & ROLL
for(axis = 0; axis < 2; axis++){
rc = rcCommand[axis] << 1;
error = rc - imu.gyroData[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error, -16000, +16000); // WindUp 16 bits is ok here
if(abs(imu.gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] >> 7) * conf.pid[axis].I8 >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250
// PTerm = mul(rc, conf.pid[axis].P8) >> 6;
PTerm = (rc * conf.pid[axis].P8) >> 6;
if(
flags.ANGLE_MODE ||
flags.HORIZON_MODE
){
// axis relying on ACC
// 50 degrees max inclination
errorAngle = constrain(rc + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; // 16 bits is ok
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);
// PTermACC = mul(errorAngle, conf.pid[PIDLEVEL].P8) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could
PTermACC = (errorAngle * conf.pid[PIDLEVEL].P8) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could
int16_t limit = conf.pid[PIDLEVEL].D8 * 5;
PTermACC = constrain(PTermACC, -limit, +limit);
// ITermACC = mul(errorAngleI[axis], conf.pid[PIDLEVEL].I8) >> 12; // 32 bits is needed for calculation:10000*I8 c
ITermACC = (errorAngleI[axis] * conf.pid[PIDLEVEL].I8) >> 12; // 32 bits is needed for calculation:10000*I8 c
ITerm = ITermACC + ((ITerm-ITermACC) * prop >> 9);
PTerm = PTermACC + ((PTerm-PTermACC) * prop >> 9);
}
// PTerm -= mul(imu.gyroData[axis], dynP8[axis]) >> 6; // 32 bits is needed for calculation
PTerm -= (imu.gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation
delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is
lastGyro[axis] = imu.gyroData[axis];
DTerm = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// DTerm = mul(DTerm,dynD8[axis]) >> 5; // 32 bits is needed for calculation
DTerm = (DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
} // End of for(axis...
// YAW
#define GYRO_P_MAX 300
#define GYRO_I_MAX 250
// rc = mul(rcCommand[YAW] , (2 * conf.yawRate + 30)) >> 5;
rc = (rcCommand[YAW] * (2 * conf.yawRate + 30)) >> 5;
error = rc - imu.gyroData[YAW];
// errorGyroI_YAW += mul(error,conf.pid[YAW].I8);
errorGyroI_YAW += (error * conf.pid[YAW].I8);
errorGyroI_YAW = constrain(errorGyroI_YAW, 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
if(abs(rc) > 50)
errorGyroI_YAW = 0;
// PTerm = mul(error,conf.pid[YAW].P8) >> 6;
PTerm = (error * conf.pid[YAW].P8) >> 6;
ITerm = constrain((int16_t)(errorGyroI_YAW >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
axisPID[YAW] = PTerm + ITerm;
#elif PID_CONTROLLER == 2 // alexK
#define GYRO_I_MAX 256
#define ACC_I_MAX 256
prop = _min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500]
// ----------PID controller----------
for(axis = 0; axis < 3; axis++){
//-----Get the desired angle rate depending on flight mode
if(
(flags.ANGLE_MODE || flags.HORIZON_MODE) &&
axis < 2
){
// MODE relying on ACC
// Calculate error and limit the angle to 50 degrees max inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; // 16
}
if(axis == 2){
// YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t) (conf.yawRate + 27) * rcCommand[2]) >> 5);
} else {
if(!flags.ANGLE_MODE){
// Control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if(flags.HORIZON_MODE){
// Mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += ((int32_t) errorAngle * conf.pid[PIDLEVEL].I8) >> 8;
}
} else {
//it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = ((int32_t) errorAngle * conf.pid[PIDLEVEL].P8) >> 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 - imu.gyroData[axis];
// -----calculate P component
PTerm = ((int32_t) RateError * conf.pid[axis].P8) >> 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] += (((int32_t) RateError * cycleTime) >> 11) * conf.pid[axis].I8;
// 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
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 = ((int32_t) 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*conf.pid[axis].D8)>>8;
// Solve overflow in calculation above...
DTerm = ((int32_t)deltaSum*conf.pid[axis].D8) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
#else
#error "*** you must set PID_CONTROLLER to one existing implementation"
#endif
mixTable();
// Do not update servos during unarmed calibration of sensors which are sensitive to vibration
/********************
#if defined(DISABLE_SERVOS_WHEN_UNARMED)
if(flags.ARMED)
writeServos();
#else
if(
(flags.ARMED) ||
((!calibratingG) && (!calibratingA)))
writeServos();
#endif
********************/
writeMotors();
}
void TASK::process_rc(void) {
}
void TASK::process_modules(void) {
}

164
src/task.h

@ -1,164 +0,0 @@
#ifndef TASK_H
#define TASK_H
#include "Arduino.h"
#include "main.h"
class TASK {
public:
TASK(void);
void begin(void);
void run(void);
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
uint8_t rcSticks; // This hold sticks position for command combos
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta;
int16_t PTerm = 0, ITerm = 0, DTerm, PTermACC, ITermACC;
int16_t lastGyro[2] = {0, 0};
int16_t errorAngleI[2] = {0, 0};
#if PID_CONTROLLER == 1
int32_t errorGyroI_YAW;
int16_t delta1[2], delta2[2];
int16_t errorGyroI[2] = {0, 0};
#elif PID_CONTROLLER == 2
int16_t delta1[3], delta2[3];
int32_t errorGyroI[3] = {0, 0, 0};
int16_t lastError[3] = {0, 0, 0};
int16_t deltaSum;
int16_t AngleRateTmp, RateError;
#endif
uint32_t rcTime = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
int16_t initialThrottleHold;
int16_t rc;
int32_t prop = 0;
int16_t magHold, headFreeModeHold; // [-180; +180]
uint16_t cycleTime = 0; // This is the number in micro second to achieve a full loop, it can differ a little and is taken
uint16_t calibratingA = 0; // The calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then w
uint16_t calibratingB = 0; // Baro calibration = get new ground pressure value
uint16_t calibratingG;
uint8_t vbatMin = VBATNOMINAL; // Lowest battery voltage in 0.1V steps
uint8_t rcOptions[CHECKBOXITEMS];
int16_t sonarAlt;
int16_t BaroPID = 0;
int16_t errorAltitudeI = 0;
// begin GPS common variables and define
#if GPS
gps_conf_struct GPS_conf;
#endif
int16_t GPS_angle[2] = {0, 0}; // the angles that must be applied for GPS correction
int32_t GPS_coord[2];
int32_t GPS_home[2];
int32_t GPS_hold[2];
int32_t GPS_prev[2];
int32_t GPS_poi[2]; // Coordinates of the current poi
int32_t GPS_directionToPoi; // direction to the actual poi (used to set heading to poi)
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // distance to home - unit: meter
int16_t GPS_directionToHome; // direction to home - unit: degree
uint16_t GPS_altitude; // GPS altitude - unit: meter
uint16_t GPS_speed; // GPS speed - unit: cm/s
uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update
uint16_t GPS_ground_course = 0; // - unit: degree*10
uint32_t GPS_time;
uint8_t GPS_mode; // contains the current selected gps flight mode
uint8_t NAV_error = 0; // Last error situation of the nav engine
uint8_t NAV_state = 0; // State of the nav engine
uint8_t GPS_saved_mission_state; // The mission state saved when poshold invoked during mission
uint8_t prv_gps_modes = 0; // GPS_checkbox items packed into 1 byte for checking GPS mode changes
uint32_t nav_timer_stop = 0; // common timer used in navigation (contains the desired stop time in millis()
uint16_t nav_hold_time; // time in seconds to hold position
uint8_t NAV_paused_at = 0; // This contains the mission step where poshold paused the runing mission.
uint8_t next_step = 1; // The mission step which is upcoming it equals with the mission_step stored in EEPROM
int16_t jump_times = -10; // How many loops do we have to do (alt/100 from mission step) -10 means not used jet, -1 unlimited
uint8_t isAltHoldChanged = 0;
uint16_t AltHoldCorr = 0;
uint32_t AltHold = 0; // in cm
// Altitude control state
#define ASCENDING 1
#define DESCENDING -1
#define REACHED_ALT 0
// The orginal altitude used as base our new altitude during nav
int32_t original_altitude;
// This is the target what we want to reach
int32_t target_altitude;
// This is the interim value which is feeded into the althold controller
int32_t alt_to_hold;
uint32_t alt_change_timer;
int8_t alt_change_flag;
uint32_t alt_change;
uint8_t land_detect; // land detector variable
uint8_t alarmArray[ALRM_FAC_SIZE]; // array
#if GPS
// Mission step structure
mission_step_struct mission_step;
#endif
// Possible action codes for a mission step
#define MISSION_WAYPOINT 1 //Set waypoint
#define MISSION_HOLD_UNLIM 2 //Poshold unlimited
#define MISSION_HOLD_TIME 3 //Hold for a predetermined time
#define MISSION_RTH 4 //Return to HOME
#define MISSION_SET_POI 5 //Set POINT of interest
#define MISSION_JUMP 6 //Jump to the given step (#times)
#define MISSION_SET_HEADING 7 //Set heading to a given orientation (parameter 1 is the waym 0-359 degree
#define MISSION_LAND 8 //Land at the given position
#define MISSION_FLAG_END 0xA5 //Flags that this is the last step
#define MISSION_FLAG_CRC_ERROR 0xFE //Returned WP had an EEPROM CRC error
#define MISSION_FLAG_HOME 0x01 //Returned WP is the home position
#define MISSION_FLAG_HOLD 0x02 //Returned WP is the hold position
#define MISSION_FLAG_DO_LAND 0x20 //Land when reached desired point (used in RTH)
#define MISSION_FLAG_NAV_IN_PROG 0xff //Navigation is in progress, returned wp is home
#define LAT 0
#define LON 1
// The desired bank towards North (Positive) or South (Negative) : latitude
// The desired bank towards East (Positive) or West (Negative) : longitude
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
#if BARO
int32_t baroPressure;
int16_t baroTemperature;
int32_t baroPressureSum;
#endif
alt_t alt;
att_t att;
imu_t imu;
conf_t conf;
static uint8_t dynP8[2], dynD8[2];
#if defined(THROTTLE_ANGLE_CORRECTION)
int16_t throttleAngleCorrection = 0; // Correction of throttle in lateral wind,
int8_t cosZ = 100; // cos(angleZ)*100
#endif
int16_t axisPID[3];
private:
void process_rc(void);
void process_modules(void);
};
#endif
Loading…
Cancel
Save