#include <util/delay.h>
#include <stddef.h>
#include <string.h>
#include "configuration.h"
#include "controlMixer.h"
#include "rc.h"
#include "output.h"
#include "flight.h"
int16_t variables
[VARIABLE_COUNT
];
ParamSet_t staticParams
;
ChannelMap_t channelMap
;
RCTrim_t rcTrim
;
IMUConfig_t IMUConfig
;
volatile DynamicParams_t dynamicParams
;
VersionInfo_t versionInfo
;
FlightMode_t currentFlightMode
= FLIGHT_MODE_MANUAL
;
// updated by considering airspeed..
volatile uint16_t isFlying
= 0;
const MMXLATION XLATIONS
[] = {
{offsetof(ParamSet_t
, externalControl
), offsetof(DynamicParams_t
, externalControl
),0,255},
{offsetof(ParamSet_t
, gyroPID
[0].
P), offsetof(DynamicParams_t
, gyroPID
[0].
P),0,255},
{offsetof(ParamSet_t
, gyroPID
[0].
I), offsetof(DynamicParams_t
, gyroPID
[0].
I),0,255},
{offsetof(ParamSet_t
, gyroPID
[0].
D), offsetof(DynamicParams_t
, gyroPID
[0].
D),0,255},
{offsetof(ParamSet_t
, gyroPID
[1].
P), offsetof(DynamicParams_t
, gyroPID
[1].
P),0,255},
{offsetof(ParamSet_t
, gyroPID
[1].
I), offsetof(DynamicParams_t
, gyroPID
[1].
I),0,255},
{offsetof(ParamSet_t
, gyroPID
[1].
D), offsetof(DynamicParams_t
, gyroPID
[1].
D),0,255},
{offsetof(ParamSet_t
, gyroPID
[2].
P), offsetof(DynamicParams_t
, gyroPID
[2].
P),0,255},
{offsetof(ParamSet_t
, gyroPID
[2].
I), offsetof(DynamicParams_t
, gyroPID
[2].
I),0,255},
{offsetof(ParamSet_t
, gyroPID
[2].
D), offsetof(DynamicParams_t
, gyroPID
[2].
D),0,255},
{offsetof(ParamSet_t
, outputFlash
[0].
timing), offsetof(DynamicParams_t
, output0Timing
),0,255},
{offsetof(ParamSet_t
, outputFlash
[1].
timing), offsetof(DynamicParams_t
, output1Timing
),0,255},
};
uint8_t configuration_applyVariableToParam
(uint8_t src
, uint8_t min
, uint8_t max
) {
uint8_t result
;
if (src
>=(256-VARIABLE_COUNT
)) result
= variables
[src
-(256-VARIABLE_COUNT
)];
else result
= src
;
if (result
< min
) result
= min
;
else if (result
> max
) result
= max
;
return result
;
}
void configuration_applyVariablesToParams
(void) {
uint8_t i
, src
;
uint8_t* pointerToTgt
;
for(i
=0; i
<sizeof(XLATIONS
)/sizeof(MMXLATION
); i
++) {
src
= *((uint8_t*)(&staticParams
) + XLATIONS
[i
].
sourceIdx);
pointerToTgt
= (uint8_t*)(&dynamicParams
) + XLATIONS
[i
].
targetIdx;
*pointerToTgt
= configuration_applyVariableToParam
(src
, XLATIONS
[i
].
min, XLATIONS
[i
].
max);
}
// User parameters are always variable.
for (i
=0; i
<sizeof(staticParams.
userParams); i
++) {
src
= *((uint8_t*)(&staticParams
) + offsetof(ParamSet_t
, userParams
) + i
);
pointerToTgt
= (uint8_t*)(&dynamicParams
) + offsetof(DynamicParams_t
, userParams
) + i
;
*pointerToTgt
= configuration_applyVariableToParam
(src
, 0, 255);
}
}
void configuration_setFlightParameters
(uint8_t newFlightMode
) {
currentFlightMode
= newFlightMode
;
flight_updateFlightParametersToFlightMode
();
}
// Called after a change in configuration parameters, as a hook for modules to take over changes.
void configuration_paramSetDidChange
(void) {
// This should be OK to do here as long as we don't change parameters during emergency flight. We don't.
configuration_setFlightParameters
(currentFlightMode
);
// Immediately load changes to output, and also signal the paramset change.
output_init
();
}
void setOtherDefaults
(void) {
// Control
staticParams.
externalControl = 0;
staticParams.
IFactor = 52;
staticParams.
airspeedCorrection = 1;
staticParams.
isFlyingThreshold = 4;
staticParams.
minFlashAirspeed = 5;
staticParams.
maxFlashAirspeed = 9;
// Servos
staticParams.
servoCount = 6;
staticParams.
servos[CONTROL_ELEVATOR
].
reverse = 0;
staticParams.
servos[CONTROL_AILERONS
].
reverse = 0; // 1 for extra.
staticParams.
servos[CONTROL_RUDDER
].
reverse = 0;
for (uint8_t i
=0; i
<4; i
++) {
staticParams.
servos[i
].
minValue = 80;
staticParams.
servos[i
].
maxValue = 80;
}
// Battery warning and emergency flight
staticParams.
batteryWarningVoltage = 101; // 3.7 each for 3S
for (uint8_t i
=0; i
<3; i
++) {
staticParams.
gyroPID[i
].
P = 40;
staticParams.
gyroPID[i
].
I = 20;
staticParams.
gyroPID[i
].
D = 20;
staticParams.
gyroPID[i
].
iMax = 30; // 60 for extra.
}
staticParams.
stickIElevator = 40;
staticParams.
stickIAilerons = 60;
staticParams.
stickIRudder = 20;
// Outputs
staticParams.
outputFlash[0].
bitmask = 1; //0b01011111;
staticParams.
outputFlash[0].
timing = 15;
staticParams.
outputFlash[1].
bitmask = 3; //0b11110011;
staticParams.
outputFlash[1].
timing = 15;
staticParams.
outputDebugMask = 0;
staticParams.
outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP
| OUTPUTFLAGS_FLASH_1_AT_BEEP
| OUTPUTFLAGS_USE_ONBOARD_LEDS
;
}
/***************************************************/
/* Default Values for parameter set 1 */
/***************************************************/
void paramSet_default
(uint8_t setnumber
) {
setOtherDefaults
();
for (uint8_t i
=0; i
<8; i
++) {
staticParams.
userParams[i
] = i
;
}
staticParams.
bitConfig =
CFG_GYRO_SATURATION_PREVENTION
;
memcpy(staticParams.
name, "Default\0", 6);
}
void IMUConfig_default
(void) {
IMUConfig.
gyroPIDFilterConstant = 10;
IMUConfig.
gyroDFilterConstant = 1;
IMUConfig.
rateTolerance = 120;
IMUConfig.
gyroDWindowLength = 8;
IMUConfig.
gyroQuadrant = 4;
IMUConfig.
imuReversedFlags = 0;
}
/***************************************************/
/* Default Values for R/C Channels */
/***************************************************/
void channelMap_default
(void) {
channelMap.
RCPolarity = 1;
channelMap.
HWTrim = 175;
channelMap.
variableOffset = 131;
channelMap.
channels[CH_ELEVATOR
] = 1;
channelMap.
channels[CH_AILERONS
] = 0;
channelMap.
channels[CH_THROTTLE
] = 2;
channelMap.
channels[CH_RUDDER
] = 3;
channelMap.
channels[CH_POTS
+ 0] = 4;
channelMap.
channels[CH_POTS
+ 1] = 5;
channelMap.
channels[CH_POTS
+ 2] = 6;
channelMap.
channels[CH_POTS
+ 3] = 7;
}