Subversion Repositories Projects

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

 *   Copyright (C) 2009 by Claas Anders "CaScAdE" Rathje                    *

 *   admiralcascade@gmail.com                                               *

 *   Project-URL: 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.              *

 *                                                                          *

 *                                                                          *

 *   Credits to:                                                            *

 *   Holger Buss & Ingo Busker from mikrokopter.de for the MK project       *

 *   Gregor "killagreg" Stobrawa for making the MK code readable            *

 *   Klaus "akku" Buettner for the hardware                                 *

 *   Manuel "KeyOz" Schrape for explaining the MK protocol to me            *

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

#include <avr/io.h>

#include <avr/interrupt.h>

#include <util/delay.h>

/* TODO:

 * - verifiy correctness of values

 * - clean up code :)

 */

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

 * Debugging and general purpose definitions

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

#define ALLCHARSDEBUG 0         // set to 1 and flash firmware to see all chars

#define WRITECHARS 224          // set to 2XX and flash firmware to write new char

                                                        // enables the allchars as well to see results

#define NTSC 0                          // set to 1 for NTSC mode + lifts the bottom line

#define ARTHORIZON 0            // set to 1 to enable roll&nick artificial horizon

#define NOOSD 0                         // set to 1 to disable OSD completely

#define NOOSD_BUT_WRN 0         // set to 1 to disable OSD completely but show 

                                                        // battery and receive signal warnings

#define UBAT_WRN 94                     // voltage for blinking warning, like FC settings

#define RCLVL_WRN 100           // make the RC level blink if below this number

// ### read datasheet before changing stuff below this line :)

#define BLINK   0b01001111      // attribute byte for blinking chars

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

 * FLAGS usable during runtime

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

#define COSD_FLAG_NTSC                  1

#define COSD_FLAG_ARTHORIZON    2

#define COSD_FLAG_NOOSD                 4

#define COSD_FLAG_NOOSD_BUT_WRN 8

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

 * Software SPI to communicate with MAX7456

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

#define MAX_CS_HIGH             PORTA |=  (1 << PA1);

#define MAX_CS_LOW              PORTA &= ~(1 << PA1);

#define MAX_SDIN_HIGH           PORTA |=  (1 << PA2);

#define MAX_SDIN_LOW            PORTA &= ~(1 << PA2);

#define MAX_SCLK_HIGH           PORTA |=  (1 << PA3);

#define MAX_SCLK_LOW            PORTA &= ~(1 << PA3);

#define MAX_RESET_HIGH          PORTA |=  (1 << PA5);

#define MAX_RESET_LOW           PORTA &= ~(1 << PA5);

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

 * LED controll

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

#define LED1_ON                 PORTC |=  (1 << PC0);

#define LED1_OFF                PORTC &= ~(1 << PC0);

#define LED2_ON                 PORTC |=  (1 << PC1);

#define LED2_OFF                PORTC &= ~(1 << PC1);

#define LED3_ON                 PORTC |=  (1 << PC2);

#define LED3_OFF                PORTC &= ~(1 << PC2);

#define LED4_ON                 PORTC |=  (1 << PC3);

#define LED4_OFF                PORTC &= ~(1 << PC3);

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

 * switch controll

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

#define S1_PRESSED              !(PINC & (1<<PC5))

#define S2_PRESSED              !(PINC & (1<<PC4))

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

 * gain some fake arm compat :)

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

#define u8 uint8_t

#define s8 int8_t

#define u16 uint16_t

#define s16 int16_t

#define u32 uint32_t

#define s32 int32_t

#if !(ALLCHARSDEBUG|WRITECHARS)

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

 * MK data strucs & flags

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

#define NC_FLAG_FREE                    1

#define NC_FLAG_PH                      2

#define NC_FLAG_CH                      4

#define NC_FLAG_RANGE_LIMIT             8

#define NC_SERIAL_LINK_OK               16

#define NC_FLAG_TARGET_REACHED          32

#define FLAG_MOTOR_RUN  1

#define FLAG_FLY        2

#define FLAG_CALIBRATE  4

#define FLAG_START      8

/*

 * FC Debug Struct

 * portions taken and adapted from

 * http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.72p%2Fuart.h

 */

typedef struct {

    uint8_t Digital[2];

    uint16_t Analog[32]; // Debugvalues

} __attribute__((packed)) DebugOut_t;

/*

 * NaviCtrl OSD Structs

 * portions taken and adapted from

 * http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=NaviCtrl&path=%2Ftags%2FV0.14e%2Fuart1.h

 */

typedef struct {

    s32 Longitude; // in 1E-7 deg

    s32 Latitude; // in 1E-7 deg

    s32 Altitude; // in mm

    u8 Status; // validity of data

} __attribute__((packed)) GPS_Pos_t;

typedef struct {

    s16 Distance; // distance to target in cm

    s16 Bearing; // course to target in deg

} __attribute__((packed)) GPS_PosDev_t;

typedef struct {

    GPS_Pos_t CurrentPosition; // see ubx.h for details

    GPS_Pos_t TargetPosition;

    GPS_PosDev_t TargetPositionDeviation;

    GPS_Pos_t HomePosition;

    GPS_PosDev_t HomePositionDeviation;

    u8 WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1

    u8 WaypointNumber; // number of stored waypoints

    u8 SatsInUse; // no of satellites used for position solution

    s16 Altimeter; // hight according to air pressure

    s16 Variometer; // climb(+) and sink(-) rate

    u16 FlyingTime; // in seconds

    u8 UBat; // Battery Voltage in 0.1 Volts

    u16 GroundSpeed; // speed over ground in cm/s (2D)

    s16 Heading; // current flight direction in deg as angle to north

    s16 CompassHeading; // current compass value

    s8 AngleNick; // current Nick angle in 1°

    s8 AngleRoll; // current Rick angle in 1°

    u8 RC_Quality; // RC_Quality

    u8 MKFlags; // Flags from FC

    u8 NCFlags; // Flags from NC

    u8 Errorcode; // 0 --> okay

    u8 OperatingRadius; // current operation radius around the Home Position in m

    u8 Reserve[7]; // for future use

} __attribute__((packed)) NaviData_t;

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

 * global definitions and global vars

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

#define baud 57600

#define RXD_BUFFER_LEN          150

#define TXD_BUFFER_LEN          150

volatile uint8_t rxd_buffer_locked = 0;

volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];

volatile uint8_t txd_buffer[TXD_BUFFER_LEN];

volatile uint8_t ReceivedBytes = 0;

volatile uint8_t *pRxData = 0;

volatile uint8_t RxDataLen = 0;

volatile uint16_t setsReceived = 0;

volatile NaviData_t naviData;

volatile DebugOut_t debugData;

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

// 16bit should be enough, normal LiPos don't last that long

volatile uint16_t uptime = 0;

volatile uint16_t timer = 0;

#endif // ends !(ALLCHARSDEBUG|WRITECHARS)

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

 * MAX7456 SPI & Display stuff

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

/**

 * Send a byte through SPI

 */

void spi_send(uint8_t byte) {

    for (int8_t i = 7; i >= 0; i--) {

        if (((byte >> i) & 1)) {

            MAX_SDIN_HIGH

        } else {

            MAX_SDIN_LOW

        }

        MAX_SCLK_HIGH

        MAX_SCLK_LOW

    }

}

/**

 *  Send <byte> to <address> of MAX7456

 */

void spi_send_byte(uint8_t address, uint8_t byte) {

    // start sending

    MAX_CS_LOW

    spi_send(address);

    spi_send(byte);

    // end sending

    MAX_CS_HIGH

}

/**

 *  write a <character> to <address> of MAX7456 display memory

 */

void write_char(uint16_t address, char character) {

    spi_send_byte(0x05, (address & 0xFF00) >> 8); // DMAH

    spi_send_byte(0x06, (address & 0x00FF)); // DMAL

    spi_send_byte(0x07, character); // DMDI

}

/**

 *  write a character <attribute> to <address> of MAX7456 display memory

 */

void write_char_att(uint16_t address, char attribute) {

    // the only important part is that the DMAH[1] is set

    // so we add 2 which binary is the 2nd lowest byte

    spi_send_byte(0x05, ((address & 0xFF00) >> 8) | 2); // DMAH

    spi_send_byte(0x06, (address & 0x00FF)); // DMAL

    spi_send_byte(0x07, attribute); // DMDI

}

/**

 *  write a <character> at <x>/<y> to MAX7456 display memory

 */

void write_char_xy(uint8_t x, uint8_t y, char character) {

    uint16_t address = y * 30 + x;

    write_char(address, character);

}

/**

 *  write a  character <attribute> at <x>/<y> to MAX7456 display memory

 */

void write_char_att_xy(uint8_t x, uint8_t y, char attribute) {

    uint16_t address = y * 30 + x;

    write_char_att(address, attribute);

}

/**

 *  clear display by writing blank characters all over it

 */

void clear(void) {

    uint16_t memory_address = 0;

    for (unsigned int a = 0; a < 480; a++) {

        write_char(memory_address++, 0);

    }

}

/**

 *  write an ascii <character> to <address> of MAX7456 display memory

 */

void write_ascii_char(uint16_t address, char c) {

    if (c == 32) c = 0; // remap space

    else if (c > 48 && c <= 57) c -= 48; // remap numbers

    else if (c == '0') c = 10; // remap zero

    else if (c >= 65 && c <= 90) c -= 54; // remap big letters

    else if (c >= 97 && c <= 122) c -= 60; // remap small letters

    else if (c == '(') c = 63; // remap

    else if (c == ')') c = 64; // remap

    else if (c == '.') c = 65; // remap

    else if (c == '?') c = 66; // remap

    else if (c == ';') c = 67; // remap

    else if (c == ':') c = 68; // remap

    else if (c == ',') c = 69; // remap

    else if (c == '\'') c = 70; // remap

    else if (c == '/') c = 71; // remap

    else if (c == '"') c = 72; // remap

    else if (c == '-') c = 73; // remap minus

    else if (c == '<') c = 74; // remap

    else if (c == '>') c = 75; // remap

    else if (c == '@') c = 76; // remap

    write_char(address, c);

}

/**

 *  write an ascii <string> at <x>/<y> to MAX7456 display memory

 */

void write_ascii_string(uint8_t x, uint8_t y, char *string) {

    while (*string) {

        write_ascii_char(((x++)+(y * 30)), *string);

        string++;

    }

}

/**

 *  Write only the last three digits of a <number> at <x>/<y> to MAX7456

 *  display memory. takes full 16bit numbers as well for stuff

 *  like compass only taking three characters (values <= 999)

 */

void write_3digit_number_u(uint8_t x, uint8_t y, uint16_t number) {

    uint16_t num = 100;

    uint8_t started = 0;

    while (num > 0) {

        uint8_t b = number / num;

        if (b > 0 || started || num == 1) {

            write_ascii_char((x++)+(y * 30), '0' + b);

            started = 1;

        } else {

            write_ascii_char((x++)+(y * 30), 0);

        }

        number -= b * num;

        num /= 10;

    }

}

/**

 *  Write only the last two digits of a number at <x>/<y> to MAX7456

 *  display memory. takes full 16bit numbers as well for stuff

 *  like seconds only taking two characters (values <= 99)

 *  Since this is used for seconds only and it looks better, there

 *  is a trading 0 attached

 */

void write_2digit_number_u(uint8_t x, uint8_t y, uint16_t number) {

    uint16_t num = 10;

    uint8_t started = 0;

    while (num > 0) {

        uint8_t b = number / num;

        if (b > 0 || started || num == 1) {

            write_ascii_char((x++)+(y * 30), '0' + b);

            started = 1;

        } else {

            write_ascii_char((x++)+(y * 30), '0');

        }

        number -= b * num;

        num /= 10;

    }

}

/**

 *  write a unsigned number as /10th at <x>/<y> to MAX7456 display memory

 */

void write_number_u_10th(uint8_t x, uint8_t y, uint16_t number) {

    uint16_t num = 10000;

    uint8_t started = 0;

    while (num > 0) {

        uint8_t b = number / num;

        if (b > 0 || started || num == 1) {

            if ((num / 10) == 0) write_char((x++)+(y * 30), 65);

            write_ascii_char((x++)+(y * 30), '0' + b);

            started = 1;

        } else {

            write_ascii_char((x++)+(y * 30), 0);

        }

        number -= b * num;

        num /= 10;

    }

}

/**

 *  write a unsigned number at <x>/<y> to MAX7456 display memory

 */

void write_number_u(uint8_t x, uint8_t y, uint16_t number) {

    uint16_t num = 10000;

    uint8_t started = 0;

    while (num > 0) {

        uint8_t b = number / num;

        if (b > 0 || started || num == 1) {

            write_ascii_char((x++)+(y * 30), '0' + b);

            started = 1;

        } else {

            write_ascii_char((x++)+(y * 30), 0);

        }

        number -= b * num;

        num /= 10;

    }

}

/**

 *  write a signed number at <x>/<y> to MAX7456 display memory

 */

void write_number_s(uint8_t x, uint8_t y, int16_t w) {

    if (((uint16_t) w) > 32767) {

        w = w - 65536;

        int16_t num = -10000;

        uint8_t started = 0;

        while (num < 0) {

            uint8_t b = w / num;

            if (b > 0 || started || num == 1) {

                if (!started) write_char((x - 1)+(y * 30), 0x49);

                write_ascii_char((x++)+(y * 30), '0' + b);

                started = 1;

            } else {

                write_ascii_char((x++)+(y * 30), 0);

            }

            w -= b * num;

            num /= 10;

        }

    } else {

        write_char((x)+(y * 30), 0);

        write_number_u(x, y, w);

    }

}

/**

 *  write <seconds> as human readable time at <x>/<y> to MAX7456 display mem

 */

void write_time(uint8_t x, uint8_t y, uint16_t seconds) {

    uint16_t min = seconds / 60;

    seconds -= min * 60;

    write_3digit_number_u(x, y, min);

    write_char_xy(x + 3, y, 68);

    write_2digit_number_u(x + 4, y, seconds);

}

/**

 * for testing write all chars to screen

 */

void write_all_chars() {

    uint16_t x = 3, y = 2, t = 0;

    while (t < 256) {

        write_char_xy(x++, y, t++);

        if (x > 25) {

            y++;

            x = 3;

        }

    }

}

#if !(ALLCHARSDEBUG|WRITECHARS)

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

 * USART stuff

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

/**

 * init usart1

 */

void usart1_init() {

    UBRR1H = ((F_CPU / (16UL * baud)) - 1) >> 8;

    UBRR1L = (F_CPU / (16UL * baud)) - 1;

    // Enable receiver and transmitter; enable RX interrupt

    UCSR1B = (1 << RXEN1) | (1 << TXEN1) | (1 << RXCIE1);

    //asynchronous 8N1

    UCSR1C = (1 << URSEL1) | (3 << UCSZ10);

}

/**

 * send a single <character> through usart1

 */

void usart1_putc(unsigned char character) {

    // wait until UDR ready

    while (!(UCSR1A & (1 << UDRE1)));

    UDR1 = character;

}

/**

 * send a <string> throught usart1

 */

void usart1_puts(char *s) {

    while (*s) {

        usart1_putc(*s);

        s++;

    }

}

/**

 * receive data through usart1

 * portions taken and adapted from

 * http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Fbranches%2FV0.72p+Code+Redesign+killagreg%2Fuart0.c

 */

ISR(SIG_USART1_RECV) {

    if (rxd_buffer_locked) return; // if rxd buffer is locked immediately return

    LED1_ON

            static uint16_t crc;

    static uint8_t ptr_rxd_buffer = 0;

    uint8_t crc1, crc2;

    uint8_t c;

    c = UDR1; // catch the received byte

    // the rxd buffer is unlocked

    if ((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received

    {

                /*

                // skip other datasets

        if (ptr_rxd_buffer == 2 && rxd_buffer[ptr_rxd_buffer] != 'O') {

                        ptr_rxd_buffer = 0; // reset rxd buffer

                rxd_buffer_locked = 0; // unlock rxd buffer

                }*/

                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer

        crc = c; // init crc

    } else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes

    {

        if (c != '\r') // no termination character

        {

            rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer

            crc += c; // update crc

        } else // termination character was received

        {

            // the last 2 bytes are no subject for checksum calculation

            // they are the checksum itself

            crc -= rxd_buffer[ptr_rxd_buffer - 2];

            crc -= rxd_buffer[ptr_rxd_buffer - 1];

            // calculate checksum from transmitted data

            crc %= 4096;

            crc1 = '=' + crc / 64;

            crc2 = '=' + crc % 64;

            // compare checksum to transmitted checksum bytes

            if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2 == rxd_buffer[ptr_rxd_buffer - 1])) { // checksum valid

                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character

                ReceivedBytes = ptr_rxd_buffer + 1; // store number of received bytes

                rxd_buffer_locked = 1; // lock the rxd buffer

            } else { // checksum invalid

                rxd_buffer_locked = 0; // unlock rxd buffer

            }

            ptr_rxd_buffer = 0; // reset rxd buffer pointer

        }

    } else // rxd buffer overrun

    {

        ptr_rxd_buffer = 0; // reset rxd buffer

        rxd_buffer_locked = 0; // unlock rxd buffer

    }

    LED1_OFF

}

/**

 * Decode the recevied Buffer

 * portions taken and adapted from

 * http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.72p%2Fuart.c

 */

void Decode64(void) {

    uint8_t a, b, c, d;

    uint8_t x, y, z;

    uint8_t ptrIn = 3;

    uint8_t ptrOut = 3;

    uint8_t len = ReceivedBytes - 6;

    while (len) {

        a = rxd_buffer[ptrIn++] - '=';

        b = rxd_buffer[ptrIn++] - '=';

        c = rxd_buffer[ptrIn++] - '=';

        d = rxd_buffer[ptrIn++] - '=';

        x = (a << 2) | (b >> 4);

        y = ((b & 0x0f) << 4) | (c >> 2);

        z = ((c & 0x03) << 6) | d;

        if (len--) rxd_buffer[ptrOut++] = x;

        else break;

        if (len--) rxd_buffer[ptrOut++] = y;

        else break;

        if (len--) rxd_buffer[ptrOut++] = z;

        else break;

    }

    pRxData = &rxd_buffer[3];

    RxDataLen = ptrOut - 3;

}

/**

 * request Data through USART in special MK format by adding checksum and

 * encode data in modified Base64

 * portions taken and adapted from

 * http://svn.mikrokopter.de/mikrowebsvn/filedetails.php?repname=FlightCtrl&path=%2Ftags%2FV0.72p%2Fuart.c

 */

void sendMKData(unsigned char cmd, unsigned char addr, unsigned char *snd, unsigned char len) {

    unsigned int pt = 0;

    unsigned char a, b, c;

    unsigned char ptr = 0;

    txd_buffer[pt++] = '#'; // Start-Byte

    txd_buffer[pt++] = 'a' + addr; // Adress

    txd_buffer[pt++] = cmd; // Command

    while (len) {

        if (len) {

            a = snd[ptr++];

            len--;

        } else a = 0;

        if (len) {

            b = snd[ptr++];

            len--;

        } else b = 0;

        if (len) {

            c = snd[ptr++];

            len--;

        } else c = 0;

        txd_buffer[pt++] = '=' + (a >> 2);

        txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));

        txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));

        txd_buffer[pt++] = '=' + (c & 0x3f);

    }

    // add crc

    unsigned int tmpCRC = 0, i;

    for (i = 0; i < pt; i++) {

        tmpCRC += txd_buffer[i];

    }

    tmpCRC %= 4096;

    txd_buffer[i++] = '=' + tmpCRC / 64;

    txd_buffer[i++] = '=' + tmpCRC % 64;

    txd_buffer[i++] = '\r';

    usart1_puts((char*) txd_buffer);

}

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

 * timer stuff

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

/*

 * timer kicks in every 1000uS

 */

ISR(TIMER0_OVF_vect) {

    OCR0 = 6; // preload

    if (!timer--) {

        uptime++;

        timer = 999;

    }

}

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

 * compass stuff

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

/**

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

 */

uint8_t heading_conv(uint16_t heading) {

    if (heading > 23 && heading < 68) {

        //direction = "NE";

        return 0;

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

        //direction = "E ";

        return 1;

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

        //direction = "SE";

        return 2;

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

        //direction = "S ";

        return 3;

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

        //direction = "SW";

        return 4;

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

        //direction = "W ";

        return 5;

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

        //direction = "NW";

        return 6;

    }

    //direction = "N ";

    return 7;

}

/**

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

 */

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

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

    char rose[48] = {216, 215, 216, 211, 216, 215, 216, 213, 216, 215, 216, 212,

                    216, 215, 216, 214, 216, 215, 216, 211, 216, 215, 216, 213,

                    216, 215, 216, 212, 216, 215, 216, 214, 216, 215, 216, 211,

                    216, 215, 216, 213, 216, 215, 216, 212, 216, 215, 216, 214};

        // 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

    uint8_t front = 19 + (heading / 22);

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

                write_char_xy(x++, y, rose[front - 4 + i]);

    }

}

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

 * artificial horizon

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

// remember last time displayed values

int8_t old_af_x = -1, old_af_y = -1;

/**

 * draw roll und nick indicators (could be enhanced to full artificial horizon)

 * from line <firstline> to <listlines> for given <nick> and <roll> values

 */

void draw_artificial_horizon(uint8_t firstline, uint8_t lastline, int16_t nick, int16_t roll) {

        char noodle[5] = {225, 225, 226, 227, 227};

        uint8_t center_x = 15;

        uint8_t center_y = lastline - firstline;

        center_y = 7;

        write_char_xy(center_x,center_y,228);

        uint8_t cpos, nicky, rollx;

        // which line

        int8_t ypos =  nick / 20;

        // which character from the array?

        if (nick < 0) {

                cpos = -1*((nick - (ypos * 20))/4);

                ypos--;

        } else cpos = 4-((nick - (ypos * 20))/4);

        if (cpos > 4) cpos = 4;

        nicky = center_y - ypos;

        if (nicky > lastline) nicky = lastline;

        else if (nicky < firstline) nicky = firstline;

        // ensure roll-borders

        rollx = (roll / 8)+15;

        if (rollx < 2) rollx = 2;

        else if (rollx > 28) rollx = 28;

        // clear roll

        if (old_af_x != rollx && old_af_x >= 0) {

                write_char_xy(old_af_x,13,0);

        }

        // clear nick

        if (old_af_y != nicky && old_af_y >= 0) {

                write_char_xy(center_x-1,old_af_y,0);

                write_char_xy(center_x+1,old_af_y,0);

        }

        // draw nick

        write_char_xy(center_x-1,nicky,noodle[cpos]);

        write_char_xy(center_x+1,nicky,noodle[cpos]);

        // draw roll

        write_char_xy(rollx,lastline,229);

        // update old vars