WebSVN - FlightCtrl - Diff - Rev 1955 and 1960 - /branches/dongfang_FC

 // +  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.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <util/delay.h>
-#include <avr/eeprom.h>
 #include <stddef.h>
+#include <string.h>
 #include "configuration.h"
-#include "eeprom.h"
+#include "sensors.h"
+#include "rc.h"
+int16_t variables[8] = {0,0,0,0,0,0,0,0};
+paramset_t staticParams;
+channelMap_t channelMap;
-int16_t variables[8] = {0,0,0,0,0,0,0,0};
+mixerMatrix_t mixerMatrix;
-dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
+dynamicParam_t dynamicParams; // = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
 uint8_t CPUType = ATMEGA644;
-uint8_t BoardRelease = 13;
+uint8_t boardRelease = 13;
 uint8_t requiredMotors;
 /************************************************************************
  * Map the parameter to pot values
  * Replacing this code by the code below saved almost 1 kbyte.
  ************************************************************************/
 void configuration_applyVariablesToParams(void) {
   uint8_t i;
-#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
-#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
-  SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight);
-  SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100);
-  SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100);
-  SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect);
+#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
-  SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect);
+#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
-  SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,0,255);
+  SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
-  SET_POT(dynamicParams.GyroI,staticParams.GyroI);
+  SET_POT(dynamicParams.gyroI, staticParams.gyroI);
-  SET_POT(dynamicParams.GyroD,staticParams.GyroD);
-  SET_POT(dynamicParams.IFactor,staticParams.IFactor);
+  SET_POT(dynamicParams.gyroD, staticParams.gyroD);
-  for (i=0; i<sizeof(staticParams.UserParams1); i++) {
-    SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]);
-  }
+  SET_POT(dynamicParams.IFactor, staticParams.IFactor);
-  for (i=0; i<sizeof(staticParams.UserParams2); i++) {
+  SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor);
-    SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]);
+  SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect);
-  }
-  SET_POT(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl);
-  SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit);
+  SET_POT(dynamicParams.externalControl, staticParams.externalControl);
-  SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1);
+  SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1);
-  SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2);
+  SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2);
-  SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection);
-  SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability);
+  SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection);
-  SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
+  SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability);
-  SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
+  SET_POT(dynamicParams.maxControlActivityForAcc,staticParams.maxControlActivityForAcc);
-#if defined (USE_NAVICTRL)
+  SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100);
-  SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl);
+  SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100);
-  SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain);
+  SET_POT(dynamicParams.heightSetting,staticParams.heightSetting);
+  SET_POT(dynamicParams.heightACCEffect,staticParams.heightACCEffect);
-  SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP);
+  SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl);
-  SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI);
+  for (i=0; i<sizeof(staticParams.userParams); i++) {
+    SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
-  SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD);
+  }
-  SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC);
+  SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl);
-  SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255);
+  SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl);
-  SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection);
-  SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation);
-#endif
+  SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255);
-  SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl);
+  SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255);
 }
-const XLATION XLATIONS[] = {
-  {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
-  {offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)},
-  {offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)},
-  {offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)},
-  {offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)},
-  {offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)},
-  {offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)},
-  {offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)},
-  {offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)},
-  {offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)},
-  {offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)},
-  {offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)},
-  {offsetof(paramset_t, NaviGpsModeControl),
-   offsetof(dynamicParam_t, NaviGpsModeControl)},
-  {offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)},
-  {offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)},
-  {offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)},
-  {offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)},
-  {offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)},
   {offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)},
-  {offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)},
+const XLATION XLATIONS[] = {
-  {offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)}
+  //  {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
-};
-const MMXLATION MMXLATIONS[] = {
-  {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
-  {offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightP),0,150},
-  {offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255},
+};
   {offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255},
-  {offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255},
+const MMXLATION MMXLATIONS[] = {
-  {offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255}
+  //  {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
     if (src < 255) {
       *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max);
+    }
+  }
-  for (i=0; i<sizeof(staticParams.UserParams1); i++) {
+  for (i=0; i<sizeof(staticParams.userParams); i++) {
-    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i));
+    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, userParams) + i));
-    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i);
-    if (src < 255) {
-      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
-    }
-  }
-  for (i=0; i<sizeof(staticParams.UserParams2); i++) {
-    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i));
-    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i);
+    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i);
     if (src < 255) {
       *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
+    }
+  }
  * tedious to have to modify the code for how to turn on and off LEDs when deploying
  * on different HW version....
  */
 uint8_t getBoardRelease(void) {
-  uint8_t BoardRelease = 13;
+  uint8_t boardRelease = 13;
   // the board release is coded via the pull up or down the 2 status LED
   PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
   DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
   _delay_loop_2(1000); // make some delay
   switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
     case 0x00:
-      BoardRelease = 10; // 1.0
+      boardRelease = 10; // 1.0
       break;
     case 0x01:
-      BoardRelease = 11; // 1.1 or 1.2
+      boardRelease = 11; // 1.1 or 1.2
       break;
     case 0x02:
-      BoardRelease = 20; // 2.0
+      boardRelease = 20; // 2.0
       break;
     case 0x03:
-      BoardRelease = 13; // 1.3
+      boardRelease = 13; // 1.3
       break;
     default:
       break;
+    }
   // set LED ports as output
   DDRB |= (1<<DDB1)|(1<<DDB0);
+  RED_ON;
+  GRN_OFF;
+  return boardRelease;
+}
+void setOtherDefaults(void) {
+  // Height Control
+  staticParams.heightP = 10;
+  staticParams.heightD = 30;
+  staticParams.heightSetting = 251;
+  staticParams.heightMaxThrottleChange = 10;
+  staticParams.heightSlewRate = 4;
+  staticParams.heightACCEffect = 30;
+  // Control
+  staticParams.stickP = 8;
+  staticParams.stickD = 12;
+  staticParams.stickYawP = 12;
+  staticParams.stickThrottleD = 12;
+  staticParams.minThrottle = 8;
+  staticParams.maxThrottle = 230;
+  staticParams.externalControl = 0;
+  // IMU
+  staticParams.gyroPIDFilterConstant = 1;
+  staticParams.gyroATTFilterConstant = 1;
+  staticParams.gyroDFilterConstant = 1;
+  staticParams.accFilterConstant = 4;
+  staticParams.gyroP = 60;
+  staticParams.gyroI = 80;
+  staticParams.gyroD = 4;
+  // set by gyro code.
+  // staticParams.zerothOrderCorrection =
+  // staticParams.driftCompDivider =
+  // staticParams.driftCompLimit =
+  staticParams.axisCoupling1 = 90;
+  staticParams.axisCoupling2 = 67;
+  staticParams.axisCouplingYawCorrection = 0;
+  staticParams.dynamicStability = 50;
+  staticParams.IFactor = 32;
+  staticParams.compassYawEffect = 128;
+  // Servos
+  for (uint8_t i=0; i<2; i++) {
+    staticParams.servoConfigurations[i].manualControl = 128;
+    staticParams.servoConfigurations[i].compensationFactor = 0;
+    staticParams.servoConfigurations[i].minValue = 64;
+    staticParams.servoConfigurations[i].maxValue = 192;
+    staticParams.servoConfigurations[i].flags = 0;
+  }
+  staticParams.servoCount = 4;
+  // Battery warning and emergency flight
+  staticParams.batteryVoltageWarning = 101;  // 3.7 each for 3S
+  staticParams.emergencyThrottle = 35;
+  staticParams.emergencyFlightDuration = 30;
+  // Outputs
+  staticParams.outputFlash[0].bitmask = 0b01011111;
+  staticParams.outputFlash[0].timing = 15;
+  staticParams.outputFlash[1].bitmask = 0b11110011;
+  staticParams.outputFlash[1].timing = 15;
+  staticParams.outputFlags = 0;
+}
+/***************************************************/
+/*    Default Values for parameter set 1           */
+/***************************************************/
+void paramSet_default(uint8_t setnumber) {
+  setOtherDefaults();
+  gyro_setDefaults();
+  for (uint8_t i=0; i<8; i++) {
+    staticParams.userParams[i] = 0;
+  }
+  staticParams.bitConfig =
+    CFG_GYRO_SATURATION_PREVENTION |
+    CFG_HEADING_HOLD;
+  staticParams.bitConfig2 = 0;
+  memcpy(staticParams.name, "Default\0", 6);
+}
+/***************************************************/
+/*    Default Values for Mixer Table               */
+/***************************************************/
+void mixerMatrix_default(void) { // Quadro
+  uint8_t i;
+  // mixerMatric.revision = EEMIXER_REVISION;
+  // clear mixer table (but preset throttle)
+  for (i = 0; i < 16; i++) {
+    mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
+    mixerMatrix.motor[i][MIX_PITCH] = 0;
+    mixerMatrix.motor[i][MIX_ROLL] = 0;
+    mixerMatrix.motor[i][MIX_YAW] = 0;
+  }
+  // default = Quadro
+  mixerMatrix.motor[0][MIX_PITCH] = +64;
+  mixerMatrix.motor[0][MIX_YAW] = +64;
+  mixerMatrix.motor[1][MIX_PITCH] = -64;
+  mixerMatrix.motor[1][MIX_YAW] = +64;
+  mixerMatrix.motor[2][MIX_ROLL] = -64;
+  mixerMatrix.motor[2][MIX_YAW] = -64;
+  mixerMatrix.motor[3][MIX_ROLL] = +64;
+  mixerMatrix.motor[3][MIX_YAW] = -64;
+  memcpy(mixerMatrix.name, "Quadro\0", 7);
+}
+void channelMap_default(void) {
+  channelMap.channels[CH_PITCH] = 2;
+  channelMap.channels[CH_ROLL] = 1;
+  channelMap.channels[CH_THROTTLE] = 3;
+  channelMap.channels[CH_YAW] = 4;
+  channelMap.channels[CH_POTS + 0] = 5;
+  channelMap.channels[CH_POTS + 1] = 6;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/configuration.c - Rev 1955 → 1960