void pulseMotors(uint8_t quantity)
{
uint8_t i;
for ( i = 0; i < quantity; i++ )
{
writeAllMotors( eepromConfig.minThrottle );
delay(250);
writeAllMotors( (float)MINCOMMAND );
delay(250);
}
}
void HardFault_Handler(void)
{
// fall out of the sky
writeAllMotors(masterConfig.escAndServoConfig.mincommand);
while (1);
}
void stopMotors(void)
{
writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand);
delay(50); // give the timers and ESCs a chance to react.
}
void HardFault_Handler(void)
{
// fall out of the sky
writeAllMotors(mcfg.mincommand);
while (1);
}
void initOutput() {
#if defined(TRACE)
Serial.println(">>Start initOutput");
#endif
for(uint8_t i=0;i<4;i++) {
pinMode(PWM_PIN[i],OUTPUT);
}
// Initialization TIMER2 80Mhz counting from 0 until 256
T2CON = 0x0030 ; // 0x0030=0000000000110000
// ON=0 (Timer is disable for now)
// FRZ=0 (Continue operation even when CPU is in Debug Exception mode)
// SIDL=0 (Continue operation even in Idle mode)
// TGATE=0, (Gated time accumulation is disabled)
// TCKPS=011 (1:8 prescale value) frequency 39KHz ??
// T32=0 (TMRx and TMRy form separate 16-bit timer)
// TCS=0 (Internal peripheral clock)
TMR2 = 0x0000; // start TIMER2 from 0...
PR2 = 0x0100; // ... until 256
// Configure the control register OC1CON for the output compare channel 1
OC1CON = 0; // clear OC1
OC1CONbits.OCM = 0b110; // OCM=110: PWM mode on OC1, Fault pin disable
// Configure the compare register OC1R and compare register secondary OC1RS for the output compare channel 1
OC1RS = 1500; // set buffered PWM duty cycle in counts,
// duty cycle is OC1RS/(PR2+1)
OC1R = 1500; // set initial PWM duty cycle in counts
// Configure the control register OC2CON for the output compare channel 2
OC2CON = 0; // clear OC2
OC2CONbits.OCM = 0b110; // OCM=110: PWM mode on OC2, Fault pin disable
// Configure the compare register OC2R and compare register secondary OC1RS for the output compare channel 2
OC2RS = 1500; // set buffered PWM duty cycle in counts,
// duty cycle is OC2RS/(PR2+1)
OC2R = 1500; // set initial PWM duty cycle in counts
// Configure the control register OC3CON for the output compare channel 3
OC3CON = 0; // clear OC3
OC3CONbits.OCM = 0b110; // OCM=110: PWM mode on OC3, Fault pin disable
// Configure the compare register OC3R and compare register secondary OC1RS for the output compare channel 3
OC3RS = 1500; // set buffered PWM duty cycle in counts,
// duty cycle is OC3RS/(PR2+1)
OC3R = 1500; // set initial PWM duty cycle in counts
// Configure the control register OC4CON for the output compare channel 4
OC4CON = 0; // clear OC4
OC4CONbits.OCM = 0b110; // OCM=110: PWM mode on OC4, Fault pin disable
// Configure the compare register OC4R and compare register secondary OC4RS for the output compare channel 4
OC4RS = 1500; // set buffered PWM duty cycle in counts,
// duty cycle is OC4RS/(PR2+1)
OC4R = 1500; // set initial PWM duty cycle in counts
// Enable Timer 2 and OCX
T2CONSET = 0x8000; // Enable Timer2
OC1CONSET = 0x8000; // Enable OC1
OC2CONSET = 0x8000; // Enable OC2
OC3CONSET = 0x8000; // Enable OC3
OC4CONSET = 0x8000; // Enable OC4
writeAllMotors(MINCOMMAND);
delay(300);
#if defined(TRACE)
Serial.println("<<End initOutput");
#endif
}
void stopMotors(void)
{
writeAllMotors(disarmMotorOutput);
delay(50); // give the timers and ESCs a chance to react.
}
void stopMotors(void)
{
writeAllMotors(calculateMotorOff());
delay(50); // give the timers and ESCs a chance to react.
}
void HardFault_Handler(void)
{
writeAllMotors(mcfg.mincommand);
while (1);
}
void escCalibration(void)
{
escCalibrating = true;
armed = false;
cliPrint("\nESC Calibration:\n\n");
cliPrint("!!!! CAUTION - Remove all propellers and disconnect !!!!\n");
cliPrint("!!!! flight battery before proceeding any further !!!!\n\n");
cliPrint("Type 'Y' to continue, anything other character exits\n\n");
while (cliAvailable() == false);
temp = cliRead();
if (temp != 'Y')
{
cliPrint("ESC Calibration Canceled!!\n\n");
escCalibrating = false;
return;
}
///////////////////////////////////
cliPrint("For ESC Calibration:\n");
cliPrint(" Enter 'h' for Max Command....\n");
cliPrint(" Enter 'm' for Mid Command....\n");
cliPrint(" Enter 'l' for Min Command....\n");
cliPrint(" Enter 'x' to exit....\n\n");
cliPrint("For Motor Order Verification:\n");
cliPrint(" Enter '0' to turn off all motors....\n");
cliPrint(" Enter '1' to turn on Motor1....\n");
cliPrint(" Enter '2' to turn on Motor2....\n");
cliPrint(" Enter '3' to turn on Motor3....\n");
cliPrint(" Enter '4' to turn on Motor4....\n");
cliPrint(" Enter '5' to turn on Motor5....\n");
cliPrint(" Enter '6' to turn on Motor6....\n");
cliPrint(" Enter '7' to turn on Motor7....\n");
cliPrint(" Enter '8' to turn on Motor8....\n\n");
///////////////////////////////////
while(true)
{
while (cliAvailable() == false);
temp = cliRead();
switch (temp)
{
case 'h':
cliPrint("Applying Max Command....\n\n");
writeAllMotors(eepromConfig.maxThrottle);
break;
case 'm':
cliPrint("Applying Mid Command....\n\n");
writeAllMotors(eepromConfig.midCommand);
break;
case 'l':
cliPrint("Applying Min Command....\n\n");
writeAllMotors(MINCOMMAND);
break;
case 'x':
cliPrint("Applying Min Command, Exiting Calibration....\n\n");
writeAllMotors(MINCOMMAND);
escCalibrating = false;
return;
break;
case '0':
cliPrint("Motors at Min Command....\n\n");
writeAllMotors(MINCOMMAND);
break;
case '1':
cliPrint("Motor1 at Min Throttle....\n\n");
pwmEscWrite(0, eepromConfig.minThrottle);
break;
case '2':
cliPrint("Motor2 at Min Throttle....\n\n");
pwmEscWrite(1, eepromConfig.minThrottle);
break;
case '3':
cliPrint("Motor3 at Min Throttle....\n\n");
pwmEscWrite(2, eepromConfig.minThrottle);
break;
case '4':
cliPrint("Motor4 at Min Throttle....\n\n");
pwmEscWrite(3, eepromConfig.minThrottle);
break;
case '5':
cliPrint("Motor5 at Min Throttle....\n\n");
pwmEscWrite(4, eepromConfig.minThrottle);
break;
case '6':
cliPrint("Motor6 at Min Throttle....\n\n");
pwmEscWrite(5, eepromConfig.minThrottle);
break;
case '7':
cliPrint("Motor7 at Min Throttle....\n\n");
pwmEscWrite(6, eepromConfig.minThrottle);
break;
case '8':
cliPrint("Motor8 at Min Throttle....\n\n");
pwmEscWrite(7, eepromConfig.minThrottle);
break;
case '?':
cliPrint("For ESC Calibration:\n");
cliPrint(" Enter 'h' for Max Command....\n");
cliPrint(" Enter 'm' for Mid Command....\n");
cliPrint(" Enter 'l' for Min Command....\n");
cliPrint(" Enter 'x' to exit....\n\n");
cliPrint("For Motor Order Verification:\n");
cliPrint(" Enter '0' to turn off all motors....\n");
cliPrint(" Enter '1' to turn on Motor1....\n");
cliPrint(" Enter '2' to turn on Motor2....\n");
cliPrint(" Enter '3' to turn on Motor3....\n");
cliPrint(" Enter '4' to turn on Motor4....\n");
cliPrint(" Enter '5' to turn on Motor5....\n");
cliPrint(" Enter '6' to turn on Motor6....\n");
cliPrint(" Enter '7' to turn on Motor7....\n");
cliPrint(" Enter '8' to turn on Motor8....\n\n");
break;
}
}
}
void setup() {
#if !defined(GPS_PROMINI)
UARTInit(115200);
#endif
LEDPIN_PINMODE;
SHIELDLED_PINMODE;
//POWERPIN_PINMODE;
//BUZZERPIN_PINMODE;
//STABLEPIN_PINMODE;
//POWERPIN_OFF;
/* Initialize GPIO (sets up clock) */
GPIOInit();
init_microsec();
enable_microsec();
init_timer16PWM();
enable_PWMtimer();
/******** special version of MultiWii to calibrate all attached ESCs ************/
#if defined(ESC_CALIB_CANNOT_FLY)
writeAllMotors(ESC_CALIB_HIGH);
delayMs(3000);
writeAllMotors(ESC_CALIB_LOW);
delayMs(500);
while (1) {
delayMs(5000);
blinkLED(2,20, 2);
}
// statement never reached
#endif
writeAllMotors(MINCOMMAND);
delayMs(300);
readEEPROM();
checkFirstTime();
configureReceiver();
#if defined(OPENLRSv2MULTI)
initOpenLRS();
#endif
initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)||defined(I2C_NAV)
GPS_set_pids();
#endif
previousTime = micros();
#if defined(GIMBAL)
calibratingA = 400;
#endif
calibratingG = 400;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER)
for(uint8_t i=0;i<=PMOTOR_SUM;i++)
pMeter[i]=0;
#endif
#if defined(ARMEDTIMEWARNING)
ArmedTimeWarningMicroSeconds = (ARMEDTIMEWARNING *1000000);
#endif
/************************************/
#if defined(GPS_SERIAL)
SerialOpen(GPS_SERIAL,GPS_BAUD);
delay(400);
for(uint8_t i=0;i<=5;i++){
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
if(!GPS_Present){
SerialEnd(GPS_SERIAL);
SerialOpen(0,SERIAL_COM_SPEED);
}
#if !defined(GPS_PROMINI)
GPS_Present = 1;
#endif
GPS_Enable = GPS_Present;
#endif
/************************************/
#if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)|| defined(I2C_NAV)
GPS_Enable = 1;
#endif
#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64)
initLCD();
#endif
#ifdef LCD_TELEMETRY_DEBUG
telemetry_auto = 1;
#endif
#ifdef LCD_CONF_DEBUG
configurationLoop();
#endif
#ifdef LANDING_LIGHTS_DDR
init_landing_lights();
#endif
#if defined(LED_FLASHER)
init_led_flasher();
led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
#endif
f.SMALL_ANGLES_25=1; // important for gyro only conf
//initialise median filter structures
#ifdef MEDFILTER
#ifdef SONAR
initMedianFilter(&SonarFilter, 5);
#endif
#endif
initWatchDog();
}
