Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2073 → Rev 2074

/branches/dongfang_FC_rewrite/configuration.h
181,14 → 181,19
uint8_t airpressureWindowLength; // 0 means: Use filter.
uint8_t airpressureDWindowLength; // values 1..5 are legal.
uint8_t airpressureAccZCorrection;
 
uint8_t heightP;
uint8_t heightI;
uint8_t heightD;
 
uint8_t heightSetting;
uint8_t heightControlMaxIntegralIn;
uint8_t heightControlMaxIntegralOut;
uint8_t heightControlMaxThrottleChange;
 
uint8_t heightControlTestOscPeriod;
uint8_t heightControlTestOscAmplitude;
 
// Servos
uint8_t servoCount;
uint8_t servoManualMaxSpeed;
/branches/dongfang_FC_rewrite/controlMixer.c
152,11 → 152,6
// This will init the values (not just add to them).
RC_periodicTaskAndPRTY(tempPRTY);
 
debugOut.analog[16] = tempPRTY[CONTROL_PITCH];
debugOut.analog[17] = tempPRTY[CONTROL_ROLL];
debugOut.analog[18] = tempPRTY[CONTROL_THROTTLE];
debugOut.analog[19] = tempPRTY[CONTROL_YAW];
 
// Add external control to RC
EC_periodicTaskAndPRTY(tempPRTY);
 
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
13,12 → 13,25
#include "attitude.h"
 
typedef enum {
GPS_FLIGHT_MODE_UNDEF,
GPS_FLIGHT_MODE_FREE,
GPS_FLIGHT_MODE_AID,
GPS_FLIGHT_MODE_HOME,
NAVI_FLIGHT_MODE_UNDEF,
NAVI_FLIGHT_MODE_FREE,
NAVI_FLIGHT_MODE_AID,
NAVI_FLIGHT_MODE_HOME,
} FlightMode_t;
 
typedef enum {
NAVI_STATUS_FREEFLIGHT=0,
NAVI_STATUS_INVALID_GPS=1,
NAVI_STATUS_BAD_GPS_SIGNAL=2,
NAVI_STATUS_MANUAL_OVERRIDE=5,
NAVI_STATUS_POSITION_HOLD=7,
NAVI_STATUS_RTH=8,
NAVI_STATUS_HOLD_POSITION_INVALID=9,
NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10,
NAVI_STATUS_RTH_POSITION_INVALID=11,
NAVI_STATUS_GPS_TIMEOUT=12
} NaviStatus_t;
 
#define GPS_POSINTEGRAL_LIMIT 32000
#define LOG_NAVI_STICK_GAIN 3
#define GPS_P_LIMIT 100
35,22 → 48,24
// GPS coordinates for home position
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
// the current flight mode
FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
int16_t naviSticks[2] = {0,0};
 
uint8_t naviStatus;
 
// ---------------------------------------------------------------------------------
void navi_updateFlightMode(void) {
static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
 
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
flightMode = GPS_FLIGHT_MODE_FREE;
flightMode = NAVI_FLIGHT_MODE_FREE;
} else {
if (dynamicParams.naviMode < 50)
flightMode = GPS_FLIGHT_MODE_FREE;
flightMode = NAVI_FLIGHT_MODE_FREE;
else if (dynamicParams.naviMode < 180)
flightMode = GPS_FLIGHT_MODE_AID;
flightMode = NAVI_FLIGHT_MODE_AID;
else
flightMode = GPS_FLIGHT_MODE_HOME;
flightMode = NAVI_FLIGHT_MODE_HOME;
}
 
if (flightMode != flightModeOld) {
260,7 → 275,7
if (MKFlags & MKFLAG_CALIBRATE) {
MKFlags &= ~(MKFLAG_CALIBRATE);
if (navi_writeCurrPositionTo(&homePosition)) {
// shift north to simulate an offset.
// shift north to simulate an offset.
// homePosition.latitude += 10000L;
beep(500);
}
269,7 → 284,8
switch (GPSInfo.status) {
case INVALID: // invalid gps data
navi_setNeutral();
if (flightMode != GPS_FLIGHT_MODE_FREE) {
naviStatus = NAVI_STATUS_INVALID_GPS;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
beep(1); // beep if signal is neccesary
}
break;
282,6 → 298,7
else {
navi_setNeutral();
GPSInfo.status = INVALID;
naviStatus = NAVI_STATUS_GPS_TIMEOUT;
}
break;
case NEWDATA: // new valid data from gps device
289,14 → 306,15
beep_rythm++;
if (navi_isGPSSignalOK()) {
switch (flightMode) { // check what's to do
case GPS_FLIGHT_MODE_FREE:
case NAVI_FLIGHT_MODE_FREE:
// update hold position to current gps position
navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
// disable gps control
navi_setNeutral();
naviStatus = NAVI_STATUS_FREEFLIGHT;
break;
 
case GPS_FLIGHT_MODE_AID:
case NAVI_FLIGHT_MODE_AID:
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
// update hold point to current gps position
304,6 → 322,7
// disable gps control
navi_setNeutral();
GPS_P_Delay = 0;
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
} else { // GPS control active
if (GPS_P_Delay < 7) {
// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
310,17 → 329,21
GPS_P_Delay++;
navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
navi_PIDController(NULL); // activates only the D-Part
} else
naviStatus = NAVI_STATUS_POSITION_HOLD;
} else {
navi_PIDController(&holdPosition); // activates the P&D-Part
naviStatus = NAVI_STATUS_POSITION_HOLD;
}
}
} else { // invalid Hold Position
// try to catch a valid hold position from gps data input
navi_writeCurrPositionTo(&holdPosition);
navi_setNeutral();
naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID;
}
break;
 
case GPS_FLIGHT_MODE_HOME:
case NAVI_FLIGHT_MODE_HOME:
if (homePosition.status != INVALID) {
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
330,21 → 353,24
} else {// GPS control active
navi_PIDController(&homePosition);
}
naviStatus = NAVI_STATUS_RTH;
} else {
// bad home position
beep(50); // signal invalid home position
// try to hold at least the position as a fallback option
 
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) {
// MK controlled by user
navi_setNeutral();
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
} else {
// GPS control active
navi_PIDController(&holdPosition);
naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD;
}
} else { // try to catch a valid hold position
navi_writeCurrPositionTo(&holdPosition);
naviStatus = NAVI_STATUS_RTH_POSITION_INVALID;
navi_setNeutral();
}
}
357,7 → 383,8
else { // gps data quality is bad
// disable gps control
navi_setNeutral();
if (flightMode != GPS_FLIGHT_MODE_FREE) {
naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
// beep if signal is not sufficient
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
beep(100);
373,4 → 400,10
 
PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
 
debugOut.analog[16] = flightMode;
debugOut.analog[17] = naviStatus;
 
debugOut.analog[18] = naviSticks[CONTROL_PITCH];
debugOut.analog[19] = naviSticks[CONTROL_ROLL];
}
/branches/dongfang_FC_rewrite/heightControl.c
14,10 → 14,12
 
#define LATCH_TIME 3
 
int32_t setHeight;
int32_t targetHeight;
//int32_t rampedTargetHeight;
 
//rampinguint8_t heightRampingTimer = 0;
uint16_t testOscPrescaler = 0;
uint8_t testOscTimer = 0;
 
int32_t maxHeightThisFlight;
int32_t iHeight;
 
24,7 → 26,8
void HC_setGround(void) {
analog_setGround();
// This should also happen when height control is enabled in-flight.
/*rampedTargetHeight = */ targetHeight = analog_getHeight();
setHeight = targetHeight = analog_getHeight();
testOscTimer = 0;
maxHeightThisFlight = 0;
iHeight = 0;
}
48,7 → 51,8
if (setHeightLatch <= LATCH_TIME) {
if (setHeightLatch == LATCH_TIME) {
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
/* rampedTargetHeight = */ targetHeight = height;
setHeight = height;
testOscTimer = 0;
iHeight = 0;
}
// Time not yet reached.
60,7 → 64,7
}
} else {
// Switch is not activated; take the "max-height" as the target height.
targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it)
setHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it)
}
/*
83,7 → 87,27
}
}
*/
// uint8_t heightControlTestOscPeriod;
// uint8_t heightControlTestOscAmplitude;
 
testOscPrescaler++;
if (testOscPrescaler == 488) {
testOscPrescaler = 0;
testOscTimer++;
if (testOscTimer == staticParams.heightControlTestOscPeriod) {
testOscTimer = 0;
iHeight = 0;
} else if (testOscTimer == staticParams.heightControlTestOscPeriod/2) {
iHeight = 0;
}
}
 
if (testOscTimer < staticParams.heightControlTestOscPeriod/2)
targetHeight = setHeight;
else
targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * 25L;
 
//if (staticParams.)
// height, in meters (so the division factor is: 100)
// debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
// Calculated 0 alt number: 108205
90,6 → 114,10
// Experimental 0 alt number: 117100
}
 
#define LOG_PHEIGHT_SCALE 10
#define LOG_IHEIGHT_SCALE 24
#define LOG_DHEIGHT_SCALE 6
 
// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
115,7 → 143,7
}
 
int32_t heightErrorForIntegral = heightError;
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10;
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE;
 
if (heightErrorForIntegral > heightErrorForIntegralLimit) {
heightErrorForIntegral = heightErrorForIntegralLimit;
138,8 → 166,8
iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
}
 
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;
 
debugOut.analog[10] = dThrottleP;
debugOut.analog[11] = dThrottleI;
/branches/dongfang_FC_rewrite/makefile
273,7 → 273,7
AVRDUDE_RESET = -u -U lfuse:r:m
 
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) -B 1
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) -B 5
 
# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
/branches/dongfang_FC_rewrite/uart0.c
104,10 → 104,10
"heightErr/10 ",
"controlActivity ",
"accVector ", //15
"ppm 0 ",
"ppm 1 ",
"ppm 2 ",
"ppm 3 ",
"NaviMode ",
"NaviStatus ",
"NaviStickP ",
"NaviStickR ",
"control act wghd", //20
"acc vector wghd ",
"Height[dm] ",