void serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
#ifdef USE_SERIALRX_SPEKTRUM
case SERIALRX_SPEKTRUM1024:
enabled = spektrumInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SPEKTRUM2048:
enabled = spektrumInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_SBUS
case SERIALRX_SBUS:
enabled = sbusInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_SUMD
case SERIALRX_SUMD:
enabled = sumdInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_SUMH
case SERIALRX_SUMH:
enabled = sumhInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_XBUS
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
enabled = xBusInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_IBUS
case SERIALRX_IBUS:
enabled = ibusInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_JETIEXBUS
case SERIALRX_JETIEXBUS:
enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
break;
#endif
default:
enabled = false;
break;
}
if (!enabled) {
featureClear(FEATURE_RX_SERIAL);
rxRuntimeConfig->rcReadRawFn = nullReadRawRC;
}
}
void serialRxInit(rxConfig_t *rxConfig)
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SBUS:
enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SUMD:
enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SUMH:
enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
}
if (!enabled) {
featureClear(FEATURE_RX_SERIAL);
rcReadRawFunc = nullReadRawRC;
}
}
void serialRxInit(rxConfig_t *rxConfig)
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
rxRefreshRate = 22000;
enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SPEKTRUM2048:
rxRefreshRate = 11000;
enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SBUS:
rxRefreshRate = 11000;
enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SUMD:
rxRefreshRate = 11000;
enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SUMH:
rxRefreshRate = 11000;
enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
rxRefreshRate = 11000;
enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_IBUS:
rxRefreshRate = 20000; // TODO - Verify speed
enabled = ibusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
}
if (!enabled) {
featureClear(FEATURE_RX_SERIAL);
rcReadRawFunc = nullReadRawRC;
}
}
void receiverCLI()
{
char rcOrderString[9];
float tempFloat;
uint8_t index;
uint8_t receiverQuery = 'x';
uint8_t validQuery = false;
NVIC_InitTypeDef NVIC_InitStructure;
cliBusy = true;
cliPortPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPortPrint("Receiver CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliPortRead();
cliPortPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPortPrint("\nReceiver Type: ");
switch(systemConfig.receiverType)
{
case PPM:
cliPortPrint("PPM\n");
break;
case SPEKTRUM:
cliPortPrint("Spektrum\n");
break;
}
cliPortPrint("Current RC Channel Assignment: ");
for (index = 0; index < 8; index++)
rcOrderString[systemConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPortPrint(rcOrderString); cliPortPrint("\n");
cliPortPrintF("Secondary Spektrum: ");
if ((systemConfig.slaveSpektrum == true) && false) // HJI Inhibit Slave Spektrum on Naze32 Pro
cliPortPrintF("Installed\n");
else
cliPortPrintF("Uninstalled\n");
cliPortPrintF("Mid Command: %4ld\n", (uint16_t)systemConfig.midCommand);
cliPortPrintF("Min Check: %4ld\n", (uint16_t)systemConfig.minCheck);
cliPortPrintF("Max Check: %4ld\n", (uint16_t)systemConfig.maxCheck);
cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)systemConfig.minThrottle);
cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)systemConfig.maxThrottle);
tempFloat = systemConfig.rollAndPitchRateScaling * 180000.0 / PI;
cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = systemConfig.yawRateScaling * 180000.0 / PI;
cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat);
cliPortPrintF("Roll Rate Cmd Tau: %6.2f\n", systemConfig.rollRateCmdLowPassTau);
cliPortPrintF("Pitch Rate Cmd Tau: %6.2f\n\n", systemConfig.pitchRateCmdLowPassTau);
tempFloat = systemConfig.attitudeScaling * 180000.0 / PI;
cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat);
cliPortPrintF("Roll Attitude Cmd Tau: %6.2f\n", systemConfig.rollAttCmdLowPassTau);
cliPortPrintF("Pitch Attitude Cmd Tau: %6.2f\n\n", systemConfig.pitchAttCmdLowPassTau);
cliPortPrintF("Arm Delay Count: %3d Frames\n", systemConfig.armCount);
cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", systemConfig.disarmCount);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Toggle PPM/Spektrum Satellite Receiver
if (systemConfig.receiverType == PPM)
{
NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE);
systemConfig.receiverType = SPEKTRUM;
spektrumInit();
}
else
{
NVIC_InitStructure.NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE);
systemConfig.receiverType = PPM;
ppmRxInit();
}
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 8 );
parseRcChannels( rcOrderString );
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Toggle Slave Spektrum State
// HJI Inhibit Slave Spektrum on Naze32 Pro if (systemConfig.slaveSpektrum == true)
systemConfig.slaveSpektrum = false;
// HJI Inhibit Slave Spektrum on Naze32 Pro else
// HJI Inhibit Slave Spektrum on Naze32 Pro systemConfig.slaveSpektrum = true;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read RC Control Points
systemConfig.midCommand = readFloatCLI();
systemConfig.minCheck = readFloatCLI();
systemConfig.maxCheck = readFloatCLI();
systemConfig.minThrottle = readFloatCLI();
systemConfig.maxThrottle = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read Arm/Disarm Counts
systemConfig.armCount = (uint8_t)readFloatCLI();
systemConfig.disarmCount = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Max Rate Value
systemConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI;
systemConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Max Attitude Value
systemConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'H': // Read Rate Cmd Tau Value
systemConfig.rollRateCmdLowPassTau = readFloatCLI();
systemConfig.pitchRateCmdLowPassTau = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'I': // Read Attitude Cmd Tau Value
systemConfig.rollAttCmdLowPassTau = readFloatCLI();
systemConfig.pitchAttCmdLowPassTau = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write System EEPROM Parameters
cliPortPrint("\nWriting System EEPROM Parameters....\n\n");
writeSystemEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n");
cliPortPrint(" 'B' Set RC Control Order BTAER1234\n");
cliPortPrint(" 'C' Toggle Slave Spektrum State\n");
cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n");
cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n");
cliPortPrint(" 'G' Set Maximum Attitude Command\n");
cliPortPrint(" 'H' Set Roll/Pitch Rate Command Filters HROLL;PITCH\n");
cliPortPrint(" 'I' Set Roll/Pitch Att Command Filters IROLL;PITCH\n");
cliPortPrint(" 'W' Write System EEPROM Parameters\n");
cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
bool sensorsOK = false;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t* loopbackPort1 = NULL;
serialPort_t* loopbackPort2 = NULL;
#endif
initEEPROM();
checkFirstTime(false);
readEEPROM();
systemInit(mcfg.emf_avoidance);
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
#endif
activateConfig();
// configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
initBoardAlignment();
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect();
// production debug output
#ifdef PROD_DEBUG
productionDebug();
#endif
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3);
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
imuInit(); // Mag is initialized inside imuInit
mixerInit(); // this will set core.useServo var depending on mixer type
serialInit(mcfg.serial_baudrate);
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = mcfg.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = mcfg.midrc;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
case 9:
pwm_params.adcChannel = PWM8;
break;
default:
pwm_params.adcChannel = 0;
break;
}
pwmInit(&pwm_params);
core.numServos = pwm_params.numServos;
// configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(&rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(&rcReadRawFunc);
break;
case SERIALRX_SUMD:
sumdInit(&rcReadRawFunc);
break;
case SERIALRX_MSP:
mspInit(&rcReadRawFunc);
break;
}
} else { // spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
// gpsInit will return if FEATURE_GPS is not enabled.
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
if (feature(FEATURE_SOFTSERIAL)) {
//mcfg.softserial_baudrate = 19200; // Uncomment to override config value
setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted);
setupSoftSerialSecondary(mcfg.softserial_2_inverted);
#ifdef SOFTSERIAL_LOOPBACK
loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n");
loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]);
serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n");
#endif
//core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial.
}
if (feature(FEATURE_TELEMETRY))
initTelemetry();
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLE = 1;
// loopy
while (1) {
loop();
#ifdef SOFTSERIAL_LOOPBACK
if (loopbackPort1) {
while (serialTotalBytesWaiting(loopbackPort1)) {
uint8_t b = serialRead(loopbackPort1);
serialWrite(loopbackPort1, b);
//serialWrite(core.mainport, 0x01);
//serialWrite(core.mainport, b);
};
}
if (loopbackPort2) {
while (serialTotalBytesWaiting(loopbackPort2)) {
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
serialRead(loopbackPort2);
#else
uint8_t b = serialRead(loopbackPort2);
serialWrite(loopbackPort2, b);
//serialWrite(core.mainport, 0x02);
//serialWrite(core.mainport, b);
#endif // OLIMEXINO
};
}
#endif
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
#if 0
// PC12, PA15
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
GPIOA->BRR = 0x8000; // set low 15
GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
GPIOC->BRR = 0x1000; // set low 12
#endif
#if 0
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOB->BRR = 0x18; // set low 4 & 3
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif
systemInit();
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
#endif
checkFirstTime(false);
readEEPROM();
// configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
mixerInit(); // this will set useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
case 9:
pwm_params.adcChannel = PWM8;
break;
default:
pwm_params.adcChannel = 0;
break;
}
pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum below will override that
rcReadRawFunc = pwmReadRawRC;
if (feature(FEATURE_SPEKTRUM)) {
spektrumInit();
rcReadRawFunc = spektrumReadRawRC;
} else {
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
if (feature(FEATURE_GPS))
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
serialInit(mcfg.serial_baudrate);
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
// loopy
while (1) {
loop();
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
#if 0
// PC12, PA15
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
GPIOA->BRR = 0x8000; // set low 15
GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
GPIOC->BRR = 0x1000; // set low 12
#endif
#if 0
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOB->BRR = 0x18; // set low 4 & 3
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif
systemInit();
readEEPROM();
checkFirstTime(false);
serialInit(cfg.serial_baudrate);
// We have these sensors
#ifndef FY90Q
// AfroFlight32
sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG);
#else
// FY90Q
sensorsSet(SENSOR_ACC);
#endif
mixerInit(); // this will set useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = cfg.motor_pwm_rate;
pwm_params.servoPwmRate = cfg.servo_pwm_rate;
pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum will override that
rcReadRawFunc = pwmReadRawRC;
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
if (feature(FEATURE_SPEKTRUM)) {
spektrumInit();
rcReadRawFunc = spektrumReadRawRC;
} else {
// spektrum and GPS are mutually exclusive
// Optional GPS - available only when using PPM, otherwise required pins won't be usable
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_GPS))
gpsInit(cfg.gps_baudrate);
#ifdef SONAR
if (feature(FEATURE_SONAR))
Sonar_init();
#endif
}
}
previousTime = micros();
if (cfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
f.SMALL_ANGLES_25 = 1;
// loopy
while (1) {
loop();
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
serialPort_t* loopbackPort = NULL;
systemInit();
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
#endif
checkFirstTime(false);
readEEPROM();
// configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
initBoardAlignment();
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
mixerInit(); // this will set core.useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
//pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
pwm_params.useUART = feature(FEATURE_GPS);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = mcfg.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = mcfg.midrc;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
case 9:
pwm_params.adcChannel = PWM8;
break;
default:
pwm_params.adcChannel = 0;
break;
}
pwmInit(&pwm_params);
// configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(&rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(&rcReadRawFunc);
break;
case SERIALRX_SUMD:
sumdInit(&rcReadRawFunc);
break;
#if defined(SKYROVER)
case SERIALRX_HEXAIRBOT:
hexairbotInit(&rcReadRawFunc);
break;
#endif
}
} else { // spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
// gpsInit will return if FEATURE_GPS is not enabled.
// Sanity check below - protocols other than NMEA do not support baud rate autodetection
if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
mcfg.gps_baudrate = 0;
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
serialInit(mcfg.serial_baudrate);
DEBUG_PRINT("Booting..\r\n");
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
if (feature(FEATURE_SOFTSERIAL)) {
setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted);
#ifdef SOFTSERIAL_LOOPBACK
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n");
#endif
}
if (feature(FEATURE_TELEMETRY))
initTelemetry();
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
DEBUG_PRINT("Start\r\n");
// loopy
while (1) {
loop();
#ifdef SOFTSERIAL_LOOPBACK
if (loopbackPort) {
while (serialTotalBytesWaiting(loopbackPort)) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
//serialWrite(core.mainport, b);
};
}
#endif
}
}
void systemInit(void)
{
RCC_ClocksTypeDef rccClocks;
///////////////////////////////////
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
// Turn on peripherial clocks
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); // USART1, USART2
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE); // ADC2
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); // PPM RX
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM ESC Out 1 & 2
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM ESC Out 5 & 6
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM Servo Out 1, 2, 3, & 4
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); // 500 Hz dt Counter
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); // 100 Hz dt Counter
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15, ENABLE); // PWM ESC Out 3 & 4
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // RangeFinder PWM
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // Spektrum Frame Sync
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); // GPS
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX
///////////////////////////////////////////////////////////////////////////
spiInit(SPI2);
///////////////////////////////////
checkSensorEEPROM(false);
checkSystemEEPROM(false);
readSensorEEPROM();
readSystemEEPROM();
///////////////////////////////////
if (systemConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
///////////////////////////////////
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
///////////////////////////////////
gpsPortClearBuffer = &uart2ClearBuffer;
gpsPortNumCharsAvailable = &uart2NumCharsAvailable;
gpsPortPrintBinary = &uart2PrintBinary;
gpsPortRead = &uart2Read;
telemPortAvailable = &uart1Available;
telemPortPrint = &uart1Print;
telemPortPrintF = &uart1PrintF;
telemPortRead = &uart1Read;
///////////////////////////////////
initMixer();
usbInit();
gpioInit();
uart1Init();
uart2Init();
LED0_OFF;
delay(10000); // 10 seconds of 20 second delay for sensor stabilization
checkUsbActive();
///////////////////////////////////
#ifdef __VERSION__
cliPortPrintF("\ngcc version " __VERSION__ "\n");
#endif
cliPortPrintF("\nFF32mini Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __FF32MINI_VERSION);
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
cliPortPrint("\nRunning on external HSE clock....\n");
}
else
{
cliPortPrint("\nERROR: Running on internal HSI clock....\n");
}
RCC_GetClocksFreq(&rccClocks);
cliPortPrintF("\nHCLK-> %2d MHz\n", rccClocks.HCLK_Frequency / 1000000);
cliPortPrintF( "PCLK1-> %2d MHz\n", rccClocks.PCLK1_Frequency / 1000000);
cliPortPrintF( "PCLK2-> %2d MHz\n", rccClocks.PCLK2_Frequency / 1000000);
cliPortPrintF( "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);
if (systemConfig.receiverType == PPM)
cliPortPrint("Using PPM Receiver....\n\n");
else
cliPortPrint("Using Spektrum Satellite Receiver....\n\n");
initUBLOX();
delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..
///////////////////////////////////
adcInit();
aglInit();
pwmServoInit();
if (systemConfig.receiverType == SPEKTRUM)
spektrumInit();
else
ppmRxInit();
timingFunctionsInit();
batteryInit();
initFirstOrderFilter();
initMavlink();
initPID();
LED0_ON;
initMPU6000();
initMag();
initPressure();
}
void systemInit(void)
{
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
// Turn on peripherial clocks
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); // USART1
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE); // ADC2
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM Servo Out 1 & 2
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM ESC Out 3 & 4
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM ESC Out 5,6,7, & 8
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); // 500 Hz dt Counter
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); // 100 Hz dt Counter
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15, ENABLE); // PWM ESC Out 1 & 2
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // PPM RX
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // Spektrum Frame Sync
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX
///////////////////////////////////////////////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
cliInit();
gpioInit();
telemetryInit();
adcInit();
LED0_OFF;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
LED0_ON;
batteryInit();
pwmServoInit(eepromConfig.servoPwmRate);
if (eepromConfig.receiverType == SPEKTRUM)
spektrumInit();
else
ppmRxInit();
spiInit(SPI2);
timingFunctionsInit();
initFirstOrderFilter();
initPID();
initMPU6000();
initMag();
initPressure();
}
int main(void) {
// Setup pins
DDRB = 0x00; // All pins inputs
DDRC = 0x07; // PC0,1,2 as outputs for the LEDs
DDRD = 0x0b; // PD0 as output for LED, PD1 for TXD, PD3 for buzzer
PORTD = 0xe0; // Pullups for switches
PORTB = 0x3f; // Pullups for switches
// LEDs
LEDOff();
// ADC
ADMUX = 0x4f; // start off with mux on internal input
ADCSRA = 0x80; // ADC EN
DIDR0 = 0x38; // disable digital buffers on ADC pins
// Timer
TCCR0A = 0x00; // Normal mode
TCCR0B = 0x03; // prescaler at 64 results in 125kHz ticks
// UART
cli(); // disable global interrupts
stickInit(); // initialise stick input
spektrumInit();
// Get startup mode
trainerPlugged = TRAINERPIN;
bindSwitch = BINDPIN;
transmitMode = 0;
if(trainerPlugged == 0) transmitMode = eTM_trainer_master; // fixme or trainer slave, if PPM signal present
if(bindSwitch == 0) transmitMode = eTM_bind;
// Timer loop!
static uint8_t fastScaler = 255;
static uint8_t slowScaler = 255;
static uint16_t secondScaler = 0xffff;
// Buzzer
TCCR2B = 0x03; // prescaler at 64
stopNote();
noteBuffer[0] = NOTE6G;
noteBuffer[1] = NOTE6GS;
noteBuffer[2] = NOTESTOP;
noteCounter = 0;
noteInterruptable = 0;
while(1) {
while(TCNT0 < FASTLOOPCOUNT)
{
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED0Duty/3) LED0On(); // red LED too bright, reduce duty
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED1Duty) LED1On();
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED2Duty) LED2On();
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED3Duty) LED3On();
}
TCNT0 = 0; // reduce jitter by resetting soon (SPEKTRUM does not like jitter!)
LEDOff();
// should be going at about 625Hz
getDigital();
stickGetRawADC(rawstickvalues);
if(++fastScaler >= OVERSAMPLE) //should be 22ms for DSMX (Nano Board can not handle it faster!!)
{
fastScaler = 0;
// this loop runs slower than 50Hz
fastLoop();
/*throVoltage = 0;
ruddVoltage = 0;
elevVoltage = 0;
aileVoltage = 0;*/
rawstickvalues[0]=0;
rawstickvalues[1]=0;
rawstickvalues[2]=0;
rawstickvalues[3]=0;
if(++slowScaler >= 6)
{ // should be going at about 10Hz
slowScaler = 0;
slowLoop();
}
}
// we are here every 2ms
if(++secondScaler >=500)
{
// every second
secondScaler=0;
IdleSeconds++;
if(auxSwitch && mixToggle)
{
FlySeconds++;
if(FlySeconds == FLYSECONDS_MAX)
oneTone(NOTE7B);
if(FlySeconds == FLYSECONDS_MAX+10)
twoTone(NOTE7C);
if(IdleSeconds >= FLYSECONDS_MAX*2)
{
twoTone(NOTE7C);
}
}
}
}
}
void receiverCLI()
{
char rcOrderString[9];
float tempFloat;
uint8_t index;
uint8_t receiverQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPortPrint("Receiver CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliPortRead();
cliPortPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPortPrint("\nReceiver Type: ");
switch(eepromConfig.receiverType)
{
case PPM:
cliPortPrint("PPM\n");
break;
case SPEKTRUM:
cliPortPrint("Spektrum\n");
break;
}
cliPortPrint("Current RC Channel Assignment: ");
for (index = 0; index < 8; index++)
rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPortPrint(rcOrderString); cliPortPrint("\n");
#if defined(STM32F40XX)
cliPortPrintF("Secondary Spektrum: ");
if (eepromConfig.slaveSpektrum == true)
cliPortPrintF("Installed\n");
else
cliPortPrintF("Uninstalled\n");
#endif
cliPortPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand);
cliPortPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck);
cliPortPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck);
cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle);
cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle);
tempFloat = eepromConfig.rollAndPitchRateScaling * 180000.0 / PI;
cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.yawRateScaling * 180000.0 / PI;
cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat);
tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI;
cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat);
cliPortPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount);
cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Toggle PPM/Spektrum Satellite Receiver
if (eepromConfig.receiverType == PPM)
{
#if defined(STM32F30X)
TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE);
#endif
#if defined(STM32F40XX)
TIM_ITConfig(TIM1, TIM_IT_CC4, DISABLE);
#endif
eepromConfig.receiverType = SPEKTRUM;
spektrumInit();
}
else
{
#if defined(STM32F30X)
TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE);
#endif
#if defined(STM32F40XX)
TIM_ITConfig(TIM6, TIM_IT_Update, DISABLE);
#endif
eepromConfig.receiverType = PPM;
#if defined(STM32F30X)
ppmRxInit();
#endif
#if defined(STM32F40XX)
agl_ppmRxInit();
#endif
}
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 8 );
parseRcChannels( rcOrderString );
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
#if defined(STM32F40XX)
case 'C': // Toggle Slave Spektrum State
if (eepromConfig.slaveSpektrum == true)
eepromConfig.slaveSpektrum = false;
else
eepromConfig.slaveSpektrum = true;
receiverQuery = 'a';
validQuery = true;
break;
#endif
///////////////////////////
case 'D': // Read RC Control Points
eepromConfig.midCommand = readFloatCLI();
eepromConfig.minCheck = readFloatCLI();
eepromConfig.maxCheck = readFloatCLI();
eepromConfig.minThrottle = readFloatCLI();
eepromConfig.maxThrottle = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read Arm/Disarm Counts
eepromConfig.armCount = (uint8_t)readFloatCLI();
eepromConfig.disarmCount = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Max Rate Value
eepromConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI;
eepromConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Max Attitude Value
eepromConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n");
cliPortPrint(" 'B' Set RC Control Order BTAER1234\n");
#if defined(STM32F40XX)
cliPortPrint(" 'C' Toggle Slave Spektrum State\n");
#endif
cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n");
cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n");
cliPortPrint(" 'G' Set Maximum Attitude Command\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void radioInit(void) {
uint16_t radioType = (uint16_t)p[RADIO_TYPE];
int i;
AQ_NOTICE("Radio init\n");
memset((void *)&radioData, 0, sizeof(radioData));
radioData.mode = (radioType>>12) & 0x0f;
for (i = 0; i < RADIO_NUM; i++) {
radioInstance_t *r = &radioData.radioInstances[i];
USART_TypeDef *uart;
// determine UART
switch (i) {
case 0:
uart = RC1_UART;
break;
#ifdef RC2_UART
case 1:
uart = RC2_UART;
break;
#endif
#ifdef RC3_UART
case 2:
uart = RC3_UART;
break;
#endif
default:
uart = 0;
break;
}
r->radioType = (radioType>>(i*4)) & 0x0f;
r->channels = &radioData.allChannels[RADIO_MAX_CHANNELS * i];
utilFilterInit(&r->qualityFilter, (1.0f / 50.0f), 0.75f, 0.0f);
switch (r->radioType) {
case RADIO_TYPE_SPEKTRUM11:
case RADIO_TYPE_SPEKTRUM10:
case RADIO_TYPE_DELTANG:
if (uart) {
spektrumInit(r, uart);
radioRCSelect(i, 0);
AQ_PRINTF("Spektrum on RC port %d\n", i);
}
break;
case RADIO_TYPE_SBUS:
if (uart) {
futabaInit(r, uart);
radioRCSelect(i, 1);
AQ_PRINTF("Futaba on RC port %d\n", i);
}
break;
case RADIO_TYPE_PPM:
ppmInit(r);
AQ_PRINTF("PPM on RC port %d\n", i);
break;
case RADIO_TYPE_SUMD:
if (uart) {
grhottInit(r, uart);
radioRCSelect(i, 0);
AQ_PRINTF("GrHott on RC port %d\n", i);
}
break;
case RADIO_TYPE_MLINK:
if (uart) {
mlinkrxInit(r, uart);
radioRCSelect(i, 0);
AQ_PRINTF("Mlink on RC port %d\n", i);
}
break;
case RADIO_TYPE_NONE:
break;
default:
AQ_NOTICE("WARNING: Invalid radio type!\n");
break;
}
}
switch (radioData.mode) {
case RADIO_MODE_DIVERSITY:
// select first available radio to start with
for (i = 0; i < RADIO_NUM; i++) {
if (radioData.radioInstances[i].radioType > RADIO_TYPE_NONE) {
radioMakeCurrent(&radioData.radioInstances[i]);
break;
}
}
break;
case RADIO_MODE_SPLIT:
radioMakeCurrent(&radioData.radioInstances[0]);
break;
}
// set mode default
radioData.channels[(int)p[RADIO_FLAP_CH]] = -700;
radioTaskStack = aqStackInit(RADIO_STACK_SIZE, "RADIO");
radioData.radioTask = CoCreateTask(radioTaskCode, (void *)0, RADIO_PRIORITY, &radioTaskStack[RADIO_STACK_SIZE-1], RADIO_STACK_SIZE);
}
