Subversion Repositories FlightCtrl

#include <stdlib.h>
#include <inttypes.h>
#include "eeprom.h"
#include "timer2.h"
#include "rc.h"
#include "externalControl.h"
#include "uart0.h"
#include "printf_P.h"
#include "analog.h"
#include "twimaster.h"
#include "attitude.h"
#include "menu.h"

#if (!defined (USE_NAVICTRL))
uint8_t maxMenuItem = 13;
#else
#ifdef USE_NAVICTRL
#include "gps.c"
uint8_t maxMenuItem = 14;
#endif
#endif
uint8_t menuItem = 0;
uint8_t remoteKeys = 0;

#define KEY1    0x01
#define KEY2    0x02
#define KEY3    0x04
#define KEY4    0x08
#define KEY5    0x10

int8_t displayBuff[DISPLAYBUFFSIZE];
uint8_t dispPtr = 0;

/************************************/
/*        Clear LCD Buffer          */
/************************************/
void LCD_clear(void) {
  uint8_t i;
  for (i = 0; i < DISPLAYBUFFSIZE; i++)
    displayBuff[i] = ' ';
}

/************************************/
/*        Update Menu on LCD        */
/************************************/
// Display with 20 characters in 4 lines
void LCD_printMenu(void) {
  if (remoteKeys & KEY1) {
    if (menuItem)
      menuItem--;
    else
      menuItem = maxMenuItem;
  }

  if (remoteKeys & KEY2) {
    if (menuItem == maxMenuItem)
      menuItem = 0;
    else
      menuItem++;
  }
  if ((remoteKeys & KEY1) && (remoteKeys & KEY2))
    menuItem = 0;

  LCD_clear();

  if (menuItem > maxMenuItem)
    menuItem = maxMenuItem;
  // print menu item number in the upper right corner
  if (menuItem < 10) {
    LCD_printfxy(17, 0, "[%i]", menuItem);
  } else {
    LCD_printfxy(16, 0, "[%i]", menuItem);
  }

  switch (menuItem) {
  case 0: // Version Info Menu Item
    LCD_printfxy(0, 0, "+ MikroKopter +");
    LCD_printfxy(
        0,
        1,
        "HW:V%d.%d SW:%d.%d%c",
        boardRelease/10, boardRelease%10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a');
    LCD_printfxy(0, 2, "Setting: %d %s", getActiveParamSet(), motorMixer.name);
    if (I2CTimeout < 6) {
      LCD_printfxy(0, 3, "I2C Error!!!");
    } else if (missingMotor) {
      LCD_printfxy(0, 3, "Missing BL-Ctrl:%d", missingMotor);
    } else
      LCD_printfxy(0, 3, "(c) Holger Buss");
    break;
    /*
     case 1:// Height Control Menu Item
     if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
     LCD_printfxy(0,0,"Height:    %5i",ReadingHeight);
     LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight);
     LCD_printfxy(0,2,"Air Press.:%5i",0);
     LCD_printfxy(0,3,"Offset    :%5i",0);
     }
     else
     {
     LCD_printfxy(0,1,"No ");
     LCD_printfxy(0,2,"Height Control");
     }
     break;
     */
  case 2: // Attitude Menu Item
    LCD_printfxy(0, 0, "Attitude");
    LCD_printfxy(0, 1, "Nick:      %5i",
        attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL);
    LCD_printfxy(0, 2, "Roll:      %5i",
        attitude[ROLL ] / GYRO_DEG_FACTOR_PITCHROLL);
    LCD_printfxy(0, 3, "Heading(M):%5i", magneticHeading);
    break;
  case 3: // Remote Control Channel Menu Item
    LCD_printfxy(0, 0, "C1:%4i  C2:%4i ", PPM_in[1], PPM_in[2]);
    LCD_printfxy(0, 1, "C3:%4i  C4:%4i ", PPM_in[3], PPM_in[4]);
    LCD_printfxy(0, 2, "C5:%4i  C6:%4i ", PPM_in[5], PPM_in[6]);
    LCD_printfxy(0, 3, "C7:%4i  C8:%4i ", PPM_in[7], PPM_in[8]);
    break;
  case 4: // Remote Control Mapping Menu Item
    LCD_printfxy(
        0,
        0,
        "Ni:%4i  Ro:%4i ",
        PPM_in[channelMap.channels[CH_PITCH]], PPM_in[channelMap.channels[CH_ROLL]]);
    LCD_printfxy(
        0,
        1,
        "Gs:%4i  Ya:%4i ",
        PPM_in[channelMap.channels[CH_THROTTLE]], PPM_in[channelMap.channels[CH_YAW]]);
    LCD_printfxy(
        0,
        2,
        "P1:%4i  P2:%4i ",
        PPM_in[channelMap.channels[CH_POTS]], PPM_in[channelMap.channels[CH_POTS+1]]);
    LCD_printfxy(
        0,
        3,
        "P3:%4i  P4:%4i ",
        PPM_in[channelMap.channels[CH_POTS+2]], PPM_in[channelMap.channels[CH_POTS+3]]);
    break;
    /*
     case 5:// Gyro Sensor Menu Item
     LCD_printfxy(0,0,"Gyro - Sensor");
     switch(BoardRelease) {
     case 10:
     LCD_printfxy(0,1,"Nick %4i (%3i.%i)", AdValueGyroNick - HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset % HIRES_GYRO_AMPLIFY);
     LCD_printfxy(0,2,"Roll %4i (%3i.%i)", AdValueGyroRoll - HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset % HIRES_GYRO_AMPLIFY);
     LCD_printfxy(0,3,"Yaw  %4i (%3i)", AdValueGyroYaw , YawOffset);
     break;
     case 11:
     case 12:
     case 20: // divice Offests by 2 becuse 2 samples are added in adc isr
     LCD_printfxy(0,1,"Nick %4i (%3i.%i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7)
     LCD_printfxy(0,2,"Roll %4i (%3i.%i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7)
     LCD_printfxy(0,3,"Yaw  %4i (%3i)",YawOffset  - AdValueGyroYaw , YawOffset/2);
     break;

     case 13:
     default: // divice Offests by 2 becuse 2 samples are added in adc isr
     LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7)
     LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7)
     LCD_printfxy(0,3,"Yaw  %4i (%3i)(%3i)",YawOffset  - AdValueGyroYaw , YawOffset/2, 0);
     break;
     }
     break;
     case 6:// Acceleration Sensor Menu Item
     LCD_printfxy(0,0,"ACC - Sensor");
     LCD_printfxy(0,1,"Nick   %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR
     LCD_printfxy(0,2,"Roll   %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR
     LCD_printfxy(0,3,"Height %4i (%3i)",0,0);
     break;
     */
  case 7: // Battery Voltage / Remote Control Level
    LCD_printfxy(0, 1, "Voltage:  %3i.%1iV", UBat/10, UBat%10);
    LCD_printfxy(0, 2, "RC-Level: %5i", RCQuality);
    break;
  case 8: // Compass Menu Item
    LCD_printfxy(0, 0, "Compass       ");
    LCD_printfxy(0, 0, "TODO: Impl.   ");
    break;
  case 9: // Poti Menu Item
    LCD_printfxy(0, 0, "Variables     ");
    LCD_printfxy(0, 0, "TODO: Impl.   ");
    break;
  case 10: // Servo Menu Item
    LCD_printfxy(0, 0, "Servos        ");
    LCD_printfxy(0, 0, "TODO: Impl.   ");
    break;
  case 11: //Extern Control
    LCD_printfxy(0, 0, "ExternControl  ");
    LCD_printfxy(0, 1, "Ni:%4i  Ro:%4i ",
        externalControl.pitch, externalControl.roll);
    LCD_printfxy(0, 2, "Gs:%4i  Ya:%4i ",
        externalControl.throttle, externalControl.yaw);
    LCD_printfxy(0, 3, "Hi:%4i  Cf:%4i ",
        externalControl.height, externalControl.config);
    break;

  case 12: //BL Communication errors
    LCD_printfxy(0, 0, "BL-Ctrl Errors ");
    LCD_printfxy(0, 1, " %3d  %3d  %3d  %3d ",
        motor[0].error, motor[1].error, motor[2].error, motor[3].error);
    LCD_printfxy(0, 2, " %3d  %3d  %3d  %3d ",
        motor[4].error, motor[5].error, motor[6].error, motor[7].error);
    LCD_printfxy(0, 3, " %3d  %3d  %3d  %3d ",
        motor[8].error, motor[9].error, motor[10].error, motor[11].error);
    break;

  case 13: //BL Overview
    LCD_printfxy(0, 0, "BL-Ctrl found ");
    LCD_printfxy(
        0,
        1,
        " %c   %c   %c   %c ",
        motor[0].present + '-', motor[1].present + '-', motor[2].present + '-', motor[3].present + '-');
    LCD_printfxy(
        0,
        2,
        " %c   %c   %c   %c ",
        motor[4].present + '-', motor[5].present + '-', motor[6].present + '-', motor[7].present + '-');
    LCD_printfxy(0, 3, " %c   -   -   - ", motor[8].present + '-');
    if (motor[9].present)
      LCD_printfxy(4, 3, "10");
    if (motor[10].present)
      LCD_printfxy(8, 3, "11");
    if (motor[11].present)
      LCD_printfxy(12, 3, "12");
    break;

#if (defined (USE_NAVICTRL))
    case 14: //GPS Lat/Lon coords
    if (GPSInfo.status == INVALID) {
      LCD_printfxy(0,0,"No GPS data!");
    } else {
      switch (GPSInfo.satfix)
      {
        case SATFIX_NONE:
        LCD_printfxy(0,0,"Sats: %d Fix: No", GPSInfo.satnum);
        break;
        case SATFIX_2D:
        LCD_printfxy(0,0,"Sats: %d Fix: 2D", GPSInfo.satnum);
        break;
        case SATFIX_3D:
        LCD_printfxy(0,0,"Sats: %d Fix: 3D", GPSInfo.satnum);
        break;
        default:
        LCD_printfxy(0,0,"Sats: %d Fix: ??", GPSInfo.satnum);
        break;
      }
      int16_t i1,i2,i3;
      i1 = (int16_t)(GPSInfo.longitude/10000000L);
      i2 = abs((int16_t)((GPSInfo.longitude%10000000L)/10000L));
      i3 = abs((int16_t)(((GPSInfo.longitude%10000000L)%10000L)/10L));
      LCD_printfxy(0,1,"Lon: %d.%03d%03d deg",i1, i2, i3);
      i1 = (int16_t)(GPSInfo.latitude/10000000L);
      i2 = abs((int16_t)((GPSInfo.latitude%10000000L)/10000L));
      i3 = abs((int16_t)(((GPSInfo.latitude%10000000L)%10000L)/10L));
      LCD_printfxy(0,2,"Lat: %d.%03d%03d deg",i1, i2, i3);
      i1 = (int16_t)(GPSInfo.altitude/1000L);
      i2 = abs((int16_t)(GPSInfo.altitude%1000L));
      LCD_printfxy(0,3,"Alt: %d.%03d m",i1, i2);
    }
    break;
#endif

  default:
    maxMenuItem = menuItem - 1;
    menuItem = 0;
    break;
  }
  remoteKeys = 0;
}