6,33 → 6,44 |
#include "rc.h" |
#include "output.h" |
#include "flight.h" |
#include "debug.h" |
|
int16_t variables[VARIABLE_COUNT]; |
ParamSet_t staticParams; |
ChannelMap_t channelMap; |
MotorMixer_t motorMixer; |
OutputMixer_t outputMixer; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
|
uint8_t CPUType; |
uint8_t boardRelease; |
uint8_t requiredMotors; |
|
VersionInfo_t versionInfo; |
|
// MK flags. TODO: Replace by enum. State machine. |
uint16_t isFlying = 0; |
volatile uint8_t MKFlags = 0; |
uint16_t isFlying; |
volatile uint8_t MKFlags; |
|
const MMXLATION XLATIONS[] = { |
{offsetof(ParamSet_t, flightMode), offsetof(DynamicParams_t, flightMode),0,255}, |
|
{offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255}, |
{offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255}, |
{offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255}, |
{offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255}, |
{offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255}, |
|
{offsetof(ParamSet_t, attGyroP), offsetof(DynamicParams_t, attGyroP),0,250}, |
{offsetof(ParamSet_t, attGyroI), offsetof(DynamicParams_t, attGyroI),0,250}, |
{offsetof(ParamSet_t, attGyroD), offsetof(DynamicParams_t, attGyroD),0,250}, |
|
{offsetof(ParamSet_t, rateGyroP), offsetof(DynamicParams_t, rateGyroP),0,250}, |
{offsetof(ParamSet_t, rateGyroI), offsetof(DynamicParams_t, rateGyroI),0,250}, |
{offsetof(ParamSet_t, rateGyroD), offsetof(DynamicParams_t, rateGyroD),0,250}, |
|
{offsetof(ParamSet_t, yawGyroP), offsetof(DynamicParams_t, yawGyroP),0,250}, |
{offsetof(ParamSet_t, yawGyroI), offsetof(DynamicParams_t, yawGyroI),0,250}, |
{offsetof(ParamSet_t, yawGyroD), offsetof(DynamicParams_t, yawGyroD),0,250}, |
|
{offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255}, |
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
{offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255}, |
{offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255}, |
{offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255}, |
{offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255}, |
52,6 → 63,19 |
return result; |
} |
|
void configuration_fixVariableParams(uint8_t variableNumber) { |
// If variable value is to great, limit it. |
uint8_t value = variables[variableNumber]; |
if (value >= 256-VARIABLE_COUNT) |
value = 256-VARIABLE_COUNT - 1; |
for (uint8_t i=0; i<sizeof(DynamicParams_t); i++) { |
uint8_t* pointerToParam =(uint8_t*)(&dynamicParams + i); |
if (*pointerToParam == 256-VARIABLE_COUNT + variableNumber) { |
*pointerToParam = value; |
} |
} |
} |
|
void configuration_applyVariablesToParams(void) { |
uint8_t i, src; |
uint8_t* pointerToTgt; |
119,20 → 143,29 |
GRN_OFF; |
} |
|
void configuration_setNormalFlightParameters(void) { |
flight_setParameters(staticParams.IFactor, dynamicParams.gyroP, |
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
dynamicParams.gyroP, staticParams.yawIFactor); |
void configuration_setNormalFlightMode(void) { |
FlightMode_t flight_flightMode; |
if (dynamicParams.flightMode < 255/3) |
flight_flightMode = FM_RETURN_TO_LEVEL; |
else if (dynamicParams.flightMode < 255*2/3) |
flight_flightMode = FM_HEADING_HOLD; |
else |
flight_flightMode = FM_RATE; |
flight_setMode(flight_flightMode); |
debugOut.analog[20] = flight_flightMode; |
} |
|
void configuration_setFailsafeFlightParameters(void) { |
flight_setParameters(0, 90, 120, 90, 120); |
void configuration_setFailsafeFlightMode(void) { |
flight_setMode(FM_RETURN_TO_LEVEL); |
} |
|
// 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_setNormalFlightParameters(); |
// No longer necessary. |
// configuration_setNormalFlightMode(); |
flight_setParameters(); |
output_setParameters(); |
// Immediately load changes to output, and also signal the paramset change. |
output_init(); |
} |
140,7 → 173,9 |
void setOtherDefaults(void) { |
// Height Control |
staticParams.airpressureFilterConstant = 8; |
//staticParams.airpressureWindowLength = 0; |
staticParams.airpressureWindowLength = 8; |
staticParams.airpressureDWindowLength = 24; |
|
staticParams.airpressureAccZCorrection = 128+56; |
staticParams.heightP = 10; |
staticParams.heightD = 30; |
148,40 → 183,45 |
staticParams.heightControlMaxThrottleChange = 10; |
|
// Control |
staticParams.flightMode = 248; |
staticParams.stickP = 8; |
staticParams.stickD = 12; |
staticParams.stickYawP = 12; |
staticParams.stickThrottleD = 12; |
staticParams.stickD = 0; |
staticParams.stickYawP = 8; |
staticParams.stickThrottleD = 4; |
|
staticParams.minThrottle = 8; |
staticParams.maxThrottle = 230; |
|
staticParams.externalControl = 0; |
staticParams.attitudeControl = 0; |
staticParams.motorSmoothing = 0; |
staticParams.dynamicStability = 80; // 0 means: Regulation only by decreasing throttle. 200: Only by increasing. |
|
staticParams.gyroP = 60; |
staticParams.gyroI = 80; |
staticParams.gyroD = 4; |
staticParams.attGyroP = PID_NORMAL_VALUE; |
staticParams.attGyroI = PID_NORMAL_VALUE; |
staticParams.attGyroIMax = 0; |
staticParams.attGyroD = PID_NORMAL_VALUE * 3/2; |
|
staticParams.rateGyroP = PID_NORMAL_VALUE; |
staticParams.rateGyroI = PID_NORMAL_VALUE; |
staticParams.rateGyroIMax = 0; |
staticParams.rateGyroD = PID_NORMAL_VALUE; |
|
staticParams.yawGyroP = PID_NORMAL_VALUE; |
staticParams.yawGyroI = PID_NORMAL_VALUE; |
staticParams.yawGyroD = PID_NORMAL_VALUE; |
|
// set by gyro-specific code: gyro_setDefaults(). |
// staticParams.zerothOrderCorrection = |
// staticParams.driftCompDivider = |
// staticParams.driftCompLimit = |
|
staticParams.dynamicStability = 50; |
staticParams.IFactor = 52; |
staticParams.yawIFactor = 100; |
staticParams.compassMode = 0; |
staticParams.compassYawCorrection = 64; |
staticParams.compassP = 50; |
staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128; |
staticParams.compassP = PID_NORMAL_VALUE; |
staticParams.levelCorrection[0] = 249; // var1 |
staticParams.levelCorrection[1] = 250; // var2 |
|
// Servos |
staticParams.servoCount = 7; |
staticParams.servoCount = 8; |
staticParams.servoManualMaxSpeed = 10; |
for (uint8_t i=0; i<2; i++) { |
staticParams.servoConfigurations[i].manualControl = 128; |
staticParams.servoConfigurations[i].stabilizationFactor = 0; |
staticParams.servoConfigurations[i].minValue = 32; |
staticParams.servoConfigurations[i].maxValue = 224; |
staticParams.servoConfigurations[i].stabilizationFactor = 100; |
staticParams.servoConfigurations[i].flags = 0; |
} |
|
196,12 → 236,10 |
staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
staticParams.outputFlash[1].timing = 15; |
|
staticParams.outputDebugMask = DEBUG_ACC0THORDER; |
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
staticParams.outputDebugMask = DEBUG_INVERTED; |
staticParams.outputFlags = /* OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | */ OUTPUTFLAGS_USE_ONBOARD_LEDS; |
|
staticParams.naviMode = 0; // free. |
staticParams.airpressureWindowLength = 0; |
staticParams.airpressureDWindowLength = 24; |
|
staticParams.heightControlMaxIntegralIn = 125; |
staticParams.heightControlMaxIntegralOut = 75; |
220,7 → 258,7 |
staticParams.userParams[i] = i; |
} |
|
staticParams.bitConfig = CFG_GYRO_SATURATION_PREVENTION; |
staticParams.bitConfig = 0;//CFG_GYRO_SATURATION_PREVENTION; |
|
memcpy(staticParams.name, "Default\0", 6); |
} |
230,9 → 268,8 |
IMUConfig.gyroDWindowLength = 3; |
// IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.accFilterConstant = 10; |
IMUConfig.rateTolerance = 120; |
IMUConfig.gyroActivityDamping = 24; |
|
|
|
gyro_setDefaultParameters(); |
} |
|
239,59 → 276,71 |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/***************************************************/ |
void motorMixer_default(void) { // Quadro |
void outputMixer_default(void) { // Quadro |
uint8_t i; |
// clear mixer table (but preset throttle) |
for (i = 0; i < MAX_MOTORS; i++) { |
motorMixer.matrix[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
motorMixer.matrix[i][MIX_PITCH] = 0; |
motorMixer.matrix[i][MIX_ROLL] = 0; |
motorMixer.matrix[i][MIX_YAW] = 0; |
motorMixer.matrix[i][MIX_OPPOSITE_MOTOR] = (uint8_t)-1; |
for (i = 0; i < NUM_OUTPUTS; i++) { |
outputMixer[i].flightControls[MIXER_SOURCE_THROTTLE] = i < 4 ? (1<<LOG_MOTOR_MIXER_UNIT) : 0; |
outputMixer[i].flightControls[MIXER_SOURCE_PITCH] = 0; |
outputMixer[i].flightControls[MIXER_SOURCE_ROLL] = 0; |
outputMixer[i].flightControls[MIXER_SOURCE_YAW] = 0; |
outputMixer[i].oppositeMotor = (uint8_t)-1; |
outputMixer[i].auxSource = (uint8_t)-1; |
outputMixer[i].outputType = i < 4 ? OUTPUT_TYPE_MOTOR : OUTPUT_TYPE_UNDEFINED; |
outputMixer[i].minValue = i < 4 ? 1 : 128; |
outputMixer[i].maxValue = i < 4 ? 255 : 128; |
} |
/* |
// default = Quadro+ |
motorMixer.matrix[0][MIX_PITCH] = +64; |
motorMixer.matrix[0][MIX_YAW] = +64; |
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1; |
outputMixer[0].flightControls[MIXER_SOURCE_PITCH] = -(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[0].flightControls[MIXER_SOURCE_YAW] = +(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[0].oppositeMotor = 1; |
|
motorMixer.matrix[1][MIX_PITCH] = -64; |
motorMixer.matrix[1][MIX_YAW] = +64; |
motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0; |
outputMixer[1].flightControls[MIXER_SOURCE_PITCH] = +(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[1].flightControls[MIXER_SOURCE_YAW] = +(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[1].oppositeMotor = 0; |
|
outputMixer[2].flightControls[MIXER_SOURCE_ROLL] = +(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[2].flightControls[MIXER_SOURCE_YAW] = -(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[2].oppositeMotor = 3; |
|
motorMixer.matrix[2][MIX_ROLL] = -64; |
motorMixer.matrix[2][MIX_YAW] = -64; |
motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3; |
outputMixer[3].flightControls[MIXER_SOURCE_ROLL] = -(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[3].flightControls[MIXER_SOURCE_YAW] = -(1<<LOG_MOTOR_MIXER_UNIT); |
outputMixer[3].oppositeMotor = 2; |
|
motorMixer.matrix[3][MIX_ROLL] = +64; |
motorMixer.matrix[3][MIX_YAW] = -64; |
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2; |
outputMixer[8].outputType = OUTPUT_TYPE_SERVO; |
outputMixer[8].auxSource = MIXER_SOURCE_AUX_RCCHANNEL; |
outputMixer[8].minValue = 40; |
outputMixer[8].maxValue = 256-40; |
|
memcpy(motorMixer.name, "Quadro +\0", 9); |
*/ |
|
// default = Quadro |
motorMixer.matrix[0][MIX_PITCH] = +64; |
motorMixer.matrix[0][MIX_ROLL] = +64; |
motorMixer.matrix[0][MIX_YAW] = +64; |
outputMixer[9].outputType = OUTPUT_TYPE_SERVO; |
outputMixer[9].auxSource = MIXER_SOURCE_AUX_RCCHANNEL+1; |
outputMixer[9].minValue = 10; |
outputMixer[9].maxValue = 256-10; |
|
/* |
// default = Quadro X |
motorMixer.matrix[0][MIX_PITCH] = +(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[0][MIX_ROLL] = +(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[0][MIX_YAW] = +(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1; |
|
motorMixer.matrix[1][MIX_PITCH] = -64; |
motorMixer.matrix[1][MIX_ROLL] = -64; |
motorMixer.matrix[1][MIX_YAW] = +64; |
motorMixer.matrix[1][MIX_PITCH] = -(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[1][MIX_ROLL] = -(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[1][MIX_YAW] = +(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0; |
|
motorMixer.matrix[2][MIX_PITCH] = +64; |
motorMixer.matrix[2][MIX_ROLL] = -64; |
motorMixer.matrix[2][MIX_YAW] = -64; |
motorMixer.matrix[2][MIX_PITCH] = +(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[2][MIX_ROLL] = -(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[2][MIX_YAW] = -(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3; |
|
motorMixer.matrix[3][MIX_PITCH] = -64; |
motorMixer.matrix[3][MIX_ROLL] = +64; |
motorMixer.matrix[3][MIX_YAW] = -64; |
motorMixer.matrix[3][MIX_PITCH] = -(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[3][MIX_ROLL] = +(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[3][MIX_YAW] = -(1<<LOG_MOTOR_MIXER_SCALER); |
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2; |
|
memcpy(motorMixer.name, "Quadro X\0", 9); |
*/ |
} |
|
/***************************************************/ |
303,8 → 352,8 |
channelMap.channels[CH_ROLL] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
channelMap.channels[CH_YAW] = 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; |
channelMap.channels[CH_VARIABLES + 0] = 4; |
channelMap.channels[CH_VARIABLES + 1] = 5; |
channelMap.channels[CH_VARIABLES + 2] = 6; |
channelMap.channels[CH_VARIABLES + 3] = 7; |
} |