2,216 → 2,152 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include <stdio.h> |
|
#include "timer0.h" |
#include "timer2.h" |
#include "uart0.h" |
#include "uart1.h" |
//#include "uart1.h" |
#include "output.h" |
#include "menu.h" |
#include "attitude.h" |
#include "commands.h" |
#include "flight.h" |
//#include "commands.h" |
//#include "flight.h" |
//#include "profiler.h" |
#include "rc.h" |
#include "analog.h" |
#include "configuration.h" |
#include "printf_P.h" |
#include "twimaster.h" |
#include "controlMixer.h" |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#include "eeprom.h" |
#include "beeper.h" |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#include "eeprom.h" |
|
int16_t main(void) { |
uint16_t timer; |
uint16_t timer; |
|
// disable interrupts global |
cli(); |
// disable interrupts global |
cli(); |
|
// analyze hardware environment |
setCPUType(); |
setBoardRelease(); |
// analyze hardware environment |
setCPUType(); |
setBoardRelease(); |
|
// disable watchdog |
MCUSR &= ~(1 << WDRF); |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
fdevopen(uart_putchar, NULL); |
|
// This is strange: It should NOT be necessarty to do. But the call of the same, |
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
channelMap_default(); |
// disable watchdog |
MCUSR &= ~(1 << WDRF); |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
|
// initalize modules |
output_init(); |
timer0_init(); |
timer2_init(); |
usart0_init(); |
//if (CPUType == ATMEGA644P);// usart1_Init(); |
RC_Init(); |
analog_init(); |
I2C_init(); |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
#endif |
// initalize modules |
output_init(); |
timer0_init(); |
// timer2_init(); |
usart0_init(); |
//if (CPUType == ATMEGA644P);// usart1_Init(); |
RC_Init(); |
I2C_init(); |
analog_init(); |
#ifdef USE_MK3MAG |
MK3MAG_init(); |
MK3MAG_init(); |
#endif |
#ifdef USE_DIRECT_GPS |
usart1_init(); |
usart1_init(); |
#endif |
|
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
motorMixer_readOrDefault(); |
paramSet_readOrDefault(); |
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
outputMixer_readOrDefault(); |
paramSet_readOrDefault(); |
|
// enable interrupts global |
sei(); |
// enable interrupts global |
sei(); |
|
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: Custom"); |
printf("\n\r CPU: Atmega644"); |
if (CPUType == ATMEGA644P) |
printf("p"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: Custom"); |
printf("\n\r CPU: Atmega644"); |
if (CPUType == ATMEGA644P) |
printf("p"); |
printf("\n\rSoftware: V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, |
VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
|
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
|
// Instead, while away the time by flashing the 2 outputs: |
// First J16, then J17. Makes it easier to see which is which. |
timer = setDelay(200); |
outputSet(0,1); |
GRN_OFF; |
RED_ON; |
while (!checkDelay(timer)) |
; |
// Instead, while away the time by flashing the 2 outputs: |
// First J16, then J17. Makes it easier to see which is which. |
timer = setDelay(200); |
output_setLED(0, 1); |
GRN_OFF; |
RED_ON; |
while (!checkDelay(timer)) |
; |
|
timer = setDelay(200); |
outputSet(0,0); |
outputSet(1,1); |
RED_OFF; |
GRN_ON; |
while (!checkDelay(timer)) |
; |
timer = setDelay(200); |
output_setLED(0, 0); |
output_setLED(1, 1); |
RED_OFF; |
GRN_ON; |
while (!checkDelay(timer)) |
; |
|
timer = setDelay(200); |
while (!checkDelay(timer)) |
; |
outputSet(1,0); |
GRN_OFF; |
timer = setDelay(200); |
while (!checkDelay(timer)) |
; |
output_setLED(1, 0); |
GRN_OFF; |
|
twi_diagnostics(); |
twi_diagnostics(); |
|
printf("\n\r==================================="); |
printf("\n\r==================================="); |
|
#ifdef USE_NAVICTRL |
printf("\n\rSupport for NaviCtrl"); |
printf("\n\rSupport for NaviCtrl"); |
#endif |
|
#ifdef USE_DIRECT_GPS |
printf("\n\rDirect (no NaviCtrl) navigation"); |
printf("\n\rDirect (no NaviCtrl) navigation"); |
#endif |
|
controlMixer_setNeutral(); |
controlMixer_setNeutral(); |
|
// Cal. attitude sensors and reset integrals. |
attitude_setNeutral(); |
I2CTimeout = 5000; |
|
// Init flight parameters |
// flight_setNeutral(); |
// Cal. attitude sensors and reset integrals. |
attitude_setNeutral(); |
|
beep(2000); |
// Init flight parameters |
// flight_setNeutral(); |
|
printf("\n\rControl: "); |
if (staticParams.bitConfig & CFG_HEADING_HOLD) |
printf("Heading Hold"); |
else printf("RTL Mode"); |
beep(2000); |
printf("\n\n\r"); |
|
printf("\n\n\r"); |
timer2_init(); |
|
/* |
* Main loop updates attitude and does some nonessential tasks |
* like beeping alarms and computing servo values. |
*/ |
while (1) { |
attitude_update(); |
|
LCD_clear(); |
// This is fair to leave here - servo values only need update after a change in attitude anyway. |
// calculateServoValues(); |
|
I2CTimeout = 5000; |
|
while (1) { |
if (runFlightControl) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
if (!analogDataReady) { |
// Analog data should have been ready but is not!! |
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
|
J4HIGH; |
// This is probably the correct order: |
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
// Flight control uses results from both. |
calculateFlightAttitude(); |
controlMixer_periodicTask(); |
commands_handleCommands(); |
flight_control(); |
J4LOW; |
|
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout |
if (!I2CTimeout) { |
I2C_Reset(); |
I2CTimeout = 5; |
} |
beepI2CAlarm(); |
} |
|
// Allow serial data transmission if there is still time, or if we are not flying anyway. |
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
usart0_transmitTxData(); |
} |
|
usart0_processRxData(); |
|
if (checkDelay(timer)) { |
if (UBat <= UBAT_AT_5V) { |
// Do nothing. The voltage on the input side of the regulator is <5V; |
// we must be running off USB power. Keep it quiet. |
} else if (UBat < staticParams.batteryVoltageWarning) { |
beepBatteryAlarm(); |
} |
|
#ifdef USE_NAVICTRL |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
timer = setDelay(20); // every 20 ms |
} |
output_update(); |
} |
|
#ifdef USE_NAVICTRL |
if(!SendSPI) { |
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
// within the SPI_TransmitByte() routine the value is set to 4. |
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
SPI_TransmitByte(); |
} |
#endif |
|
calculateServoValues(); |
|
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
} |
} |
return (1); |
if (UBat <= UBAT_AT_5V || UBat >= staticParams.batteryVoltageWarning) { |
// Do nothing. The voltage on the input side of the regulator is <5V; |
// we must be running off USB power. Keep it quiet. |
MKFlags &= ~MKFLAG_LOWBAT; |
} else { |
beepBatteryAlarm(); |
MKFlags |= MKFLAG_LOWBAT; |
} |
} |
return 1; |
} |