/branches/dongfang_FC_fixedwing/analog.c |
---|
163,12 → 163,12 |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_AIRPRESSURE, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_AIRPRESSURE, |
AD_UBAT, |
AD_GYRO_PITCH, |
175,11 → 175,11 |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_AIRPRESSURE, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW |
AD_GYRO_YAW, |
AD_AIRPRESSURE |
}; |
// Feature removed. Could be reintroduced later - but should work for all gyro types then. |
405,12 → 405,12 |
} |
void analog_calibrate(void) { |
#define OFFSET_CYCLES 250 |
#define OFFSET_CYCLES 120 |
uint8_t i, axis; |
int32_t offsets[4] = { 0, 0, 0, 0 }; |
gyro_calibrate(); |
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
// determine gyro bias by averaging (requires that the aircraft does not rotate around any axis!) |
for (i = 0; i < OFFSET_CYCLES; i++) { |
delay_ms_with_adc_measurement(2, 1); |
for (axis = PITCH; axis <= YAW; axis++) { |
420,7 → 420,7 |
} |
for (axis = PITCH; axis <= YAW; axis++) { |
gyroOffset.offsets[axis] = (offsets[axis] /*+ OFFSET_CYCLES/2 */) / OFFSET_CYCLES; |
gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES/2) / OFFSET_CYCLES; |
int16_t min = (512-200) * GYRO_OVERSAMPLING; |
int16_t max = (512+200) * GYRO_OVERSAMPLING; |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
427,7 → 427,7 |
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
} |
airpressureOffset = (offsets[3] /* + OFFSET_CYCLES/2 */) / OFFSET_CYCLES; |
airpressureOffset = (offsets[3] + OFFSET_CYCLES/2) / OFFSET_CYCLES; |
int16_t min = 200; |
int16_t max = 1024-200; |
if(airpressureOffset < min || airpressureOffset > max) |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
---|
153,7 → 153,7 |
/* Default Values for R/C Channels */ |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.trim = 0; |
channelMap.HWTrim = 0; |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
---|
111,7 → 111,7 |
*/ |
typedef struct { |
uint8_t trim; |
uint8_t HWTrim; |
uint8_t variableOffset; |
uint8_t channels[MAX_CHANNELS]; |
} ChannelMap_t; |
/branches/dongfang_FC_fixedwing/commands.c |
---|
26,6 → 26,10 |
analog_calibrate(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
RC_calibrate(); |
rcTrim_writeToEEProm(); |
beepNumber(1); |
} else if (command == COMMAND_CHMOD && !repeated) { |
configuration_setFlightParameters(argument); |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
10,6 → 10,7 |
int16_t variables[VARIABLE_COUNT]; |
ParamSet_t staticParams; |
ChannelMap_t channelMap; |
RCTrim_t rcTrim; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
153,7 → 154,7 |
staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S |
for (uint8_t i=0; i<3; i++) { |
staticParams.gyroPID[i].P = 80; |
staticParams.gyroPID[i].P = 200; |
staticParams.gyroPID[i].I = 80; |
staticParams.gyroPID[i].D = 40; |
staticParams.gyroPID[i].iMax = 30; |
204,7 → 205,7 |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.RCPolarity = 1; |
channelMap.trim = 192; |
channelMap.HWTrim = 192; |
channelMap.variableOffset = 128; |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
112,13 → 112,20 |
typedef struct { |
uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky. |
uint8_t trim; |
uint8_t HWTrim; |
uint8_t variableOffset; |
uint8_t channels[MAX_CHANNELS]; |
} ChannelMap_t; |
extern ChannelMap_t channelMap; |
// With fixed wing, we need some way to trim the plane. This is done during a trim flight without gyros activated, |
// and then save in a succeeding gyro calibration command. |
typedef struct { |
int16_t trim[MAX_CHANNELS]; |
} RCTrim_t; |
extern RCTrim_t rcTrim; |
typedef struct { |
int16_t offsets[3]; |
} sensorOffset_t; |
221,6 → 228,7 |
void IMUConfig_default(void); |
void channelMap_default(void); |
void rcTrim_setZero(void); |
void paramSet_default(uint8_t setnumber); |
void configuration_setFlightParameters(uint8_t newFlightMode); |
/branches/dongfang_FC_fixedwing/eeprom.c |
---|
144,7 → 144,7 |
} |
/***************************************************/ |
/* ChannelMap */ |
/* ChannelMap and R/C trim */ |
/***************************************************/ |
void channelMap_writeToEEProm(void) { |
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(ChannelMap_t)); |
155,10 → 155,23 |
printf("\n\rwriting default channel map"); |
channelMap_default(); |
channelMap_writeToEEProm(); |
// For whatever stupid reason, the newly defaulted channel map is not used! Reset. |
wdt_enable(WDTO_500MS); |
} |
} |
void rcTrim_writeToEEProm(void) { |
writeChecksummedBlock(0, (uint8_t*)&rcTrim, EEPROM_ADR_RCTRIM, sizeof(RCTrim_t)); |
} |
void rcTrim_readOrDefault(void) { |
if (readChecksummedBlock(0, (uint8_t*)&rcTrim, EEPROM_ADR_RCTRIM, sizeof(RCTrim_t))) { |
printf("\n\rwriting zero RC trim"); |
RC_setZeroTrim(); |
rcTrim_writeToEEProm(); |
} |
} |
/***************************************************/ |
/* Sensor offsets */ |
/***************************************************/ |
/branches/dongfang_FC_fixedwing/eeprom.h |
---|
20,7 → 20,8 |
#define EEPROM_ADR_GYROOFFSET 32 |
#define EEPROM_ADR_GYROAMPLIFIER 48 |
#define EEPROM_ADR_CHANNELMAP 64 |
#define EEPROM_ADR_IMU_CONFIG 100 |
#define EEPROM_ADR_RCTRIM 96 |
#define EEPROM_ADR_IMU_CONFIG 128 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
#define CHANNELMAP_REVISION 1 |
31,6 → 32,7 |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
void rcTrim_readOrDefault(void); |
void IMUConfig_readOrDefault(void); |
void IMUConfig_writeToEEprom(void); |
/branches/dongfang_FC_fixedwing/flight.c |
---|
112,10 → 112,17 |
term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
int32_t tmp; |
tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
if (reverse[PITCH]) target[PITCH] -= tmp; else target[PITCH] += tmp; |
tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
if (reverse[ROLL]) target[ROLL] -= tmp; else target[ROLL] += tmp; |
tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
if (reverse[YAW]) target[YAW] -= tmp; else target[YAW] += tmp; |
for (axis = PITCH; axis <= YAW; axis++) { |
if (target[axis] > OVER180) { |
target[axis] -= OVER360; |
123,10 → 130,10 |
target[axis] += OVER360; |
} |
if (reverse[axis]) |
error[axis] = target[axis] - attitude[axis]; |
else |
error[axis] = attitude[axis] - target[axis]; |
//if (reverse[axis]) |
error[axis] = attitude[axis] - target[axis]; |
// else |
// error[axis] = attitude[axis] - target[axis]; |
if (error[axis] > maxError[axis]) { |
error[axis] = maxError[axis]; |
141,10 → 148,10 |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
PDPart[axis] = (((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
+ (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
if (reverse[axis]) |
PDPart[axis] = -PDPart[axis]; |
//if (reverse[axis]) |
// PDPart[axis] = -PDPart[axis]; |
} else { |
PDPart[axis] = 0; |
} |
151,14 → 158,17 |
if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
if (reverse[axis]) |
PDPart[axis] += anglePart; |
else |
PDPart[axis] -= anglePart; |
// if (reverse[axis]) |
PDPart[axis] += anglePart; |
// else |
// PDPart[axis] -= anglePart; |
} |
// Add I parts here... these are integrated errors. |
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
if (reverse[axis]) |
term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
else |
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
} |
debugOut.analog[12] = term[PITCH]; |
/branches/dongfang_FC_fixedwing/main.c |
---|
34,7 → 34,7 |
// This is strange: It should NOT be necessarty to do. But the call of the same, |
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
channelMap_default(); |
// channelMap_default(); |
// initalize modules |
output_init(); |
48,6 → 48,7 |
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
rcTrim_readOrDefault(); |
paramSet_readOrDefault(); |
// enable interrupts global |
138,32 → 139,21 |
usart0_processRxData(); |
static uint8_t aboveWarningLimitVoltageSeen = 0; |
if (checkDelay(timer)) { |
if (UBat <= UBAT_AT_5V) { |
// Do nothing. The voltage on the input side of the regulator is <5V; |
// we must be running off USB power. Keep it quiet. |
} else if (UBat < staticParams.batteryWarningVoltage) { |
beepBatteryAlarm(); |
if (UBat >= staticParams.batteryWarningVoltage) { |
aboveWarningLimitVoltageSeen = 1; |
} else { // If we are above USB voltage, or if we have once been above warning voltage |
if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) { |
beepBatteryAlarm(); |
} |
} |
#ifdef USE_NAVICTRL |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
timer = setDelay(20); // every 20 ms |
} |
output_update(); |
} |
#ifdef USE_NAVICTRL |
if(!SendSPI) { |
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
// within the SPI_TransmitByte() routine the value is set to 4. |
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
SPI_TransmitByte(); |
} |
#endif |
calculateFeaturedServoValues(); |
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
/branches/dongfang_FC_fixedwing/rc.c |
---|
119,7 → 119,7 |
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) { |
// shift signal to zero symmetric range -154 to 159 |
//signal -= 3750; // theoretical value |
signal -= (TIME(1.5) - 128 + channelMap.trim); // best value with my Futaba in zero trim. |
signal -= (TIME(1.5) - 128 + channelMap.HWTrim); |
// check for stable signal |
if (abs(signal - PPM_in[index]) < TIME(0.05)) { |
if (RCQuality < 200) |
198,16 → 198,16 |
if (RCQuality) { |
RCQuality--; |
debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
debugOut.analog[21] = RCChannel(CH_AILERONS); |
debugOut.analog[22] = RCChannel(CH_RUDDER); |
debugOut.analog[23] = RCChannel(CH_THROTTLE); |
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR]; |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS]; |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER]; |
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle! |
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); |
} // if RCQuality is no good, we just do nothing. |
debugOut.analog[20] = PRYT[CONTROL_ELEVATOR]; |
debugOut.analog[21] = PRYT[CONTROL_AILERONS]; |
debugOut.analog[22] = PRYT[CONTROL_RUDDER]; |
debugOut.analog[23] = PRYT[CONTROL_THROTTLE]; |
} // if RCQuality is no good, we just do nothing. |
} |
/* |
235,9 → 235,18 |
} |
void RC_calibrate(void) { |
// Do nothing. |
rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR); |
rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS); |
rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER); |
rcTrim.trim[CH_THROTTLE] = 0; |
} |
int16_t RC_getZeroThrottle(void) { |
return TIME (-0.5); |
} |
void RC_setZeroTrim(void) { |
for (uint8_t i=0; i<MAX_CHANNELS; i++) { |
rcTrim.trim[i] = 0; |
} |
} |
/branches/dongfang_FC_fixedwing/rc.h |
---|
30,6 → 30,9 |
void RC_calibrate(void); |
uint8_t RC_getSignalQuality(void); |
int16_t RC_getZeroThrottle(void); |
void RC_setZeroTrim(void); |
#ifdef USE_MK3MAG |
uint8_t RC_testCompassCalState(void); |
#endif |
/branches/dongfang_FC_fixedwing/uart0.c |
---|
103,8 → 103,8 |
"airspeed ", |
"RC P ", //20 |
"RC R ", |
"RC T ", |
"RC Y ", |
"RC T ", |
"Servo P ", |
"Servo R ", //25 |
"Servo T ", |