0,0 → 1,241 |
#include <avr/boot.h> |
|
#include <avr/io.h> |
#include <avr/interrupt.h> |
|
#include "main.h" |
#include "timer0.h" |
#include "uart0.h" |
#include "uart1.h" |
#include "fat16.h" |
#include "led.h" |
#include "menu.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "gps.h" |
#include "button.h" |
#include "logging.h" |
#include "settings.h" |
|
#ifdef USE_FOLLOWME |
int16_t UBat = 120; |
int16_t Zellenzahl = 0; |
int16_t PowerOn = 0; |
int16_t i = 0; |
int16_t delay = 0; |
uint16_t FOLLOWME_INTERVAL = 500; |
uint8_t CELLUNDERVOLTAGE = 33; |
uint8_t ToleranceRadius = 3; |
uint8_t FollowMeSpeed = 20; |
#endif |
|
uint16_t Error = 0; |
SysState_t SysState = STATE_UNDEFINED; |
|
int main (void) |
{ |
static uint16_t FollowMe_Timer = 0; |
|
// disable interrupts global |
//cli(); |
|
// disable watchdog |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
|
// initalize modules |
LED_Init(); |
LEDRED_ON; |
TIMER0_Init(); |
USART0_Init(); |
UBX_Init(); |
USART1_Init(); |
ADC_Init(); |
Button_Init(); |
// enable interrupts global |
sei(); |
|
// try to initialize the FAT 16 filesystem on the SD-Card |
Fat16_Init(); |
// initialize the settings |
Settings_Init(); |
// initialize logging (needs settings) |
Logging_Init(); |
|
LEDRED_OFF; |
LEDGRN_ON; |
|
//set Settings from settings.ini file |
Settings_GetParamValue(PID_FOME_INTERV, &(FOLLOWME_INTERVAL)); |
Settings_GetParamValue(PID_CELLUNDERVO, &(CELLUNDERVOLTAGE)); |
Settings_GetParamValue(PID_TOLERANCERA, &(ToleranceRadius)); |
Settings_GetParamValue(PID_FOLLOWSPEED, &(FollowMeSpeed)); |
|
#ifdef USE_SDLOGGER |
printf("\r\n\r\nHW: SD-Logger"); |
#endif |
#ifdef USE_FOLLOWME |
printf("\r\n\r\nHW: Follow-Me"); |
#endif |
printf("\r\nFollow Me\r\nSoftware:V%d.%d%c ",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\r\n------------------------------"); |
printf("\r\n"); |
|
|
BeepTime = 1000; |
|
Menu_Clear(); |
|
FollowMe_Timer = SetDelay(FOLLOWME_INTERVAL); |
|
while (1) |
{ |
// get gps data to update the follow me position |
GPS_Update(); |
DebugOut.Analog[10] = GPSData.NumOfSats; |
|
// update logging |
Logging_Update(); |
|
// check for button action and change state resectively |
if(GetButton()) |
{ |
BeepTime = 200; |
|
switch(SysState) |
{ |
case STATE_IDLE: |
if(!Error) |
{ |
SysState = STATE_SEND_FOLLOWME; // activate followme only of no error has occured |
} |
break; |
|
case STATE_SEND_FOLLOWME: |
SysState = STATE_IDLE; |
break; |
default: |
SysState = STATE_IDLE; |
break; |
} |
|
} |
|
// state machine |
DebugOut.Analog[9] = SysState; |
switch(SysState) |
{ |
case(STATE_SEND_FOLLOWME): |
{ |
if(CheckDelay(FollowMe_Timer)) // time for next message? |
{ |
if(FollowMe.Position.Status == NEWDATA) // if new |
{ // update remaining data |
FollowMe_Timer = SetDelay(FOLLOWME_INTERVAL); // reset timer |
|
FollowMe.Heading = -1; // invalid heading |
FollowMe.ToleranceRadius = ToleranceRadius; // 5 meter default |
FollowMe.HoldTime = 30; // go home after 60s without any update |
FollowMe.Event_Flag = 0; // no event |
FollowMe.Index = 1; // 2st wp |
//neu seit 0.28m |
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
FollowMe.Name [0] = "F"; // Name des Wegpunktes (ASCII) |
FollowMe.Name [1] = "o"; |
FollowMe.Name [2] = "M"; |
FollowMe.Name [3] = "e"; |
FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
FollowMe.AltitudeRate = 0; // rate to change the setpoint |
FollowMe.Speed = FollowMeSpeed; // rate to change the Position |
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
|
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
Request_SendFollowMe = 1; // triggers serial tranmission |
} |
else // no new position avalable (maybe bad gps signal condition) |
{ |
FollowMe_Timer = SetDelay(100); // reset timer on higer frequency |
} |
LEDGRN_TOGGLE; // indication of active follow me |
} |
}break; |
|
case(STATE_IDLE):LEDGRN_ON;break; |
|
default:SysState = STATE_IDLE;break; |
} |
|
// restart ADConversion if ready |
if(ADReady) |
{ |
DebugOut.Analog[0] = Adc0; |
DebugOut.Analog[1] = Adc1; |
DebugOut.Analog[2] = Adc2; |
DebugOut.Analog[3] = Adc3; |
DebugOut.Analog[4] = Adc4; |
DebugOut.Analog[5] = Adc5; |
DebugOut.Analog[6] = Adc6; |
DebugOut.Analog[7] = Adc7; |
|
#ifdef USE_FOLLOWME |
// AVcc = 5V --> 5V = 1024 counts |
// the voltage at the voltage divider reference point is 0.8V less that the UBat |
// because of the silicon diode inbetween. |
// voltage divider R2=10K, R3=3K9 |
// UAdc4 = R3/(R3+R2)*UBat= 3.9/(3.9+10)*UBat = UBat/3.564 |
UBat = (3 * UBat + (64 * Adc4) / 368) / 4; |
DebugOut.Analog[8] = UBat; |
|
// check for zellenzahl |
if(PowerOn < 100) |
{ |
if(UBat<=84) Zellenzahl = 2; |
else Zellenzahl = 3; |
PowerOn++; |
} |
DebugOut.Analog[11] = Zellenzahl; |
DebugOut.Analog[12] = PowerOn; |
|
//show recognised Zellenzahl to user |
if(i < Zellenzahl && PowerOn >= 100 && BeepTime == 0 && delay > 1000) |
{ |
BeepTime = 100; |
i++; |
delay = 0; |
} |
if(delay < 1500) delay++; |
|
|
// monitor battery undervoltage [...||(UBat<74) as temporary workaround to protect 2s lipo packs] |
if(UBat < Zellenzahl * CELLUNDERVOLTAGE) |
{ // sound for low battery |
BeepModulation = 0x0300; |
if(!BeepTime) |
{ |
BeepTime = 6000; // 0.6 seconds |
} |
Error |= ERROR_LOW_BAT; |
} |
else |
{ |
Error &= ~ERROR_LOW_BAT; |
} |
#endif |
ADReady = 0; |
ADC_Enable(); // restart ad conversion sequence |
} |
|
// serial communication |
USART0_ProcessRxData(); |
USART0_TransmitTxData(); |
|
// indicate error, blinking code tbd. |
if(Error)LEDRED_ON; |
else LEDRED_OFF; |
DebugOut.Analog[13] = Error; |
} |
return (1); |
} |
|