Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 09:16:07 +03:00
Code Issues Wiki Activity

trashed uvop keil file since that was system-specific stuff.

Browse source 
applied mwc-dev GPS code with bits of tarducopter code by sbaron. Thanks again. Moved some of the GPS config stuff into cli - gps_lpf, min/max nav speed, nav_controls_heading. Remember I don't test any GPS functionality at all, so if this makes your quad fly towards North Korea at over 9000cm/sec, this is NOT my problem.
spacing fixes in a couple files.
trashed old serial code that was under #if 0


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@161 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-06-06 15:09:44 +00:00
parent 12d6c41407
commit 19ca85963b
9 changed files with 6546 additions and 6853 deletions
Download patch file Download diff file Expand all files Collapse all files

117
src/mw.c
View file

@ -3,12 +3,9 @@
// March 2012 V2.0
#define CHECKBOXITEMS 11
#define PIDITEMS 8
int16_t debug1, debug2, debug3, debug4;
uint8_t buzzerState = 0;
uint8_t toggleBeep = 0;
uint8_t toggleBeep = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
@ -24,11 +21,11 @@ uint8_t vbat; // battery voltage in 0.1V steps
int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0;
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
uint8_t dynP8[3], dynI8[3], dynD8[3];
uint8_t rcOptions[CHECKBOXITEMS];
@ -42,19 +39,21 @@ int16_t axisPID[3];
// **********************
// GPS
// **********************
int32_t GPS_latitude, GPS_longitude;
int32_t GPS_latitude_home, GPS_longitude_home;
int32_t GPS_latitude_hold, GPS_longitude_hold;
int32_t GPS_coord[2];
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint8_t GPS_fix, GPS_fix_home = 0;
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // in meters
int16_t GPS_directionToHome = 0; // in degrees
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees*10
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees*10
uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2];
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
//Automatic ACC Offset Calibration
// **********************
@ -89,7 +88,7 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp,tmp2;
uint16_t tmp, tmp2;
static uint8_t vbatTimer = 0;
static uint8_t buzzerFreq; //delay between buzzer ring
uint8_t axis, prop1, prop2;
@ -103,7 +102,7 @@ void annexCode(void)
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
} else {
prop2 = 100 - cfg.dynThrPID;
}
@ -119,11 +118,11 @@ void annexCode(void)
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
@ -133,18 +132,18 @@ void annexCode(void)
}
}
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t)cfg.yawRate * tmp / 500;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000);
tmp = (uint32_t)(tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
// rcCommand[THROTTLE] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck);
if (headFreeMode) {
@ -165,7 +164,7 @@ void annexCode(void)
}
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
buzzerFreq = 7;
} else if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok, buzzer off
} else if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok, buzzer off
buzzerFreq = 0;
buzzerState = 0;
BEEP_OFF;
@ -173,7 +172,7 @@ void annexCode(void)
buzzerFreq = 4; // low battery
}
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
@ -209,7 +208,7 @@ void annexCode(void)
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) {
if (currentTime > GPSLEDTime && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
@ -276,16 +275,16 @@ void loop(void)
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
computeRC();
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 1) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = cfg.failsafe_throttle;
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
armed = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
okToArm = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
armed = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
okToArm = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
@ -326,7 +325,7 @@ void loop(void)
} else if (armed)
armed = 0;
rcDelayCommand = 0;
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && armed == 1 ) {
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && armed == 1) {
if (rcDelayCommand == 20)
armed = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ((rcData[YAW] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
@ -396,11 +395,8 @@ void loop(void)
}
for (i = 0; i < CHECKBOXITEMS; i++) {
rcOptions[i] = (
((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) <<1 | (rcData[AUX1] > 1700) << 2
| (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5
| (rcData[AUX3] < 1300) << 6 | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 7 | (rcData[AUX3] > 1700) << 8
| (rcData[AUX4] < 1300) << 9 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 10| (rcData[AUX4] > 1700) << 11) & cfg.activate[i]) > 0;
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2
| (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5 | (rcData[AUX3] < 1300) << 6 | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 7 | (rcData[AUX3] > 1700) << 8 | (rcData[AUX4] < 1300) << 9 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 10 | (rcData[AUX4] > 1700) << 11) & cfg.activate[i]) > 0;
}
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
@ -457,14 +453,20 @@ void loop(void)
if (sensors(SENSOR_GPS)) {
if (rcOptions[BOXGPSHOME]) {
GPSModeHome = 1;
if (GPSModeHome == 0) {
GPSModeHome = 1;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else
GPSModeHome = 0;
if (rcOptions[BOXGPSHOLD]) {
if (GPSModeHold == 0) {
GPSModeHold = 1;
GPS_latitude_hold = GPS_latitude;
GPS_longitude_hold = GPS_longitude;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
GPSModeHold = 0;
@ -545,44 +547,33 @@ void loop(void)
#endif
if (sensors(SENSOR_GPS)) {
uint16_t GPS_dist = 0;
int16_t GPS_dir = 0;
// Check that we really need to navigate ?
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
// If not. Reset nav loops and all nav related parameters
GPS_reset_nav();
} else {
float radDiff;
if (GPSModeHome == 1) {
GPS_dist = GPS_distanceToHome;
GPS_dir = GPS_directionToHome;
}
if (GPSModeHold == 1) {
GPS_dist = GPS_distanceToHold;
GPS_dir = GPS_directionToHold;
}
radDiff = (GPS_dir - heading) * M_PI / 180.0f;
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sinf(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cosf(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(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;
}
}
// **** PITCH & ROLL & YAW PID ****
for (axis = 0; axis < 3; axis++) {
if (accMode == 1 && axis < 2) { // LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTerm = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTerm = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTerm = (int32_t) errorAngle *cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp // 16 bits is ok here
ITerm = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
} else { // ACRO MODE or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTerm = rcCommand[axis];
@ -592,7 +583,7 @@ void loop(void)
errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
@ -600,7 +591,7 @@ void loop(void)
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}