Subversion Repositories FlightCtrl

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Konstanten
// + 0-250 -> normale Werte
// + 251 -> Poti1
// + 252 -> Poti2
// + 253 -> Poti3
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#ifndef EEMEM
#define EEMEM __attribute__ ((section (".eeprom")))
#endif


#include <avr/eeprom.h>
#include <string.h>
#include "eeprom.h"
#include "printf_P.h"


// byte array in eeprom
uint8_t EEPromArray[E2END+1] EEMEM;

paramset_t ParamSet;



/***************************************************/
/*    Default Values for parameter set 1           */
/***************************************************/
void ParamSet_DefaultSet1(void) // sport
{
 ParamSet.ChannelAssignment[CH_PITCH]  = 1;
 ParamSet.ChannelAssignment[CH_ROLL]  = 2;
 ParamSet.ChannelAssignment[CH_THRUST]   = 3;
 ParamSet.ChannelAssignment[CH_YAW]  = 4;
 ParamSet.ChannelAssignment[CH_POTI1] = 5;
 ParamSet.ChannelAssignment[CH_POTI2] = 6;
 ParamSet.ChannelAssignment[CH_POTI3] = 7;
 ParamSet.ChannelAssignment[CH_POTI4] = 8;
 ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;//0x01;
 ParamSet.Height_MinThrust = 30;
 ParamSet.MaxHeight     = 251;      // Wert : 0-250   251 -> Poti1
 ParamSet.Height_P      = 10;       // Wert : 0-32
 ParamSet.Height_D  = 30;       // Wert : 0-250
 ParamSet.Height_ACC_Effect = 30;  // Wert : 0-250
 ParamSet.Height_Gain = 4;  // Wert : 0-50
 ParamSet.Stick_P = 15;         // Wert : 1-24
 ParamSet.Stick_D = 30;         // Wert : 0-250
 ParamSet.Yaw_P = 12;             // Wert : 1-20
 ParamSet.Trust_Min = 8;            // Wert : 0-32
 ParamSet.Trust_Max = 230;           // Wert : 33-250
 ParamSet.GyroAccFactor = 30;      // Wert : 1-64
 ParamSet.CompassYawEffect = 128;    // Wert : 0-250
 ParamSet.Gyro_P = 80;             // Wert : 0-250
 ParamSet.Gyro_I = 150;               // Wert : 0-250
 ParamSet.LowVoltageWarning = 94;       // Wert : 0-250
 ParamSet.EmergencyThrust = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
 ParamSet.EmergencyThrustDuration = 30;            // Wert : 0-250     // Zeit bis auf EmergencyThrust geschaltet wird, wg. Rx-Problemen
 ParamSet.UfoArrangement = 0;         // X oder + Formation
 ParamSet.I_Factor = 32;
 ParamSet.UserParam1 = 253;             //zur freien Verwendung
 ParamSet.UserParam2 = 100;             //zur freien Verwendung
 ParamSet.UserParam3 = 90;             //zur freien Verwendung
 ParamSet.UserParam4 = 90;             //zur freien Verwendung
 ParamSet.UserParam5 = 90;             // zur freien Verwendung
 ParamSet.UserParam6 = 90;             // zur freien Verwendung
 ParamSet.UserParam7 = 0;             // zur freien Verwendung
 ParamSet.UserParam8 = 0;             // zur freien Verwendung
 ParamSet.ServoPitchControl = 100;     // Wert : 0-250     // Stellung des Servos
 ParamSet.ServoPitchComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 ParamSet.ServoPitchCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 ParamSet.ServoPitchMin = 50;          // Wert : 0-250     // Anschlag
 ParamSet.ServoPitchMax = 150;         // Wert : 0-250     // Anschlag
 ParamSet.ServoPitchRefresh = 5;
 ParamSet.LoopThrustLimit = 50;
 ParamSet.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
 ParamSet.LoopHysteresis = 50;
 ParamSet.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
 ParamSet.Yaw_PosFeedback = 90;
 ParamSet.Yaw_NegFeedback = 5;
 ParamSet.AngleTurnOverPitch = 100;
 ParamSet.AngleTurnOverRoll = 100;
 ParamSet.GyroAccTrim = 16;        // 1/k
 ParamSet.DriftComp = 4;
 ParamSet.DynamicStability = 100;
 memcpy(ParamSet.Name, "Sport\0",6);
}


/***************************************************/
/*    Default Values for parameter set 2           */
/***************************************************/
void ParamSet_DefaultSet2(void) // normal
{
 ParamSet.ChannelAssignment[CH_PITCH]  = 1;
 ParamSet.ChannelAssignment[CH_ROLL]  = 2;
 ParamSet.ChannelAssignment[CH_THRUST]   = 3;
 ParamSet.ChannelAssignment[CH_YAW]  = 4;
 ParamSet.ChannelAssignment[CH_POTI1] = 5;
 ParamSet.ChannelAssignment[CH_POTI2] = 6;
 ParamSet.ChannelAssignment[CH_POTI3] = 7;
 ParamSet.ChannelAssignment[CH_POTI4] = 8;
 ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;//0x01;
 ParamSet.Height_MinThrust = 30;
 ParamSet.MaxHeight     = 251;         // Wert : 0-250   251 -> Poti1
 ParamSet.Height_P      = 10;          // Wert : 0-32
 ParamSet.Height_D  = 30;          // Wert : 0-250
 ParamSet.Height_ACC_Effect = 30;     // Wert : 0-250
 ParamSet.Height_Gain = 3;     // Wert : 0-50
 ParamSet.Stick_P = 12;            // Wert : 1-24
 ParamSet.Stick_D = 16;           // Wert : 0-250
 ParamSet.Yaw_P = 6;                 // Wert : 1-20
 ParamSet.Trust_Min = 8;               // Wert : 0-32
 ParamSet.Trust_Max = 230;              // Wert : 33-250
 ParamSet.GyroAccFactor = 30;         // Wert : 1-64
 ParamSet.CompassYawEffect = 128;        // Wert : 0-250
 ParamSet.Gyro_P = 80;                // Wert : 0-250
 ParamSet.Gyro_I = 120;               // Wert : 0-250
 ParamSet.LowVoltageWarning = 94; // Wert : 0-250
 ParamSet.EmergencyThrust = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
 ParamSet.EmergencyThrustDuration = 30;            // Wert : 0-250     // Zeit bis auf EmergencyThrust geschaltet wird, wg. Rx-Problemen
 ParamSet.UfoArrangement = 0;         // X oder + Formation
 ParamSet.I_Factor = 32;
 ParamSet.UserParam1 = 253;             // zur freien Verwendung
 ParamSet.UserParam2 = 100;             // zur freien Verwendung
 ParamSet.UserParam3 = 90;             // zur freien Verwendung
 ParamSet.UserParam4 = 90;             // zur freien Verwendung
 ParamSet.UserParam5 = 90;             // zur freien Verwendung
 ParamSet.UserParam6 = 90;             // zur freien Verwendung
 ParamSet.UserParam7 = 0;             // zur freien Verwendung
 ParamSet.UserParam8 = 0;             // zur freien Verwendung
 ParamSet.ServoPitchControl = 100;     // Wert : 0-250     // Stellung des Servos
 ParamSet.ServoPitchComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 ParamSet.ServoPitchCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 ParamSet.ServoPitchMin = 50;          // Wert : 0-250     // Anschlag
 ParamSet.ServoPitchMax = 150;         // Wert : 0-250     // Anschlag
 ParamSet.ServoPitchRefresh = 5;
 ParamSet.LoopThrustLimit = 50;
 ParamSet.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
 ParamSet.LoopHysteresis = 50;
 ParamSet.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
 ParamSet.Yaw_PosFeedback = 90;        // Faktor, mit dem Yaw die Achsen Roll und Pitch verkoppelt
 ParamSet.Yaw_NegFeedback = 5;
 ParamSet.AngleTurnOverPitch = 100;
 ParamSet.AngleTurnOverRoll = 100;
 ParamSet.GyroAccTrim = 32;        // 1/k
 ParamSet.DriftComp = 4;
 ParamSet.DynamicStability = 75;
 memcpy(ParamSet.Name, "Normal\0", 7);
}


/***************************************************/
/*    Default Values for parameter set 3           */
/***************************************************/
void ParamSet_DefaultSet3(void) // beginner
{
 ParamSet.ChannelAssignment[CH_PITCH]  = 1;
 ParamSet.ChannelAssignment[CH_ROLL]  = 2;
 ParamSet.ChannelAssignment[CH_THRUST]   = 3;
 ParamSet.ChannelAssignment[CH_YAW]  = 4;
 ParamSet.ChannelAssignment[CH_POTI1] = 5;
 ParamSet.ChannelAssignment[CH_POTI2] = 6;
 ParamSet.ChannelAssignment[CH_POTI3] = 7;
 ParamSet.ChannelAssignment[CH_POTI4] = 8;
 ParamSet.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;//0x01;
 ParamSet.Height_MinThrust = 30;
 ParamSet.MaxHeight     = 251;         // Wert : 0-250   251 -> Poti1
 ParamSet.Height_P      = 10;          // Wert : 0-32
 ParamSet.Height_D  = 30;              // Wert : 0-250
 ParamSet.Height_ACC_Effect = 30;     // Wert : 0-250
 ParamSet.Height_Gain = 3;            // Wert : 0-50
 ParamSet.Stick_P = 8;              // Wert : 1-24
 ParamSet.Stick_D = 16;              // Wert : 0-250
 ParamSet.Yaw_P  = 6;                // Wert : 1-20
 ParamSet.Trust_Min = 8;               // Wert : 0-32
 ParamSet.Trust_Max = 230;              // Wert : 33-250
 ParamSet.GyroAccFactor = 30;         // Wert : 1-64
 ParamSet.CompassYawEffect = 128;       // Wert : 0-250
 ParamSet.Gyro_P = 100;               // Wert : 0-250
 ParamSet.Gyro_I = 120;               // Wert : 0-250
 ParamSet.LowVoltageWarning = 94; // Wert : 0-250
 ParamSet.EmergencyThrust = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
 ParamSet.EmergencyThrustDuration = 20;            // Wert : 0-250     // Zeit bis auf EmergencyThrust geschaltet wird, wg. Rx-Problemen
 ParamSet.UfoArrangement = 0;         // X oder + Formation
 ParamSet.I_Factor = 16;
 ParamSet.UserParam1 = 253;             // zur freien Verwendung
 ParamSet.UserParam2 = 100;             // zur freien Verwendung
 ParamSet.UserParam3 = 90;             // zur freien Verwendung
 ParamSet.UserParam4 = 90;             // zur freien Verwendung
 ParamSet.UserParam5 = 90;             // zur freien Verwendung
 ParamSet.UserParam6 = 90;             // zur freien Verwendung
 ParamSet.UserParam7 = 0;             // zur freien Verwendung
 ParamSet.UserParam8 = 0;             // zur freien Verwendung
 ParamSet.ServoPitchControl = 100;     // Wert : 0-250     // Stellung des Servos
 ParamSet.ServoPitchComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 ParamSet.ServoPitchCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 ParamSet.ServoPitchMin = 50;          // Wert : 0-250     // Anschlag
 ParamSet.ServoPitchMax = 150;         // Wert : 0-250     // Anschlag
 ParamSet.ServoPitchRefresh = 5;
 ParamSet.LoopThrustLimit = 50;
 ParamSet.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
 ParamSet.LoopHysteresis = 50;
 ParamSet.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
 ParamSet.Yaw_PosFeedback = 90;        // Faktor, mit dem Yaw die Achsen Roll und Pitch verkoppelt
 ParamSet.Yaw_NegFeedback = 5;
 ParamSet.AngleTurnOverPitch = 100;
 ParamSet.AngleTurnOverRoll = 100;
 ParamSet.GyroAccTrim = 32;        // 1/k
 ParamSet.DriftComp = 4;
 ParamSet.DynamicStability = 50;
 memcpy(ParamSet.Name, "Beginner\0", 9);
}

/***************************************************/
/*       Read Parameter from EEPROM as byte        */
/***************************************************/
uint8_t GetParamByte(uint8_t param_id)
{
        return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}

/***************************************************/
/*       Write Parameter to EEPROM as byte         */
/***************************************************/
void SetParamByte(uint8_t param_id, uint8_t value)
{
        eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}

/***************************************************/
/*       Read Parameter from EEPROM as word        */
/***************************************************/
uint16_t GetParamWord(uint8_t param_id)
{
        return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}

/***************************************************/
/*       Write Parameter to EEPROM as word         */
/***************************************************/
void SetParamWord(uint8_t param_id, uint16_t value)
{
        eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}


/***************************************************/
/*       Read Parameter Set from EEPROM            */
/***************************************************/
// number [0..5]
void ParamSet_ReadFromEEProm(uint8_t setnumber)
{
   if (setnumber > 5) setnumber = 5;
   eeprom_read_block((uint8_t *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * setnumber], PARAMSET_STRUCT_LEN);
}

/***************************************************/
/*        Write Parameter Set to EEPROM            */
/***************************************************/
// number [0..5]
void ParamSet_WriteToEEProm(uint8_t setnumber)
{
   if(setnumber > 5) setnumber = 5;
   eeprom_write_block((uint8_t *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * setnumber], PARAMSET_STRUCT_LEN);
   // set this parameter set to active set
   eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}

/***************************************************/
/*       Get active parameter set                  */
/***************************************************/
uint8_t GetActiveParamSet(void)
{
        uint8_t setnumber;
        setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]);
        if(setnumber > 5)
        {
        setnumber = 2;
        eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
        }
        return(setnumber);
}

/***************************************************/
/*       Set active parameter set                  */
/***************************************************/
void SetActiveParamSet(uint8_t setnumber)
{
        if(setnumber > 5) setnumber = 5;
        eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}

/***************************************************/
/*       Initialize EEPROM Parameter Sets          */
/***************************************************/
void ParamSet_Init(void)
{
        // version  check
        if(eeprom_read_byte(&EEPromArray[PID_VERSION]) != EEPARAM_VERSION)
        {
                // if version check faild
                printf("\n\rInit. EEPROM: Generating Default-Parameter...");
                ParamSet_DefaultSet1(); // Fill ParamSet Structure to default parameter set 1 (Sport)
                // fill all 5 parameter settings with set 1 except otherwise defined
                for (unsigned char i=0;i<6;i++)
                {
                if(i==2) ParamSet_DefaultSet2(); // Kamera
                if(i==3) ParamSet_DefaultSet3(); // Beginner
                if(i>3)  ParamSet_DefaultSet2(); // Kamera
                ParamSet_WriteToEEProm(i);
                }
                // default-Setting is parameter set 3
                SetParamByte(PID_ACTIVE_SET, 3);
                // update version info
                SetParamByte(PID_VERSION, EEPARAM_VERSION);
        }
        // read active parameter set to ParamSet stucture
        ParamSet_ReadFromEEProm(GetParamByte(PID_ACTIVE_SET));
        printf("\n\rUsing Parameter Set %d", GetParamByte(PID_ACTIVE_SET));
}