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