Rev 1053 |
Blame |
Last modification |
View Log
| RSS feed
/*****************************************************************************
* Copyright (C) 2010 seb@exse.net *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
*****************************************************************************/
#include <avr/io.h>
#include <inttypes.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
#include <math.h>
#include "main.h"
#include "osd.h"
#include "lcd.h"
#include "timer.h"
#include "usart.h"
#include "mk-data-structs.h"
NaviData_t
*naviData
;
#define TIMEOUT 200 // 2 sec
void pwm
(void)
{
if (hardware
== FC
)
{
lcd_printp_at
(0, 3, PSTR
("Only with NC !"), 0);
timer
= 100;
while (timer
> 0);
return;
}
lcd_cls
();
SwitchToNC
();
mode
= 'O';
// disable debug...
// RS232_request_mk_data (0, 'd', 0);
uint8_t tmp_dat
;
tmp_dat
= 0;
SendOutData
('d', ADDRESS_ANY
, 1, &tmp_dat
, 1);
// request OSD Data from NC every 100ms
// RS232_request_mk_data (1, 'o', 100);
tmp_dat
= 10;
SendOutData
('o', ADDRESS_NC
, 1, &tmp_dat
, 1);
timer
= TIMEOUT
;
abo_timer
= ABO_TIMEOUT
;
lcd_printp_at
(0, 0, PSTR
("GPS Alt:"), 0);
lcd_printp_at
(0, 1, PSTR
("Bar Alt:"), 0);
lcd_printp_at
(0, 2, PSTR
("Distance:"), 0);
lcd_printp_at
(0, 3, PSTR
("Bearing:"), 0);
lcd_printp_at
(0, 4, PSTR
("V-Angle:"), 0);
uint16_t bearing
= 36;
do
{
if (rxd_buffer_locked
)
{
timer
= TIMEOUT
;
Decode64
();
naviData
= (NaviData_t
*) pRxData
;
GPS_Pos_t currpos
;
currpos.
Latitude = naviData
->CurrentPosition.
Latitude;
currpos.
Longitude = naviData
->CurrentPosition.
Longitude;
write_ndigit_number_u
(11, 2, naviData
->HomePositionDeviation.
Distance / 10, 3, 0);
lcd_putc
(14, 2, 'm', 0);
write_ndigit_number_u
(11, 3, naviData
->HomePositionDeviation.
Bearing, 3, 0);
lcd_putc
(14, 3, 'm', 0);
if (naviData
->Altimeter
> 300 || naviData
->Altimeter
< -300)
{
// above 10m only write full meters
write_ndigit_number_s
(10, 1, naviData
->Altimeter
/ 30, 4, 0);
}
else
{
// up to 10m write meters.dm
write_ndigit_number_s_10th
(10, 1, naviData
->Altimeter
/ 3, 3, 0);
}
lcd_putc
(14, 1, 'm', 0);
if (((naviData
->HomePosition.
Altitude - naviData
->CurrentPosition.
Altitude)/1000) > 100 || ((naviData
->HomePosition.
Altitude - naviData
->CurrentPosition.
Altitude)/1000) < -100)
{
// above 10m only write full meters
write_ndigit_number_s
(9, 0, (naviData
->HomePosition.
Altitude - naviData
->CurrentPosition.
Altitude) / 1000, 5, 0);
}
else
{
// up to 10m write meters.dm
write_ndigit_number_s_10th
(9, 0, (naviData
->HomePosition.
Altitude - naviData
->CurrentPosition.
Altitude) / 100, 4, 0);
}
lcd_putc
(14, 0, 'm', 0);
uint16_t deg
= atan2(naviData
->Altimeter
/ 3, naviData
->HomePositionDeviation.
Distance)*180/M_PI
;
if(deg
>90)deg
=90;
if(deg
<0)deg
=0;
write_ndigit_number_s
(11, 4, deg
, 4, 0);
// wenn hoeher 10 order weiter weg als 10
if((naviData
->Altimeter
> 300)||( naviData
->HomePositionDeviation.
Distance > 100))
{
set_pwm_b
(deg
);
}
// wenn weiter weg als 20
if ( naviData
->HomePositionDeviation.
Distance > 200)
{
//set_pwm_a(360-naviData->HomePositionDeviation.Bearing);
}
set_pwm_a
(360-bearing
);
write_ndigit_number_s
(7, 7, bearing
, 5, 0);
write_ndigit_number_s
(0, 7, OCR1A
, 5, 0);
write_ndigit_number_u
(0, 6, naviData
->UBat
, 4, 0);
uint8_t cell
= (naviData
->UBat
)/4;
if(naviData
->UBat
< 127)
cell
= (naviData
->UBat
)/3;
write_ndigit_number_u
(10, 6, cell
, 4, 0);
lcd_frect
((8*0), (8*5), (cell
-34)*16 , 6, 1);
rxd_buffer_locked
= FALSE
;
}
if (!abo_timer
)
{ // renew abo every 3 sec
// request OSD Data from NC every 100ms
// RS232_request_mk_data (1, 'o', 100);
tmp_dat
= 10;
SendOutData
('o', ADDRESS_NC
, 1, &tmp_dat
, 1);
abo_timer
= ABO_TIMEOUT
;
}
if (get_key_press
(1 << KEY_PLUS
))
{
bearing
+=36;
}
if (get_key_press
(1 << KEY_MINUS
))
{
bearing
-=36;
}
}
while (!get_key_press
(1 << KEY_ESC
) && timer
);
tmp_dat
= 0;
SendOutData
('o', ADDRESS_NC
, 1, &tmp_dat
, 1);
mode
= 0;
rxd_buffer_locked
= FALSE
;
if (!timer
)
{ // timeout occured
lcd_cls
();
lcd_printp_at
(0, 0, PSTR
("ERROR: no data"), 0);
timer
= 100;
while (timer
> 0);
pwm
();
}
}