Blame |
Last modification |
View Log
| RSS feed
#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);
}