englebert
/
ExpressFC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Checkpoint commit

master
Englebert 3 years ago
parent
ba67973a03
commit
3ce47f75cd
7 changed files with 292 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      README.md
  2. 17
      config.h
  3. 1
      platformio.ini
  4. 46
      src/config.h
  5. 36
      src/main.h
  6. 220
      src/task.cpp
  7. 19
      src/task.h

4
README.md
View File

@ -9,6 +9,10 @@ OSD ----- [ ]
VMETER -- [ WORKING ] VMETER -- [ WORKING ]
CMETER -- [ WORKING ] CMETER -- [ WORKING ]
### Configurator
Planning to design a web-base for configuring the quad settings and so on. Everything using Javascripts and HTML.
This will greatly reduce the need of the configurator to be downloaded. Purely configure by its own.
### REFERENCES ### REFERENCES

17
config.h
View File

@ -1,17 +0,0 @@
#ifndef CONFIG_H
#define CONFIG_H
/**************** 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 1
/*
* GPS configuration. For now only serial GPS that I have. Set GPS 1 if exits and GPS 0 if not exists
*/
#define GPS 1
#endif

1
platformio.ini
View File

@ -16,3 +16,4 @@ upload_speed = 460800
board_build.f_cpu = 240000000L board_build.f_cpu = 240000000L
board_build.f_flash = 40000000L board_build.f_flash = 40000000L
board_build.partitions = default_16MB.csv board_build.partitions = default_16MB.csv

46
src/config.h
View File

@ -0,0 +1,46 @@
#ifndef CONFIG_H
#define CONFIG_H
/**************** 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
/*
* GPS configuration. For now only serial GPS that I have. Set GPS 1 if exits and GPS 0 if not exists
*/
#define GPS 1
/*
* Altitude Hold
*
* Defines the neutral zone of throttle stick during altitude hold, default setting is
* +/-50 uncommend and change the value below if you want to change it.
*/
#define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 50
// #define ALT_HOLD_THROTTLE_MIDPOINT 1500 // in us - if uncommented, this value is used in ALT_HOLD for throttle stick middle point instead of initialThrottleHold parameter.
/*
* Battery voltage monitorin
*
* for V BAT monitoring
*/
//#define VBAT // uncomment this line to activate the vbat code
#define VBATSCALE 131 // (*) (**) change this value if readed Battery voltage is different than real voltage
#define VBATNOMINAL 126 // 12,6V full battery nominal voltage - only used for lcd.telemetry
#define VBATLEVEL_WARN1 107 // (*) (**) 10,7V
#define VBATLEVEL_WARN2 99 // (*) (**) 9.9V
#define VBATLEVEL_CRIT 93 // (*) (**) 9.3V - critical condition: if vbat ever goes below this value, permanent alarm is triggered
#define NO_VBAT 16 // Avoid beeping without any battery
#define VBAT_OFFSET 0 // offset in 0.1Volts, gets added to voltage value - useful for zener diodes
#endif

36
src/main.h
View File

@ -1,6 +1,8 @@
#ifndef MAIN_H #ifndef MAIN_H
#define MAIN_H #define MAIN_H
#include "config.h"
// #define WIFI_SSID "veeone" // #define WIFI_SSID "veeone"
// #define WIFI_PASSPHRASE "trustno1" // #define WIFI_PASSPHRASE "trustno1"
#define WIFI_SSID "EXPRESSFC" #define WIFI_SSID "EXPRESSFC"
@ -407,40 +409,6 @@ typedef struct {
#endif #endif
typedef struct {
// Don't forget to change the reply size in GUI when change this struct;
// on/off flags
// First byte
uint8_t filtering : 1;
uint8_t lead_filter : 1;
uint8_t dont_reset_home_at_arm : 1;
uint8_t nav_controls_heading : 1;
uint8_t nav_tail_first : 1;
uint8_t nav_rth_takeoff_heading : 1;
uint8_t slow_nav : 1;
uint8_t wait_for_rth_alt : 1;
// Second byte
uint8_t ignore_throttle: 1; // Disable stick controls during mission and RTH
uint8_t takeover_baro: 1;
uint16_t wp_radius; // in cm
uint16_t safe_wp_distance; // in meter
uint16_t nav_max_altitude; // in meter
uint16_t nav_speed_max; // in cm/s
uint16_t nav_speed_min; // in cm/s
uint8_t crosstrack_gain; // * 100 (0-2.56)
uint16_t nav_bank_max; // degree * 100; (3000 default)
uint16_t rth_altitude; // in meter
uint8_t land_speed; // between 50 and 255 (100 approx = 50cm/sec)
uint16_t fence; // fence control in meters
uint8_t max_wp_number;
uint8_t checksum;
} gps_conf_struct;
// Headers // Headers
#include "wifi.h" #include "wifi.h"
#include "dshot.h" #include "dshot.h"

220
src/task.cpp
View File

@ -92,8 +92,228 @@ void TASK::run(void) {
#endif #endif
// If throttle not ignored then allow change altitude with the stick // 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 #endif
********************/
writeMotors();
} }

19
src/task.h
View File

@ -38,6 +38,15 @@ class TASK {
int32_t prop = 0; int32_t prop = 0;
int16_t magHold, headFreeModeHold; // [-180; +180] 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 // begin GPS common variables and define
#if GPS #if GPS
@ -136,6 +145,16 @@ class TASK {
imu_t imu; imu_t imu;
conf_t conf; 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: private:
void process_rc(void); void process_rc(void);

Write Preview
Loading…
Cancel
Save