Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1955 → Rev 1960

/branches/dongfang_FC_rewrite/configuration.c
49,16 → 49,20
// + 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};
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};
paramset_t staticParams;
channelMap_t channelMap;
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};
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.
68,76 → 72,43
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);
SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect);
SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,0,255);
SET_POT(dynamicParams.GyroI,staticParams.GyroI);
SET_POT(dynamicParams.GyroD,staticParams.GyroD);
SET_POT(dynamicParams.IFactor,staticParams.IFactor);
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]);
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
SET_POT(dynamicParams.gyroI, staticParams.gyroI);
SET_POT(dynamicParams.gyroD, staticParams.gyroD);
SET_POT(dynamicParams.IFactor, staticParams.IFactor);
SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor);
SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect);
 
SET_POT(dynamicParams.externalControl, staticParams.externalControl);
SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1);
SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2);
SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection);
SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability);
SET_POT(dynamicParams.maxControlActivityForAcc,staticParams.maxControlActivityForAcc);
 
SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100);
SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100);
SET_POT(dynamicParams.heightSetting,staticParams.heightSetting);
SET_POT(dynamicParams.heightACCEffect,staticParams.heightACCEffect);
SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl);
 
for (i=0; i<sizeof(staticParams.userParams); i++) {
SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
}
for (i=0; i<sizeof(staticParams.UserParams2); i++) {
SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]);
}
SET_POT(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl);
SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit);
SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1);
SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2);
SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection);
SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability);
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
 
#if defined (USE_NAVICTRL)
SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl);
SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain);
SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP);
SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI);
SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD);
SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC);
SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255);
SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection);
SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation);
#endif
SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl);
SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl);
SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl);
 
SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255);
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)},
{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},
{offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255}
// {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
};
 
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
168,21 → 139,13
}
}
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i));
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i);
for (i=0; i<sizeof(staticParams.userParams); 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);
if (src < 255) {
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
}
}
}
 
uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values
203,7 → 166,7
*/
 
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
213,16 → 176,16
 
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;
231,5 → 194,125
DDRB |= (1<<DDB1)|(1<<DDB0);
RED_ON;
GRN_OFF;
return BoardRelease;
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;
channelMap.channels[CH_POTS + 2] = 7;
channelMap.channels[CH_POTS + 3] = 8;
}