Subversion Repositories Projects

/*****************************************************************************

 *   Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net                  *

 *   based on the C-OSD code from CaScAdE                                    *

 *                http://www.mylifesucks.de/oss/c-osd/                       *

 *                                                                           *

 *   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 "main.h"

#include "osd.h"

#include "lcd.h"

#include "timer.h"

#include "usart.h"

#include "mk-data-structs.h"

#define COSD_WASFLYING                          4

/* ##########################################################################

 * global definitions and global vars

 * ##########################################################################*/

NaviData_t *naviData;

// stats for after flight

int16_t max_Altimeter = 0;

uint16_t max_GroundSpeed = 0;

int16_t max_Distance = 0;

uint8_t min_UBat = 255;

uint16_t max_FlyingTime = 0;

uint16_t max_Current = 0;

uint16_t max_Capacity = 0;

// cache old vars for blinking attribute, checkup is faster than full

// attribute write each time

volatile uint8_t last_UBat = 255;

volatile uint8_t last_RC_Quality = 255;

volatile uint16_t ftimer = 0;

// store stats description in progmem to save space

const char stats_item_0[] PROGMEM = "max Altitude:";

const char stats_item_1[] PROGMEM = "max Speed   :";

const char stats_item_2[] PROGMEM = "max Distance:";

const char stats_item_3[] PROGMEM = "min Voltage :";

const char stats_item_4[] PROGMEM = "max Time    :";

#if 1

const char stats_item_5[] PROGMEM = "max Current :";

const char stats_item_6[] PROGMEM = "UsedCapacity:";

#else

const char stats_item_5[] PROGMEM = "Long. :";

const char stats_item_6[] PROGMEM = "Lat.  :";

#endif

const char *stats_item_pointers[] PROGMEM = {

        stats_item_0,

        stats_item_1,

        stats_item_2,

        stats_item_3,

        stats_item_4,

        stats_item_5,

        stats_item_6

};

//char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W";

const char rose[48] PROGMEM = {

        0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S',

        0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O',

        0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N',

        0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W'};

// the center is char 19 (north), we add the current heading in 8th

// which would be 22.5 degrees, but float would bloat up the code

// and *10 / 225 would take ages... so we take the uncorrect way

const char str_NE[] PROGMEM = "NE";

const char str_E[] PROGMEM  = "E ";

const char str_SE[] PROGMEM = "SE";

const char str_S[] PROGMEM  = "S ";

const char str_SW[] PROGMEM = "SW";

const char str_W[] PROGMEM  = "W ";

const char str_NW[] PROGMEM = "NW";

const char str_N[] PROGMEM  = "N ";

const char *directions_p[8] PROGMEM = {

        str_NE,

        str_E,

        str_SE,

        str_S,

        str_SW,

        str_W,

        str_NW,

        str_N

};

// Flags

uint8_t COSD_FLAGS2 = 0;

/**

 * convert the <heading> gotton from NC into an index

 */

uint8_t heading_conv (uint16_t heading)

{

        if (heading > 23 && heading < 68)

        {

                return 0;               //direction = "NE";

        }

        else if (heading > 67 && heading < 113)

        {

                return 1;               //direction = "E ";

        }

        else if (heading > 112 && heading < 158)

        {

                return 2;               //direction = "SE";

        }

        else if (heading > 157 && heading < 203)

        {

                return 3;               //direction = "S ";

        }

        else if (heading > 202 && heading < 248)

        {

                return 4;               //direction = "SW";

        }

        else if (heading > 247 && heading < 293)

        {

                return 5;               //direction = "W ";

        }

        else if (heading > 292 && heading < 338)

        {

                return 6;               //direction = "NW";

        }

        return 7;       //direction = "N ";

}

/**

 * draw a compass rose at <x>/<y> for <heading>

 */

void draw_compass (uint8_t x, uint8_t y, uint16_t heading)

{

        uint8_t front = 19 + (heading / 22);

        for (uint8_t i = 0; i < 9; i++)

        {

                lcd_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), 0);

        }

}

/* ##########################################################################

 * variometer

 * ##########################################################################*/

/**

 * draw variometer arrows at <x>/<y> according to <variometer>

 */

void draw_variometer (uint8_t x, uint8_t y, uint8_t width_x, uint8_t width_y, int16_t variometer)

{

        lcd_rect (x, y - ((width_y - 1) / 2), width_x, width_y, 1);

        lcd_frect (x + 1, y - ((width_y - 1) / 2) + 1, width_x - 2, width_y - 2, 0);

        lcd_line (x, y, x + width_x, y, 1);

        if (variometer > 0)

        { // gain height

                switch (variometer / 5)

                {

                        case 0:

                                lcd_frect (x + 3, y - 1, 3, 1, 1);

                                break;

                        case 1:

                                lcd_frect (x + 2, y - 3, 5, 3, 1);

                                break;

                        case 2:

                                lcd_frect (x + 2, y - 4, 5, 4, 1);

                                break;

                        default:

                                lcd_frect (x + 1, y - 5, 7, 5, 1);

                                break;

                }

        }

        else

        { // sink

                switch (variometer / -5)

                {

                        case 0:

                                lcd_frect (x + 3, y, 3, 1, 1);

                                break;

                        case 1:

                                lcd_frect (x + 2, y, 5, 3, 1);

                                break;

                        case 2:

                                lcd_frect (x + 2, y, 5, 4, 1);

                                break;

                        default:

                                lcd_frect (x + 1, y, 7, 5, 1);

                                break;

                }

        }

}

#define TIMEOUT 200     // 2 sec

void print_statistics (void)

{

        uint8_t line = 0;

        lcd_cls ();

        // max Altitude

        lcd_printpns_at (0, line, stats_item_pointers[0], 0);

        write_ndigit_number_s (13, line, max_Altimeter / 30, 4, 0);

        lcd_putc (17, line, 'm', 0);

        // max Speed

        lcd_printpns_at (0, ++line, stats_item_pointers[1], 0);

        write_ndigit_number_u (14, line, (uint16_t) (((uint32_t) max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0);

        lcd_printpns_at(17, line, PSTR("km/h"), 0);

        // max Distance

        lcd_printpns_at (0, ++line, stats_item_pointers[2], 0);

        write_ndigit_number_u (14, line, max_Distance / 10, 3, 0);

        lcd_putc (17, line, 'm', 0);

        // max time

        lcd_printpns_at (0, ++line, stats_item_pointers[4], 0);

        write_time (13, line, max_FlyingTime);

        // min voltage

        lcd_printpns_at (0, ++line, stats_item_pointers[3], 0);

        write_ndigit_number_u_10th (13, line, min_UBat, 3, 0);

        lcd_putc (17, line, 'V', 0);

#if 1

        // max Current

        lcd_printpns_at (0, ++line, stats_item_pointers[5], 0);

        write_ndigit_number_u_10th (13, line, max_Current, 3, 0);

        lcd_putc (17, line, 'A', 0);

        // Used Capacity

        lcd_printpns_at (0, ++line, stats_item_pointers[6], 0);

        write_ndigit_number_u (13, line, max_Capacity, 4, 0);

        lcd_printpns_at(17, line, PSTR("mAh"), 0);

#else

        // longitude

        lcd_printpns_at (0, ++line, stats_item_pointers[5], 0);

        write_gps_pos (8, line, naviData->CurrentPosition.Longitude);

        // latitude

        lcd_printpns_at (0, ++line, stats_item_pointers[6], 0);

        write_gps_pos (8, line, naviData->CurrentPosition.Latitude);

#endif

        while (!get_key_press (1 << KEY_ESC))

                timer = TIMEOUT;

        COSD_FLAGS2 &= ~COSD_WASFLYING;

        get_key_press(KEY_ALL);

        lcd_cls();

}

void osd (void)

{

        uint8_t flag;

        uint8_t tmp_dat;

        // Clear statistics

        max_Altimeter = 0;

        max_GroundSpeed = 0;

        max_Distance = 0;

        min_UBat = 255;

        max_FlyingTime = 0;

        // flags from last round to check for changes

        uint8_t old_FCFlags = 0;

        uint16_t old_hh = 0;

        lcd_cls();

        if (hardware == FC)

        {

                lcd_printp_at(0, 3, PSTR("Only with NC !"), 0);

                timer = 100;

                while (timer > 0);

        }

        SwitchToNC();

        mode = 'O';

        // disable debug...

        //      RS232_request_mk_data (0, 'd', 0);

        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);

        flag = 0;

        timer = TIMEOUT;

        abo_timer = ABO_TIMEOUT;

        do

        {

                if (rxd_buffer_locked)

                {

                        timer = TIMEOUT;

                        Decode64 ();

                        naviData = (NaviData_t *) pRxData;

                        flag = 1;

                        if (naviData->FCFlags & FCFLAG_MOTOR_RUN)

                        { // should be engines running

                                // motors are on, assume we were/are flying

                                COSD_FLAGS2 |= COSD_WASFLYING;

                        }

                        else

                        {       // stats

                                if ((COSD_FLAGS2 & COSD_WASFLYING) || (get_key_press (1 << KEY_ENTER)))

                                {

                                        print_statistics ();

                                }

                        }

//                      lcd_printpns_at (0, 3, PSTR("012345678901234567890"), 0);

                        lcd_ecircle(22, 35, 16, 1);

                        // Ground Speed

                        write_ndigit_number_u (1, 0, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0);

                        lcd_printpns_at(4, 0, PSTR("km/h"), 0);

                        // Compass

                        write_ndigit_number_u (14, 0, naviData->CompassHeading, 3, 0);

                        lcd_putc (17, 0, 0x1E, 0);      // degree symbol

                        lcd_printpns_at (18, 0, (const char *) (pgm_read_word ( &(directions_p[heading_conv(naviData->CompassHeading)]))), 0);

                        draw_compass (12, 1, naviData->CompassHeading);

                        // Altitude

                        //note:lephisto:according to several sources it's /30

                        if (naviData->Altimeter > 300 || naviData->Altimeter < -300)

                        {

                                // above 10m only write full meters

                                write_ndigit_number_s (0, 1, naviData->Altimeter / 30, 4, 0);

                        }

                        else

                        {

                                // up to 10m write meters.dm

                                write_ndigit_number_s_10th (0, 1, naviData->Altimeter / 3, 3, 0);

                        }

                        lcd_putc (4, 1, 'm', 0);

                        draw_variometer (55, 7, 9, 13, naviData->Variometer);

                        // TODO: verify correctness

                        uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360;

                        lcd_ecirc_line (22, 35, 15, old_hh, 0);

                        old_hh = heading_home;

                        lcd_ecirc_line (22, 35, 15, heading_home, 1);

                        write_ndigit_number_u (7, 3, heading_home, 3, 0);

                        lcd_putc (10, 3, 0x1e, 0);      // degree symbol

                        write_ndigit_number_u (7, 2, naviData->HomePositionDeviation.Distance / 10, 3, 0);

                        lcd_putc (10, 2, 'm', 0);

                        // Sats in use

                        lcd_printp_at(10, 4, PSTR("Sats"), 0);

                        write_ndigit_number_u (8, 4, naviData->SatsInUse, 2, 0);

                        if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL)

                        {

                                lcd_putc (19, 4, 'M', 0); // rc transmitter

                        }

                        else

                        {

                                lcd_putc (19, 4, ' ', 0); // clear

                        }

#if 0

                        lcd_printp_at(11, 5, PSTR("Mode:"), 0);

                        if (naviData->NCFlags & NC_FLAG_CH)

                        {

                                lcd_printpns_at (17, 5, PSTR("CH  "), 0);

                        }

                        else if (naviData->NCFlags & NC_FLAG_PH)

                        {

                                lcd_printpns_at (17, 5, PSTR("PH  "), 0);

                        }

                        else

                        { // (naviData->NCFlags & NC_FLAG_FREE)

                                lcd_printpns_at (17, 5, PSTR("Free"), 0); // sat2 (free)

                        }

#endif

                        if (naviData->NCFlags & NC_FLAG_CH)

                        {

                                lcd_printpns_at (10, 5, PSTR("Coming Home"), 0);

                        }

                        else if (naviData->NCFlags & NC_FLAG_PH)

                        {

                                lcd_printpns_at (10, 5, PSTR("Pos. Hold  "), 0);

                        }

                        else

                        { // (naviData->NCFlags & NC_FLAG_FREE)

                                lcd_printpns_at (10, 5, PSTR("Free       "), 0);

                        }

                        // Flying time

                        write_time (7, 6, naviData->FlyingTime);

//                      lcd_printp_at (7, 6, PSTR("Fly"), 0);

                        // RC

                        write_ndigit_number_u (15, 6, naviData->RC_Quality, 3, 0);

                        lcd_putc (18, 6, 0x1F, 0);                      // RC-transmitter

                        if (naviData->NCFlags & NC_FLAG_NOSERIALLINK)

                        {

                                lcd_printpns_at(19, 6, PSTR("  "), 0);          // clear

                        }

                        else

                        {

                                lcd_printpns_at(19, 6, PSTR("PC"), 0);

                        }

                        // Battery level

                        write_ndigit_number_u_10th (0, 7, naviData->UBat, 3, 0);

                        lcd_putc (4, 7, 'V', 0);

                        // Current

                        write_ndigit_number_u_10th (7, 7, naviData->Current, 3, 0);

                        lcd_putc (11, 7, 'A', 0);

                        // Capacity

                        write_ndigit_number_u (14, 7, naviData->UsedCapacity, 4, 0);

                        lcd_printpns_at(18, 7, PSTR("mAh"), 0);

                        // remember statistics (only when engines running)

                        if (naviData->FCFlags & FCFLAG_MOTOR_RUN)

                        {

                                if (naviData->Altimeter > max_Altimeter) max_Altimeter = naviData->Altimeter;

                                if (naviData->GroundSpeed > max_GroundSpeed) max_GroundSpeed = naviData->GroundSpeed;

                                if (naviData->HomePositionDeviation.Distance > max_Distance) max_Distance = naviData->HomePositionDeviation.Distance;

                                if (naviData->UBat < min_UBat) min_UBat = naviData->UBat;

                                if (naviData->FlyingTime > max_FlyingTime) max_FlyingTime = naviData->FlyingTime;

                                if (naviData->Current > max_Current) max_Current = naviData->Current;

                                if (naviData->UsedCapacity > max_Capacity) max_Capacity = naviData->UsedCapacity;

                        }

                        // remember last values

                        last_RC_Quality = naviData->RC_Quality;

                        last_UBat = naviData->UBat;

                        old_FCFlags = naviData->FCFlags;

                        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;

                }

        }

        while (!get_key_press (1 << KEY_ESC) && timer);

        get_key_press(KEY_ALL);

        // disable OSD Data from NC

        //      RS232_request_mk_data (1, 'o', 0);

        tmp_dat = 0;

        SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);

        mode = 0;

        rxd_buffer_locked = FALSE;

        if (!timer)

        {       // timeout occured

                if (flag)

                {

                        lcd_cls ();

                }

                lcd_printp_at (0, 2, PSTR("ERROR: no data"), 0);

                timer = 100;

                while (timer > 0);

                print_statistics ();

        }

}