| 0,0 → 1,205 |
|---|
| /***************************************************************************** |
| * 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(); |
| } |
| } |