view main/robots/odr-display/display.cpp @ 237:43f43ebe35df main

latest updates
author Bob Cook <bob@bobcookdev.com>
date Sat, 27 Dec 2014 08:42:01 -0800
parents 8af48f7c2f34
children 98dfb5a2c37a
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2012-2014 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Display functions for the Outdoor Robot user interface board..
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
// ----------------------------------------------------------------------------------------

#include <avr/io.h>
#include <avr/pgmspace.h>

#include "func.h"

#include "packages/avr/device/spinwait.h"

#include "packages/common/util/printutils.h"
#include "packages/common/util/misc.h"

#include "packages/avr/lcd/ili9340/ili9340.h"

// always after other includes
#include "packages/avr/device/workaround34734.h"

// ----------------------------------------------------------------------------------------
//
//  The LCD display is 26 rows by 30 columns:
//
//                   1         2
//         012345678901234567890123456789
//         ||||||||||||||||||||||||||||||
//      0-|Outdoor Robot                H|- H = heartbeat spinner
//      1-|                              |-
//      2-|                              |-
//      3-|                              |-
//      4-|      motion controller       |-
//      5-|  f: +00 / +00  r: +00 / +00  |-
//      6-|                              |-
//      7-|       front obstacles        |-
//      8-|    000   000   000   000     |-
//      9-|                              |-
//     10-|        location (00)         |-
//      1-|  N 123 45.678  W 123 45.678  |-
//      2-|                              |-
//      3-|     origin (123.4) target    |-
//      4-| N 123 45.678    N 123 45.678 |-
//      5-| W 123 45.678    W 123 45.678 |-
//      6-|                              |-
//      7-|  inertial measurement unit   |-
//      8-|   Y +000  P +00.0  R +00.0   |-
//      9-|                              |-
//     20-|  +++++++ MENU ROW 1 +++++++  |-
//      1-|  +++++++ MENU ROW 2 ++++++>  |-
//      2-|                              |-
//      3-|                              |-
//      4-|                              |-
//      5-|       emergency active       |-
//         ||||||||||||||||||||||||||||||
//         012345678901234567890123456789
//                   1         2
//
//
//      0-|Outdoor Robot       H|-          H = heartbeat spinner
//      1-|                     |-
//      2-|      XXXXXXXX       |-          X = status message from mgr
//      3-|                     |-
//      4-|  motion controller  |-
//      5-| f:+00/+00 r:+00/+00 |-
//      6-|                     |-
//      7-|  front rangefinder  |-
//      8-|   000 000 000 000   |-
//      9-|                     |-
//     10-| lat: N 123 45.678 * |-
//      1-| lon: W 123 45.678   |-
//      2-|                     |-
//      3-| Y+000 P+00.0 R+00.0 |-
//      4-|                     |-
//      5-|ESTOP -A- -B-        |-
//         ||||||||||||||||||||| 
//         012345678901234567890
//                   1         2
//
//
// GPS, BUMP F, BUMP R
// ----------------------------------------------------------------------------------------

// ----------------------------------------------------------------------------------------

static volatile uint8_t g_spinner_count;

// ----------------------------------------------------------------------------------------

static void str_small_int( char* str, int8_t value )
{
/*
    if ( value >= 100 )
    {
        *str++ = '1';
        value -= 100;
    }
    else
    {
        *str++ = '0';
    }
*/

    if ( value >= 10 )
    {
        *str++ = ( '0' + ( value / 10 ) );
        value %= 10;
    }
    else
    {
        *str++ = '0';
    }

    *str++ = ( '0' + value );

    *str = '\0';
}

// ----------------------------------------------------------------------------------------

static void str_small_int_with_sign( char* str, int8_t value )
{
    if ( value < 0 )
    {
        value = -value;
        *str++ = '-';
    }
    else if ( value > 0 )
    {
        *str++ = '+';
    }
    else
    {
        *str++ = '0';
    }

    str_small_int( str, value );
}

// ----------------------------------------------------------------------------------------

static void str_write( char c, uint8_t x, uint8_t y )
{
    ili9340_write( c, x, y, ili9340_color_white, ili9340_color_blue );
}

// ----------------------------------------------------------------------------------------

static void str_write( const char* str, uint8_t x, uint8_t y )
{
    ili9340_write( str, x, y, ili9340_color_white, ili9340_color_blue );
}

// ----------------------------------------------------------------------------------------

static void str_write_pgm( const prog_char* str, uint8_t x, uint8_t y )
{
    ili9340_write_pgm( str, x, y, ili9340_color_white, ili9340_color_blue );
}

// ----------------------------------------------------------------------------------------

void display_init()
{
    //--    Set the SS pin as an output; this prevents the SPI machinery from blocking.

    DDRD |= ( 1 << PD3 );

    //--    The backlight is on PORTC.1 which is OC1B. Set up Timer1 to allow variable
    //      output to dim the display.

//    DDRC |= ( 1 << PC1 );

#if 0
    TCCR1A = 0

           | ( 0 << COM1A1 ) | ( 0 << COM1A0 )  // OC1A disconnected
           | ( 1 << COM1B1 ) | ( 1 << COM1B0 )  // set OC1B on match
           | ( 0 << WGM11  ) | ( 1 << WGM10  )  // Fast PWM, 8-bit
           ;

    TCCR1B = 0

           | ( 0 << WGM13 ) | ( 1 << WGM12 )                   // Fast PWM, 8-bit
           | ( 1 << CS12  ) | ( 0 << CS11  ) | ( 1 << CS10  )  // clk/1024
           ;

    OCR1B = 64;
#endif

    //--    Everything else is handled by the ILI9340 package.

    ili9340_init();

    //--    Write the initial display text to the screen.

    ili9340_fill_screen( ili9340_color_blue );
    str_write_pgm( PSTR("Outdoor Robot"), 1, 0 );
    str_write_pgm( PSTR("motion controller"), 6, 4 );
    str_write_pgm( PSTR("f:     /      r:     /"), 2, 5 );
    str_write_pgm( PSTR("front obstacles"), 7, 7 );
    str_write_pgm( PSTR("location (--)"), 8, 10 );
    str_write_pgm( PSTR("origin         target"), 5, 13 );
    str_write_pgm( PSTR("inertial measurement unit"), 2, 17 );
}

// ----------------------------------------------------------------------------------------

void display_sleep()
{
    for ( uint8_t i = 254; i > 0; --i )
    {
        OCR1B = i;
        spinwait_delay_ms( 1 );
    }
    OCR1B = 0;
}

// ----------------------------------------------------------------------------------------

void display_wake()
{
    OCR1B = 255;
}

// ----------------------------------------------------------------------------------------

static void update_heartbeat_flag()
{
    char flag;

    switch ( g_spinner_count )
    {
        case 0:
            flag = '-';
            break;

        case 1:
            flag = '\\';
            break;

        case 2:
            flag = '|';
            break;

        case 3:
            flag = '/';
            break;

        default:
            flag = '-';
            break;
    }

    if ( status_is_mgr_up() )
    {
        ili9340_write( flag, 29, 0, ili9340_color_white, ili9340_color_blue );
    }
    else
    {
        ili9340_write( flag, 29, 0, ili9340_color_white, ili9340_color_red );
    }
}

// ----------------------------------------------------------------------------------------

static void update_emergency_info()
{
    if ( status_is_emergency() )
    {
        ili9340_write_pgm( PSTR("       emergency active       "),
                           0, 25, ili9340_color_white, ili9340_color_red );
    }
    else
    {
        ili9340_write_pgm( PSTR("                              "),
                           0, 25, ili9340_color_white, ili9340_color_blue );
    }
}

// ----------------------------------------------------------------------------------------

static void update_wheel_info()
{
    if ( ! status_is_motion_ctl_alive() )
    {
        ili9340_write_pgm( PSTR("         offline          "),
                           2, 5, ili9340_color_white, ili9340_color_red );
    }
    else
    {
        str_write_pgm( PSTR("f: "),    2, 5 );
        str_write_pgm( PSTR(" / "),    8, 5 );
        str_write_pgm( PSTR("  r: "), 14, 5 );
        str_write_pgm( PSTR(" / "),   22, 5 );
    }
}

// ----------------------------------------------------------------------------------------

static void update_sonar_front_info()
{
    if ( ! status_is_sonar_front_up() )
    {
        ili9340_write_pgm( PSTR("         offline          "),
                           2, 8, ili9340_color_white, ili9340_color_red );
    }
    else if ( ! status_is_sonar_front_enabled() )
    {
        str_write_pgm( PSTR("        disabled          "), 2, 8 );
    }
    else
    {
        str_write_pgm( PSTR("  "),   2, 8 );
        str_write_pgm( PSTR("   "),  7, 8 );
        str_write_pgm( PSTR("   "), 13, 8 );
        str_write_pgm( PSTR("   "), 19, 8 );
        str_write_pgm( PSTR("   "), 25, 8 );
    }
}

// ----------------------------------------------------------------------------------------

static void update_gps_status()
{
    if ( ! status_is_gps_up() )
    {
        str_write( '-',  18, 10 );
        str_write( '-',  19, 10 );
        ili9340_write_pgm( PSTR("         offline          "),
                           2, 11, ili9340_color_white, ili9340_color_red );
    }
    else if ( ! status_does_gps_have_fix() )
    {
        str_write( '-',  18, 10 );
        str_write( '-',  19, 10 );
        str_write_pgm( PSTR("- --- ------  - --- ------"), 2, 11 );
    }
    else
    {
        str_write( ' ',  3, 11 );
        str_write( ' ',  7, 11 );
        str_write( ' ', 14, 11 );
        str_write( ' ', 15, 11 );
        str_write( ' ', 17, 11 );
        str_write( ' ', 21, 11 );
    }
}

// ----------------------------------------------------------------------------------------

static void update_imu_status()
{
    if ( ! status_is_imu_up() )
    {
        ili9340_write_pgm( PSTR("         offline          "),
                           2, 18, ili9340_color_white, ili9340_color_red );
    }
    else
    {
        str_write_pgm( PSTR(" Y "),   2, 18 );
        str_write_pgm( PSTR("  P "),  9, 18 );
        str_write_pgm( PSTR("  R "), 18, 18 );
        str_write( ' ', 27, 18 );
    }
}

// ----------------------------------------------------------------------------------------

void display_refresh()
{
    update_heartbeat_flag();
    update_emergency_info();
    update_wheel_info();
    update_sonar_front_info();
    update_gps_status();
    update_imu_status();

    ++g_spinner_count;
    g_spinner_count %= 4;
}

// ----------------------------------------------------------------------------------------

void display_set_mgr_status( const char* msg, uint8_t length )
{
    for ( uint8_t i = 0; i < length; ++i )
    {
        ili9340_write( *msg++, i + 6, 2, ili9340_color_white, ili9340_color_blue );
    }

    for ( uint8_t i = length; i < 8 /* maximum message length */; ++i )
    {
        ili9340_write( ' ', i + 6, 2, ili9340_color_white, ili9340_color_blue );
    }
}

// ----------------------------------------------------------------------------------------

void display_clear_mgr_status()
{
    ili9340_write_pgm( PSTR("       "), 6, 2, ili9340_color_white, ili9340_color_blue );
}

// ----------------------------------------------------------------------------------------

void display_update_wheel_speed( int8_t front, int8_t rear )
{
    char buf[ 6 ];

    str_small_int_with_sign( buf, front );
    ili9340_write( buf, 5, 5, ili9340_color_white, ili9340_color_blue );

    str_small_int_with_sign( buf, rear );
    ili9340_write( buf, 19, 5, ili9340_color_white, ili9340_color_blue );
}

// ----------------------------------------------------------------------------------------

void display_update_wheel_position( int8_t front, int8_t rear )
{
    char buf[ 6 ];

    str_small_int_with_sign( buf, front );
    ili9340_write( buf, 11, 5, ili9340_color_white, ili9340_color_blue );

    str_small_int_with_sign( buf, rear );
    ili9340_write( buf, 25, 5, ili9340_color_white, ili9340_color_blue );
}

// ----------------------------------------------------------------------------------------

void display_update_sonar_front
(
    uint8_t left,
    uint8_t center_left,
    uint8_t center_right,
    uint8_t right
)
{
    char  buf[ 6 ];
    char* p = buf;

    p = print_decimal( buf, left );
    *p = '\0';
    str_write( buf, 4, 8 );

    p = print_decimal( buf, center_left );
    *p = '\0';
    str_write( buf, 10, 8 );

    p = print_decimal( buf, center_right );
    *p = '\0';
    str_write( buf, 16, 8 );

    p = print_decimal( buf, right );
    *p = '\0';
    str_write( buf, 22, 8 );
}

// ----------------------------------------------------------------------------------------
//  format the IMU value as "+123.4" in the given buf (which should be 7 bytes minimum)

static void format_imu_value( int32_t value, char* buf )
{
    char* p = buf + 6; // sign, three digits, decimal pt, one digit, null
    *p-- = '\0';

    *buf = '+';

    if ( value < 0 )
    {
        *buf = '-';
        value *= -1;
    }

    // value is scaled by 10,000, so +123.4567 becomes 1,234,567
    value /= 1000; // discard three least significant digits

    for ( uint8_t i = 0; i < 4; ++i )
    {
        int32_t d = value % 10;
        *p-- = ( '0' + d );
        value /= 10;

        if ( i == 0 )
        {
            *p-- = '.';
        }
    }

    // remove the leading zeros, if any

    if ( buf[ 1 ] == '0' )
    {
        buf[ 1 ] = ' ';

        if ( buf[ 2 ] == '0' )
        {
            buf[ 2 ] = ' ';
        }
    }
}

// ----------------------------------------------------------------------------------------

void display_update_yaw( int32_t value )
{
    // yaw is displayed as a whole integer, dropping the fractional portion

    char  buf[ 8 ];
    format_imu_value( value, buf );
    buf[ 4 ] = '\0';

    str_write( buf, 5, 18 );
}

// ----------------------------------------------------------------------------------------

void display_update_pitch( int32_t value )
{
    // pitch is displayed as a the least two significant integral digits plus one digit
    // from the fraction e.g. "+xx.y"

    char  buf[ 8 ];
    format_imu_value( value, buf );
    buf[ 1 ] = buf[ 0 ];

    str_write( &buf[ 1 ], 13, 18 );
}

// ----------------------------------------------------------------------------------------

void display_update_roll( int32_t value )
{
    // roll is displayed as a the least two significant integral digits plus one digit
    // from the fraction e.g. "+xx.y"

    char  buf[ 8 ];
    format_imu_value( value, buf );
    buf[ 1 ] = buf[ 0 ];

    str_write( &buf[ 1 ], 22, 18 );
}

// ----------------------------------------------------------------------------------------
//  format the GPS value as "123 45.678" in the given buf (11 bytes minimum)

static void format_gps_value( int16_t degrees, int32_t min_thousandths, char* buf )
{
    char* p = buf + 3; // three digits for degrees followed by space
    *p-- = ' ';

    for ( uint8_t i = 0; i < 3; ++i )
    {
        int16_t d = degrees % 10;
        *p-- = ( '0' + d );
        degrees /= 10;
    }

    // remove the leading zeros, if any

    if ( buf[ 0 ] == '0' )
    {
        buf[ 0 ] = ' ';

        if ( buf[ 1 ] == '0' )
        {
            buf[ 1 ] = ' ';
        }
    }

    // now write the factional min_thousandths value

    p = buf + 10; // degrees, space, two digits, decimal point, three digits, null
    *p-- = '\0';

    for ( uint8_t i = 0; i < 5; ++i )
    {
        int32_t d = min_thousandths % 10;
        *p-- = ( '0' + d );
        min_thousandths /= 10;

        if ( i == 2 )
        {
            *p-- = '.';
        }
    }

    // remove the leading zero, if any

    if ( buf[ 4 ] == '0' )
    {
        buf[ 4 ] = ' ';
    }
}

// ----------------------------------------------------------------------------------------

void display_update_latitude( int16_t degrees, int32_t min_thousandths )
{
    if ( degrees < 0 )
    {
        str_write( 'S', 2, 11 );
        degrees *= -1;
    }
    else
    {
        str_write( 'N', 2, 11 );
    }

    char buf[ 12 ];
    format_gps_value( degrees, min_thousandths, buf );
    str_write( buf, 4, 11 );
}

// ----------------------------------------------------------------------------------------

void display_update_longitude( int16_t degrees, int32_t min_thousandths )
{
    if ( degrees < 0 )
    {
        str_write( 'W', 16, 11 );
        degrees *= -1;
    }
    else
    {
        str_write( 'E', 16, 11 );
    }

    char buf[ 12 ];
    format_gps_value( degrees, min_thousandths, buf );
    str_write( buf, 18, 11 );
}

// ----------------------------------------------------------------------------------------

void display_update_gps_fix( uint8_t satellites )
{
    if ( satellites == 0 )
    {
        return;
    }

    if ( satellites > 10 )
    {
        str_write( '1', 18, 10 );
        satellites -= 10;
    }
    else
    {
        str_write( ' ', 18, 10 );
    }

    str_write( '0' + satellites, 19, 10 );
}

// ----------------------------------------------------------------------------------------