/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
1,4 → 1,4 |
//#include "ENC-03_FC1.3.h" |
#include "sensors.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "twimaster.h" |
11,17 → 11,16 |
const uint8_t GYRO_REVERSED[3] = { 0, 0, 1 }; |
const uint8_t ACC_REVERSED[3] = { 0, 1, 0 }; |
// void gyro_init(void) {} |
void gyro_calibrate(void) { |
void gyro_calibrate(void) { |
printf("gyro_calibrate"); |
uint8_t i, axis, factor, numberOfAxesInRange = 0; |
uint16_t timeout; |
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
timeout = setDelay(2000); |
for (i = 140; i != 0; i--) { |
// If all 3 axis are in range, shorten the remaining number of iterations. |
if (numberOfAxesInRange == 3 && i > 10) |
i = 10; |
numberOfAxesInRange = 0; |
for (axis = PITCH; axis <= YAW; axis++) { |
31,34 → 30,46 |
factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
if (rawGyroSum[axis] < 510 * factor) |
DACValues.offsets[axis]--; |
gyroAmplifierOffset.offsets[axis]--; |
else if (rawGyroSum[axis] > 515 * factor) |
DACValues.offsets[axis]++; |
gyroAmplifierOffset.offsets[axis]++; |
else |
numberOfAxesInRange++; |
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
if (DACValues.offsets[axis] < 10) { |
DACValues.offsets[axis] = 10; |
} else if (DACValues.offsets[axis] > 245) { |
DACValues.offsets[axis] = 245; |
if (gyroAmplifierOffset.offsets[axis] < 10) { |
gyroAmplifierOffset.offsets[axis] = 10; |
} else if (gyroAmplifierOffset.offsets[axis] > 245) { |
gyroAmplifierOffset.offsets[axis] = 245; |
} |
} |
gyro_loadOffsets(0); |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
delay_ms_with_adc_measurement(i <= 10 ? 10 : 2); |
} |
gyroAmplifierOffset_writeToEEProm(); |
delay_ms_with_adc_measurement(70); |
} |
// Wait for I2C to finish transmission. |
while (twi_state) { |
// Did it take too long? |
if (checkDelay(timeout)) { |
printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
break; |
} |
} |
void gyro_loadOffsets(uint8_t overwriteWithDefaults) { |
uint16_t timeout = setDelay(2000); |
delay_ms_Mess(i <= 10 ? 10 : 2); |
if (overwriteWithDefaults) { |
gyroAmplifierOffset.offsets[PITCH] = |
gyroAmplifierOffset.offsets[ROLL] = |
gyroAmplifierOffset.offsets[YAW] = (uint8_t)(255 * 1.2089 / 3.0); |
} |
delay_ms_Mess(70); |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
// Wait for I2C to finish transmission. |
while (twi_state) { |
// Did it take too long? |
if (checkDelay(timeout)) { |
printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
break; |
} |
} |
} |
void gyro_setDefaults(void) { |
/branches/dongfang_FC_rewrite/analog.c |
---|
105,7 → 105,7 |
volatile sensorOffset_t gyroOffset; |
volatile sensorOffset_t accOffset; |
volatile sensorOffset_t DACValues; |
volatile sensorOffset_t gyroAmplifierOffset; |
/* |
* This allows some experimentation with the gyro filters. |
281,6 → 281,7 |
for (uint8_t axis=0; axis<2; axis++) { |
tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
/* |
* Process the gyro data for the PID controller. |
*/ |
342,9 → 343,9 |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
debugOut.analog[3] = gyro_ATT[PITCH]; |
debugOut.analog[4] = gyro_ATT[ROLL]; |
debugOut.analog[5] = yawGyro; |
debugOut.analog[3] = rawGyroSum[0]; |
debugOut.analog[4] = rawGyroSum[1]; |
debugOut.analog[5] = rawGyroSum[2]; |
} |
void analog_updateAccelerometers(void) { |
471,6 → 472,11 |
} |
void analog_setNeutral() { |
if (gyroAmplifierOffset_readFromEEProm()) { |
printf("gyro amp invalid, you must recalibrate."); |
gyro_loadOffsets(1); |
} |
if (gyroOffset_readFromEEProm()) { |
printf("gyro offsets invalid, you must recalibrate."); |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
492,7 → 498,7 |
// Setting offset values has an influence in the analog.c ISR |
// Therefore run measurement for 100ms to achive stable readings |
delay_ms_Mess(100); |
delay_ms_with_adc_measurement(100); |
// Rough estimate. Hmm no nothing happens at calibration anyway. |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
507,7 → 513,7 |
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
delay_ms_Mess(20); |
delay_ms_with_adc_measurement(20); |
for (axis = PITCH; axis <= YAW; axis++) { |
offsets[axis] += rawGyroSum[axis]; |
} |
534,7 → 540,7 |
int16_t filteredDelta; |
for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
delay_ms_Mess(10); |
delay_ms_with_adc_measurement(10); |
for (axis = PITCH; axis <= YAW; axis++) { |
deltaOffset[axis] += acc[axis]; |
} |
/branches/dongfang_FC_rewrite/analog.h |
---|
187,7 → 187,7 |
extern volatile sensorOffset_t gyroOffset; |
extern volatile sensorOffset_t accOffset; |
extern volatile sensorOffset_t DACValues; |
extern volatile sensorOffset_t gyroAmplifierOffset; |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
88,17 → 88,21 |
/***************************************************/ |
/* Read Parameter from EEPROM as word */ |
/***************************************************/ |
/* |
uint16_t getParamWord(uint16_t param_id) { |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN |
+ param_id]); |
} |
*/ |
/***************************************************/ |
/* Write Parameter to EEPROM as word */ |
/***************************************************/ |
/* |
void setParamWord(uint16_t param_id, uint16_t value) { |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
*/ |
uint8_t calculateChecksum(uint8_t* data, uint16_t length) { |
uint8_t result = 0; |
223,14 → 227,20 |
/***************************************************/ |
/* Sensor offsets */ |
/***************************************************/ |
uint8_t gyroAmplifierOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroAmplifierOffset, EEPROM_ADR_GYROAMPLIFIER, sizeof(sensorOffset_t)); |
} |
void gyroAmplifierOffset_writeToEEProm(void) { |
return writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroAmplifierOffset, EEPROM_ADR_GYROAMPLIFIER, sizeof(sensorOffset_t)); |
} |
uint8_t gyroOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)) | |
readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&DACValues, EEPROM_ADR_DACVALUES, sizeof(sensorOffset_t)); |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
void gyroOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&DACValues, EEPROM_ADR_DACVALUES, sizeof(sensorOffset_t)); |
} |
uint8_t accOffset_readFromEEProm(void) { |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
8,43 → 8,53 |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define PID_ACTIVE_SET 0 // byte |
#define EEPROM_ADR_ACCOFFSET 1 |
#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+2) |
#define EEPROM_ADR_DACVALUES (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+2) |
#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+2) |
#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(channelMap_t)+2) |
#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_CHANNELMAP+sizeof(mixerMatrix_t)+2) |
//#define EEPROM_ADR_ACCOFFSET 1 |
//#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+2) |
//#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+2) |
//#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+2) |
//#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(channelMap_t)+2) |
//#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+2) |
#define EEPROM_ADR_ACCOFFSET 10 |
#define EEPROM_ADR_GYROOFFSET 20 |
#define EEPROM_ADR_GYROAMPLIFIER 30 |
#define EEPROM_ADR_CHANNELMAP 40 |
#define EEPROM_ADR_MIXER_TABLE 60 |
#define EEPROM_ADR_PARAMSET_BEGIN 200 |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 0 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
extern void paramSet_readOrDefault(void); |
extern void channelMap_readOrDefault(void); |
extern void mixerMatrix_readOrDefault(void); |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
void mixerMatrix_readOrDefault(void); |
extern uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
extern void paramSet_writeToEEProm(uint8_t setnumber); |
uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
void paramSet_writeToEEProm(uint8_t setnumber); |
extern uint8_t channelMap_readFromEEProm(void); |
extern void channelMap_writeToEEProm(void); |
uint8_t channelMap_readFromEEProm(void); |
void channelMap_writeToEEProm(void); |
extern uint8_t mixerMatrix_eeadFromEEProm(void); |
extern void mixerMatrix_writeToEEProm(void); |
uint8_t mixerMatrix_eeadFromEEProm(void); |
void mixerMatrix_writeToEEProm(void); |
extern uint8_t accOffset_readFromEEProm(void); |
extern void accOffset_writeToEEProm(void); |
uint8_t gyroAmplifierOffset_readFromEEProm(void); |
void gyroAmplifierOffset_writeToEEProm(void); |
extern uint8_t gyroOffset_readFromEEProm(void); |
extern void gyroOffset_writeToEEProm(void); |
uint8_t gyroOffset_readFromEEProm(void); |
void gyroOffset_writeToEEProm(void); |
extern uint8_t getParamByte(uint16_t param_id); |
extern void setParamByte(uint16_t param_id, uint8_t value); |
extern uint16_t getParamWord(uint16_t param_id); |
extern void setParamWord(uint16_t param_id, uint16_t value); |
uint8_t accOffset_readFromEEProm(void); |
void accOffset_writeToEEProm(void); |
extern uint8_t getActiveParamSet(void); |
extern void setActiveParamSet(uint8_t setnumber); |
uint8_t getParamByte(uint16_t param_id); |
void setParamByte(uint16_t param_id, uint8_t value); |
//uint16_t getParamWord(uint16_t param_id); |
//void setParamWord(uint16_t param_id, uint16_t value); |
uint8_t getActiveParamSet(void); |
void setActiveParamSet(uint8_t setnumber); |
#endif //_EEPROM_H |
/branches/dongfang_FC_rewrite/invenSense.c |
---|
29,7 → 29,7 |
delay_ms(1); |
AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
// Delay_ms(10); |
delay_ms_Mess(100); |
delay_ms_with_adc_measurement(100); |
} |
void gyro_setDefaults(void) { |
/branches/dongfang_FC_rewrite/makefile |
---|
4,8 → 4,8 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 74 |
VERSION_PATCH = 100 |
VERSION_MINOR = 1 |
VERSION_PATCH = 0 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
/branches/dongfang_FC_rewrite/sensors.h |
---|
2,6 → 2,7 |
#define _SENSORS_H |
#include <inttypes.h> |
#include "configuration.h" |
/* |
* Whether (pitch, roll, yaw) gyros are reversed (see analog.h). |
12,9 → 13,8 |
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h). |
*/ |
extern const uint8_t ACC_REVERSED[3]; |
extern volatile sensorOffset_t gyroAmplifierOffset; |
extern volatile sensorOffset_t DACValues; |
/* |
* Common procedures for all gyro types. |
* FC 1.3 hardware: Searching the DAC values that return neutral readings. |
24,6 → 24,11 |
void gyro_calibrate(void); |
/* |
* FC 1.3: Output data in gyroAmplifierOffset to DAC. All other versions: Do nothing. |
*/ |
void gyro_loadOffsets(uint8_t overwriteWithDefaults); |
/* |
* Set some default FC parameters, depending on gyro type: Drift correction etc. |
*/ |
void gyro_setDefaults(void); |
/branches/dongfang_FC_rewrite/timer0.c |
---|
213,7 → 213,7 |
} |
// ----------------------------------------------------------------------- |
void delay_ms_Mess(uint16_t w) { |
void delay_ms_with_adc_measurement(uint16_t w) { |
uint16_t t_stop; |
t_stop = setDelay(w); |
while (!checkDelay(t_stop)) { |
/branches/dongfang_FC_rewrite/timer0.h |
---|
20,7 → 20,7 |
extern void timer0_init(void); |
extern void delay_ms(uint16_t w); |
extern void delay_ms_Mess(uint16_t w); |
extern void delay_ms_with_adc_measurement(uint16_t w); |
extern uint16_t setDelay(uint16_t t); |
extern int8_t checkDelay(uint16_t t); |
/branches/dongfang_FC_rewrite/twimaster.c |
---|
259,7 → 259,7 |
break; |
case 9: |
I2C_WriteByte(DACValues.offsets[DACChannel]); |
I2C_WriteByte(gyroAmplifierOffset.offsets[DACChannel]); |
break; |
case 10: |
/branches/dongfang_FC_rewrite/uart0.c |
---|
66,7 → 66,6 |
#include "output.h" |
#include "attitude.h" |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |