Subversion Repositories Projects

/*****************************************************************************
 *   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();
        }
}