Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 04.2007 Holger Buss

// + Nur für den privaten Gebrauch

// + www.MikroKopter.com

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),

// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.

// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt

// + bzgl. der Nutzungsbedingungen aufzunehmen.

// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,

// + Verkauf von Luftbildaufnahmen, usw.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,

// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts

// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"

// + eindeutig als Ursprung verlinkt und genannt werden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion

// + Benutzung auf eigene Gefahr

// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur

// + mit unserer Zustimmung zulässig

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Redistributions of source code (with or without modifications) must retain the above copyright notice,

// + this list of conditions and the following disclaimer.

// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived

// +     from this software without specific prior written permission.

// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet

// +     for non-commercial use (directly or indirectly)

// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted

// +     with our written permission

// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be

// +     clearly linked as origin

// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed

// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// +  POSSIBILITY OF SUCH DAMAGE.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Contant Values

// + 0-250 -> normale Values

// + 251 -> Poti1

// + 252 -> Poti2

// + 253 -> Poti3

// + 254 -> Poti4

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#ifndef EEMEM

#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"

// 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] = 10;                     // Throttle stick D=10

  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_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 = 12;

  staticParams.StickD = 16;

  staticParams.StickYawP = 12;

  staticParams.MinThrottle = 8;

  staticParams.MaxThrottle = 230;

  staticParams.CompassYawEffect = 128;

  staticParams.GyroP = 80;

  staticParams.GyroI = 100;

  staticParams.LowVoltageWarning = 94;

  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           */

/***************************************************/

void ParamSet_DefaultSet1(void) { // sport

  setOtherDefaults();

  gyro_setDefaults();

  setDefaultUserParams();

  memcpy(staticParams.Name, "Sport\0",6);

}

/***************************************************/

/*    Default Values for parameter set 2           */

/***************************************************/

void ParamSet_DefaultSet2(void) { // normal

  setOtherDefaults();

  gyro_setDefaults();

  setDefaultUserParams();

  staticParams.Height_Gain = 3;

  staticParams.J16Timing = 20;

  staticParams.J17Timing = 20;

  memcpy(staticParams.Name, "Normal\0", 7);

}

/***************************************************/

/*    Default Values for parameter set 3           */

/***************************************************/

void ParamSet_DefaultSet3(void) { // beginner

  setOtherDefaults();

  gyro_setDefaults();

  setDefaultUserParams();

  staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;

  staticParams.Height_Gain = 3;

  staticParams.EmergencyGasDuration = 20;

  staticParams.AxisCouplingYawCorrection = 70;

  staticParams.J16Timing = 30;

  staticParams.J17Timing = 30;

  memcpy(staticParams.Name, "Beginner\0", 9);

}

/***************************************************/

/*       Read Parameter from EEPROM as byte        */

/***************************************************/

uint8_t GetParamByte(uint16_t param_id) {

  return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);

}

/***************************************************/

/*       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);

}

/***************************************************/

/*       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);

}

/***************************************************/

/*       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();

}

/***************************************************/

/*        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();

}

/***************************************************/

/*       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);

}

/***************************************************/

/*       Set active parameter set                  */

/***************************************************/

void setActiveParamSet(uint8_t setnumber) {

  if(setnumber > 5) setnumber = 5;

  if(setnumber < 1) setnumber = 1;

  eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);

}

/***************************************************/

/*          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;

}

/***************************************************/

/*          Write Mixer Table to EEPROM            */

/***************************************************/

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;

}

/***************************************************/

/*    Default Values for Mixer Table               */

/***************************************************/

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);

}

/***************************************************/

/*       Initialize EEPROM Parameter Sets          */

/***************************************************/

void ParamSet_Init(void) {

  uint8_t Channel_Backup  = 0, i;

  // 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

    if(         (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) < 12)

                &&  (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12)

                &&  (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12)

                &&  (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]) < 12)

                )

      {

        Channel_Backup = 1;

      }

    // 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

        staticParams.ChannelAssignment[0] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]);

        staticParams.ChannelAssignment[1] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]);

        staticParams.ChannelAssignment[2] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]);

        staticParams.ChannelAssignment[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]);

        staticParams.ChannelAssignment[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]);

        staticParams.ChannelAssignment[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]);

        staticParams.ChannelAssignment[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]);

        staticParams.ChannelAssignment[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]);

      }

      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());

  // 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==============================");

}