Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1959 → Rev 1960

/branches/dongfang_FC_rewrite/eeprom.c
63,188 → 63,62
#define EEMEM __attribute__ ((section (".eeprom")))
#endif
 
#include <avr/eeprom.h>
#include <string.h>
#include "eeprom.h"
#include "printf_P.h"
#include "output.h"
// TODO: Get rid of these. They have nothing to do with eeprom.
#include "flight.h"
#include "rc.h"
#include "sensors.h"
#include <avr/eeprom.h>
 
// byte array in eeprom
uint8_t EEPromArray[E2END + 1] EEMEM;
 
paramset_t staticParams;
MixerTable_t Mixer;
 
/*
* Default for your own experiments here, so you don't have to reset them
* from MK-Tool all the time.
*/
void setDefaultUserParams(void) {
uint8_t i;
for (i = 0; i < sizeof(staticParams.UserParams1); i++) {
staticParams.UserParams1[i] = 0;
}
for (i = 0; i < sizeof(staticParams.UserParams2); i++) {
staticParams.UserParams2[i] = 0;
}
/*
* While we are still using userparams for flight parameters, do set
* some safe & meaningful default values.
*/
staticParams.UserParams1[3] = 12; // Throttle stick D=12
staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4
staticParams.UserParams2[1] = 2; // H&I motor smoothing.
staticParams.UserParams2[2] = 120; // Yaw I factor
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles.
}
 
void setOtherDefaults(void) {
/* Channel assignments were changed to the normal:
* Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4
*/
staticParams.ChannelAssignment[CH_PITCH] = 2;
staticParams.ChannelAssignment[CH_ROLL] = 1;
staticParams.ChannelAssignment[CH_THROTTLE] = 3;
staticParams.ChannelAssignment[CH_YAW] = 4;
staticParams.ChannelAssignment[CH_POTS + 0] = 5;
staticParams.ChannelAssignment[CH_POTS + 1] = 6;
staticParams.ChannelAssignment[CH_POTS + 2] = 7;
staticParams.ChannelAssignment[CH_POTS + 3] = 8;
staticParams.GlobalConfig = /* CFG_AXIS_COUPLING_ACTIVE | */ CFG_HEADING_HOLD; // CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;
staticParams.HeightMinGas = 30;
staticParams.MaxHeight = 251;
staticParams.HeightP = 10;
staticParams.HeightD = 30;
staticParams.Height_ACC_Effect = 30;
staticParams.Height_Gain = 4;
staticParams.StickP = 8;
staticParams.StickD = 12;
staticParams.StickYawP = 12;
staticParams.MinThrottle = 8;
staticParams.MaxThrottle = 230;
staticParams.CompassYawEffect = 128;
staticParams.GyroP = 80;
staticParams.GyroI = 100;
staticParams.LowVoltageWarning = 101; // 3.7 each for 3S
staticParams.EmergencyGas = 35;
staticParams.EmergencyGasDuration = 30;
staticParams.Unused0 = 0;
staticParams.IFactor = 32;
staticParams.ServoPitchControl = 100;
staticParams.ServoPitchComp = 40;
staticParams.ServoPitchCompInvert = 0;
staticParams.ServoPitchMin = 50;
staticParams.ServoPitchMax = 150;
staticParams.ServoRefresh = 5;
staticParams.LoopGasLimit = 50;
staticParams.LoopThreshold = 90;
staticParams.LoopHysteresis = 50;
staticParams.BitConfig = 0;
staticParams.AxisCoupling1 = 90;
staticParams.AxisCoupling2 = 67;
staticParams.AxisCouplingYawCorrection = 0;
staticParams.DynamicStability = 50;
staticParams.J16Bitmask = 95;
staticParams.J17Bitmask = 243;
staticParams.J16Timing = 15;
staticParams.J17Timing = 15;
staticParams.NaviGpsModeControl = 253;
staticParams.NaviGpsGain = 100;
staticParams.NaviGpsP = 90;
staticParams.NaviGpsI = 90;
staticParams.NaviGpsD = 90;
staticParams.NaviGpsPLimit = 75;
staticParams.NaviGpsILimit = 75;
staticParams.NaviGpsDLimit = 75;
staticParams.NaviGpsACC = 0;
staticParams.NaviGpsMinSat = 6;
staticParams.NaviStickThreshold = 8;
staticParams.NaviWindCorrection = 90;
staticParams.NaviSpeedCompensation = 30;
staticParams.NaviOperatingRadius = 100;
staticParams.NaviAngleLimitation = 100;
staticParams.NaviPHLoginTime = 4;
}
 
/***************************************************/
/* Default Values for parameter set 1 */
/* Read Parameter from EEPROM as byte */
/***************************************************/
void ParamSet_DefaultSet1(void) { // sport
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER
/* | CFG_AXIS_COUPLING_ACTIVE*/;
staticParams.EmergencyGasDuration = 200;
staticParams.J16Timing = 10;
staticParams.J17Timing = 10;
memcpy(staticParams.Name, "Sport\0", 6);
uint8_t getParamByte(uint16_t param_id) {
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}
 
/***************************************************/
/* Default Values for parameter set 2 */
/* Write Parameter to EEPROM as byte */
/***************************************************/
void ParamSet_DefaultSet2(void) { // normal
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER
/* | CFG_AXIS_COUPLING_ACTIVE*/ ;
staticParams.Height_Gain = 3;
staticParams.EmergencyGasDuration = 200;
staticParams.J16Timing = 20;
staticParams.J17Timing = 20;
memcpy(staticParams.Name, "Normal\0", 7);
void setParamByte(uint16_t param_id, uint8_t value) {
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
 
/***************************************************/
/* Default Values for parameter set 3 */
/* Read Parameter from EEPROM as word */
/***************************************************/
void ParamSet_DefaultSet3(void) { // beginner
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER
/* | CFG_AXIS_COUPLING_ACTIVE */;
staticParams.Height_Gain = 3;
staticParams.EmergencyGasDuration = 200;
staticParams.J16Timing = 30;
staticParams.J17Timing = 30;
memcpy(staticParams.Name, "Beginner\0", 9);
uint16_t getParamWord(uint16_t param_id) {
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN
+ param_id]);
}
 
/***************************************************/
/* Read Parameter from EEPROM as byte */
/* Write Parameter to EEPROM as word */
/***************************************************/
uint8_t GetParamByte(uint16_t param_id) {
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
void setParamWord(uint16_t param_id, uint16_t value) {
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
 
/***************************************************/
/* Write Parameter to EEPROM as byte */
/***************************************************/
void SetParamByte(uint16_t param_id, uint8_t value) {
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
uint8_t calculateChecksum(uint8_t* data, uint16_t length) {
uint8_t result = 0;
for (uint16_t i=0; i<length; i++) {
result += data[i];
}
return result;
}
 
/***************************************************/
/* 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]);
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t length, uint16_t offset) {
uint8_t checksum = calculateChecksum(data+1, length-1);
data[0] = checksum;
data[1] = revisionNumber;
eeprom_write_block(data, &EEPromArray[offset], length);
}
 
/***************************************************/
/* 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 readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t length, uint16_t offset) {
eeprom_read_block(target, &EEPromArray[offset], length);
uint8_t checksum = calculateChecksum(target+1, length-1);
return (checksum != target[0] || revisionNumber != target[1]);
}
 
/***************************************************/
251,13 → 125,10
/* Read Parameter Set from EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_ReadFromEEProm(uint8_t setnumber) {
if ((1 > setnumber) || (setnumber > 5))
setnumber = 3;
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
output_init();
uint8_t paramSet_readFromEEProm(uint8_t setnumber) {
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*sizeof(paramset_t);
output_init(); // what's that doing here??
return readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, sizeof(paramset_t), offset);
}
 
/***************************************************/
264,159 → 135,104
/* Write Parameter Set to EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_WriteToEEProm(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
return;
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH],
PARAMSET_STRUCT_LEN);
eeprom_write_block(&staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init();
void paramSet_writeToEEProm(uint8_t setnumber) {
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*sizeof(paramset_t);
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, sizeof(paramset_t), offset);
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init(); // what's that doing here??
}
 
/***************************************************/
/* Get active parameter set */
/***************************************************/
uint8_t getActiveParamSet(void) {
uint8_t setnumber;
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]);
if (setnumber > 5) {
setnumber = 3;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
return (setnumber);
void paramSet_readOrDefault() {
uint8_t setnumber = getActiveParamSet();
// parameter version check
if (setnumber<1 ||setnumber>5 || paramSet_readFromEEProm(setnumber)) {
// if version check faild
printf("\n\rInit Parameter in EEPROM");
for (uint8_t i=6; i>0; i--) {
paramSet_default(i); // Fill staticParams Structure to default parameter set 1 (Sport)
paramSet_writeToEEProm(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
}
printf("\n\rUsing Parameter Set %d", getActiveParamSet());
}
 
/***************************************************/
/* Set active parameter set */
/* MixerTable */
/***************************************************/
void setActiveParamSet(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
setnumber = 1;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
uint8_t mixerMatrix_readFromEEProm(void) {
return readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t));
}
 
/***************************************************/
/* Read MixerTable from EEPROM */
/***************************************************/
uint8_t MixerTable_ReadFromEEProm(void) {
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE])
== EEMIXER_REVISION) {
eeprom_read_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE],
sizeof(Mixer));
return 1;
} else
return 0;
void mixerMatrix_writeToEEProm(void) {
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t));
}
 
void mixerMatrix_readOrDefault(void) {
// load mixer table
if (mixerMatrix_readFromEEProm()) {
printf("\n\rGenerating default mixerMatrix");
mixerMatrix_default(); // Quadro
mixerMatrix_writeToEEProm();
}
// determine motornumber
requiredMotors = 0;
for (uint8_t i=0; i<MAX_MOTORS; i++) {
if (mixerMatrix.motor[i][MIX_THROTTLE])
requiredMotors++;
}
printf("\n\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors);
printf("\n\r==============================");
}
 
/***************************************************/
/* Write Mixer Table to EEPROM */
/* ChannelMap */
/***************************************************/
uint8_t MixerTable_WriteToEEProm(void) {
if (Mixer.Revision == EEMIXER_REVISION) {
eeprom_write_block((uint8_t *) &Mixer,
&EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
return 1;
} else
return 0;
uint8_t channelMap_readFromEEProm(void) {
return readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t));
}
 
void channelMap_writeToEEProm(void) {
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t));
}
 
void channelMap_readOrDefault(void) {
if (!channelMap_readFromEEProm()) {
printf("\n\rGenerating default channel map");
channelMap_default();
channelMap_writeToEEProm();
}
}
 
uint8_t accOffset_readFromEEProm(void) {
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
}
 
void accOffset_writeToEEProm(void) {
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t));
}
 
 
/***************************************************/
/* Default Values for Mixer Table */
/* Get active parameter set */
/***************************************************/
void MixerTable_Default(void) { // Quadro
uint8_t i;
Mixer.Revision = EEMIXER_REVISION;
// clear mixer table (but preset throttle)
for (i = 0; i < 16; i++) {
Mixer.Motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
Mixer.Motor[i][MIX_PITCH] = 0;
Mixer.Motor[i][MIX_ROLL] = 0;
Mixer.Motor[i][MIX_YAW] = 0;
}
// default = Quadro
Mixer.Motor[0][MIX_PITCH] = +64;
Mixer.Motor[0][MIX_YAW] = +64;
Mixer.Motor[1][MIX_PITCH] = -64;
Mixer.Motor[1][MIX_YAW] = +64;
Mixer.Motor[2][MIX_ROLL] = -64;
Mixer.Motor[2][MIX_YAW] = -64;
Mixer.Motor[3][MIX_ROLL] = +64;
Mixer.Motor[3][MIX_YAW] = -64;
memcpy(Mixer.Name, "Quadro\0", 7);
uint8_t getActiveParamSet(void) {
uint8_t setnumber;
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]);
if (setnumber > 4) {
setActiveParamSet(setnumber = 0);
}
return setnumber;
}
 
/***************************************************/
/* Initialize EEPROM Parameter Sets */
/* Set active parameter set */
/***************************************************/
void ParamSet_Init(void) {
uint8_t Channel_Backup = 1, i, j;
// parameter version check
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) {
// if version check faild
printf("\n\rInit Parameter in EEPROM");
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table
// check if channel mapping backup is valid
for (j = 0; j < 4 && Channel_Backup; j++) {
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS + 0]) >= 12)
Channel_Backup = 0;
}
// fill all 5 parameter settings
for (i = 1; i < 6; i++) {
switch (i) {
case 1:
ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport)
break;
case 2:
ParamSet_DefaultSet2(); // Kamera
break;
case 3:
ParamSet_DefaultSet3(); // Beginner
break;
default:
ParamSet_DefaultSet2(); // Kamera
break;
}
if (Channel_Backup) { // if we have a rc channel mapping backup in eeprom
// restore it
for (j = 0; j < 8; j++) {
staticParams.ChannelAssignment[j] = eeprom_read_byte(
&EEPromArray[EEPROM_ADR_CHANNELS + j]);
}
}
ParamSet_WriteToEEProm(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
// update version info
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION);
}
// read active parameter set to staticParams stucture
ParamSet_ReadFromEEProm(getActiveParamSet());
printf("\n\rUsing Parameter Set %d", getActiveParamSet());
void setActiveParamSet(uint8_t setnumber) {
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
 
// load mixer table
if (!MixerTable_ReadFromEEProm()) {
printf("\n\rGenerating default Mixer Table");
MixerTable_Default(); // Quadro
MixerTable_WriteToEEProm();
}
// determine motornumber
RequiredMotors = 0;
for (i = 0; i < 16; i++) {
if (Mixer.Motor[i][MIX_THROTTLE] > 0)
RequiredMotors++;
}
 
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors);
printf("\n\r==============================");
}