void generateThrottleCurve(void)
{
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
uint8_t y = 1;
if (tmp > 0)
y = 100 - currentControlRateProfile->thrMid8;
if (tmp < 0)
y = currentControlRateProfile->thrMid8;
lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = motorAndServoConfig()->minthrottle + (int32_t) (motorAndServoConfig()->maxthrottle - motorAndServoConfig()->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
}
}
void mixerResetDisarmedMotors(void)
{
int i;
// set disarmed motor values
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
motor_disarmed[i] = feature(FEATURE_3D) ? motor3DConfig()->neutral3d : motorAndServoConfig()->mincommand;
}
void mixTable(void)
{
uint32_t i;
bool isFailsafeActive = failsafeIsActive();
if (motorCount >= 4 && mixerConfig()->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[FD_YAW] = constrain(axisPID[FD_YAW], -mixerConfig()->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig()->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
if (rcModeIsActive(BOXAIRMODE)) {
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
int16_t rollPitchYawMixMin = 0;
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] =
axisPID[FD_PITCH] * currentMixer[i].pitch +
axisPID[FD_ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw;
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
}
// Scale roll/pitch/yaw uniformly to fit within throttle range
int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
int16_t throttleRange, throttle;
int16_t throttleMin, throttleMax;
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions in 3D.
// Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcData[THROTTLE] <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
throttleMax = motor3DConfig()->deadband3d_low;
throttleMin = motorAndServoConfig()->minthrottle;
throttlePrevious = throttle = rcData[THROTTLE];
} else if (rcData[THROTTLE] >= (rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
throttleMax = motorAndServoConfig()->maxthrottle;
throttleMin = motor3DConfig()->deadband3d_high;
throttlePrevious = throttle = rcData[THROTTLE];
} else if ((throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = throttleMax = motor3DConfig()->deadband3d_low;
throttleMin = motorAndServoConfig()->minthrottle;
} else { // Deadband handling from positive to negative
throttleMax = motorAndServoConfig()->maxthrottle;
throttle = throttleMin = motor3DConfig()->deadband3d_high;
}
} else {
throttle = rcCommand[THROTTLE];
throttleMin = motorAndServoConfig()->minthrottle;
throttleMax = motorAndServoConfig()->maxthrottle;
}
throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
float mixReduction = (float) throttleRange / rollPitchYawMixRange;
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = lrintf((float) rollPitchYawMix[i] * mixReduction);
}
// Get the maximum correction by setting throttle offset to center.
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
} else {
motorLimitReached = false;
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
if (isFailsafeActive) {
motor[i] = mixConstrainMotorForFailsafeCondition(i);
} else if (feature(FEATURE_3D)) {
if (throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)) {
motor[i] = constrain(motor[i], motorAndServoConfig()->minthrottle, motor3DConfig()->deadband3d_low);
} else {
motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorAndServoConfig()->maxthrottle);
}
} else {
motor[i] = constrain(motor[i], motorAndServoConfig()->minthrottle, motorAndServoConfig()->maxthrottle);
}
}
} else {
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[FD_PITCH] * currentMixer[i].pitch +
axisPID[FD_ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw;
}
// Find the maximum motor output.
int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) {
// If one motor is above the maxthrottle threshold, we reduce the value
// of all motors by the amount of overshoot. That way, only one motor
// is at max and the relative power of each motor is preserved.
if (motor[i] > maxMotor) {
maxMotor = motor[i];
}
}
int16_t maxThrottleDifference = 0;
if (maxMotor > motorAndServoConfig()->maxthrottle) {
maxThrottleDifference = maxMotor - motorAndServoConfig()->maxthrottle;
}
for (i = 0; i < motorCount; i++) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxThrottleDifference;
if (feature(FEATURE_3D)) {
if (mixerConfig()->pid_at_min_throttle
|| rcData[THROTTLE] <= rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle
|| rcData[THROTTLE] >= rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle) {
if (rcData[THROTTLE] > rxConfig()->midrc) {
motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorAndServoConfig()->maxthrottle);
} else {
motor[i] = constrain(motor[i], motorAndServoConfig()->mincommand, motor3DConfig()->deadband3d_low);
}
} else {
if (rcData[THROTTLE] > rxConfig()->midrc) {
motor[i] = motor3DConfig()->deadband3d_high;
} else {
motor[i] = motor3DConfig()->deadband3d_low;
}
}
} else {
if (isFailsafeActive) {
motor[i] = mixConstrainMotorForFailsafeCondition(i);
} else {
// If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
// do not spin the motors.
motor[i] = constrain(motor[i], motorAndServoConfig()->minthrottle, motorAndServoConfig()->maxthrottle);
if ((rcData[THROTTLE]) < rxConfig()->mincheck) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = motorAndServoConfig()->mincommand;
} else if (mixerConfig()->pid_at_min_throttle == 0) {
motor[i] = motorAndServoConfig()->minthrottle;
}
}
}
}
}
}
/* Disarmed for all mixers */
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
#if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS)
servoMixTable();
#endif
}
uint16_t mixConstrainMotorForFailsafeCondition(uint8_t motorIndex)
{
return constrain(motor[motorIndex], motorAndServoConfig()->mincommand, motorAndServoConfig()->maxthrottle);
}
void stopMotors(void)
{
writeAllMotors(feature(FEATURE_3D) ? motor3DConfig()->neutral3d : motorAndServoConfig()->mincommand);
delay(50); // give the timers and ESCs a chance to react.
}
static void applyMultirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (rcControlsConfig()->alt_hold_fast_change) {
// rapid alt changes
if (ABS(rcData[THROTTLE] - initialRawThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialRawThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorAndServoConfig()->minthrottle, motorAndServoConfig()->maxthrottle);
}
} else {
// slow alt changes, mostly used for aerial photography
if (ABS(rcData[THROTTLE] - initialRawThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcData[THROTTLE] - initialRawThrottleHold) / 2;
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorAndServoConfig()->minthrottle, motorAndServoConfig()->maxthrottle);
}
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(systemConfig()->emf_avoidance);
#endif
i2cSetOverclock(systemConfig()->i2c_highspeed);
systemInit();
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_1) {
ledInit(false);
} else {
ledInit(true);
}
#else
ledInit(false);
#endif
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(rxConfig());
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
serialInit(feature(FEATURE_SOFTSERIAL));
mixerInit(customMotorMixer(0));
#ifdef USE_SERVOS
mixerInitServos(customServoMixer(0));
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(batteryConfig()->currentMeterType);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = (
feature(FEATURE_CURRENT_METER)
&& batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC
);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = motorAndServoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = motorAndServoConfig()->servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = motorAndServoConfig()->motor_pwm_rate;
pwm_params.idlePulse = motorAndServoConfig()->mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = motor3DConfig()->neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit();
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
#ifdef DEBUG_PWM_CONFIGURATION
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
#endif
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#ifdef STM32F303xC
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPI3);
}
#else
spiInit(SPI3);
#endif
#endif
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment();
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit();
}
#endif
gyroSetSampleRate(imuConfig()->looptime, gyroConfig()->gyro_lpf, imuConfig()->gyroSync, imuConfig()->gyroSyncDenominator); // Set gyro sampling rate divider before initialization
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetLooptime);
#endif
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit();
mspSerialInit();
#ifdef USE_CLI
cliInit();
#endif
failsafeInit();
rxInit(modeActivationProfile()->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit();
navigationInit(pidProfile());
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit();
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(transponderConfig()->data);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit();
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
// Setup scheduler
schedulerInit();
setTaskEnabled(TASK_GYROPID, true);
rescheduleTask(TASK_GYROPID, imuConfig()->gyroSync ? targetLooptime - INTERRUPT_WAIT_TIME : targetLooptime);
setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
while (true) {
scheduler();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredStateForMotors) == requiredStateForMotors) {
stopMotors();
}
#ifdef TRANSPONDER
// prevent IR LEDs from burning out.
uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED;
if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) {
transponderIrDisable();
}
#endif
while (1);
}
// Default settings
STATIC_UNIT_TESTED void resetConf(void)
{
pgResetAll(MAX_PROFILE_COUNT);
setProfile(0);
pgActivateProfile(0);
setControlRateProfile(0);
featureClearAll();
featureSet(DEFAULT_RX_FEATURE);
#ifdef BOARD_HAS_VOLTAGE_DIVIDER
// only enable the VBAT feature by default if the board has a voltage divider otherwise
// the user may see incorrect readings and unexpected issues with pin mappings may occur.
featureSet(FEATURE_VBAT);
#endif
featureSet(FEATURE_FAILSAFE);
parseRcChannels("AETR1234", rxConfig());
featureSet(FEATURE_BLACKBOX);
#if defined(COLIBRI_RACE)
// alternative defaults settings for COLIBRI RACE targets
imuConfig()->looptime = 1000;
featureSet(FEATURE_ONESHOT125);
featureSet(FEATURE_LED_STRIP);
#endif
#ifdef SPRACINGF3EVO
featureSet(FEATURE_TRANSPONDER);
featureSet(FEATURE_RSSI_ADC);
featureSet(FEATURE_CURRENT_METER);
featureSet(FEATURE_TELEMETRY);
#endif
// alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets
#ifdef ALIENWII32
featureSet(FEATURE_RX_SERIAL);
featureSet(FEATURE_MOTOR_STOP);
# ifdef ALIENWIIF3
serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
batteryConfig()->vbatscale = 20;
# else
serialConfig()->portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
# endif
rxConfig()->serialrx_provider = SERIALRX_SPEKTRUM2048;
rxConfig()->spektrum_sat_bind = 5;
motorAndServoConfig()->minthrottle = 1000;
motorAndServoConfig()->maxthrottle = 2000;
motorAndServoConfig()->motor_pwm_rate = 32000;
imuConfig()->looptime = 2000;
pidProfile()->pidController = 3;
pidProfile()->P8[PIDROLL] = 36;
pidProfile()->P8[PIDPITCH] = 36;
failsafeConfig()->failsafe_delay = 2;
failsafeConfig()->failsafe_off_delay = 0;
currentControlRateProfile->rcRate8 = 130;
currentControlRateProfile->rates[ROLL] = 20;
currentControlRateProfile->rates[PITCH] = 20;
currentControlRateProfile->rates[YAW] = 100;
parseRcChannels("TAER1234", rxConfig());
*customMotorMixer(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixer(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*customMotorMixer(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*customMotorMixer(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*customMotorMixer(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*customMotorMixer(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*customMotorMixer(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*customMotorMixer(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
#endif
// copy first profile into remaining profile
PG_FOREACH_PROFILE(reg) {
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
memcpy(reg->address + i * pgSize(reg), reg->address, pgSize(reg));
}
}
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
configureRateProfileSelection(i, i % MAX_CONTROL_RATE_PROFILE_COUNT);
}
}
uint16_t getCurrentMinthrottle(void)
{
return motorAndServoConfig()->minthrottle;
}
