Subversion Repositories Projects

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

/* *********************** GENERAL FUNCTIONS ********************** */

//Extract functions (get bits from the positioning bytes

#define ISa(whichBit) getBit(panA_REG, whichBit)

#define ISb(whichBit) getBit(panB_REG, whichBit)

#define ISc(whichBit) getBit(panC_REG, whichBit)

boolean getBit(byte Reg, byte whichBit) {

  boolean State;

  State = Reg & (1 << whichBit);

  return State;

}

byte setBit(byte &Reg, byte whichBit, boolean stat) {

  if (stat) {

    Reg = Reg | (1 << whichBit);

  }

  else {

    Reg = Reg & ~(1 << whichBit);

  }

  return Reg;

}

// EEPROM reader/writers

// Utilities for writing and reading from the EEPROM

byte readEEPROM(int address) {

  return EEPROM.read(address);

}

void writeEEPROM(byte value, int address) {

  EEPROM.write(address, value);

}

void InitializeOSD() {

  loadBar();

  delay(500);

  writeEEPROM(42, CHK1);

  writeEEPROM(VER-42,CHK2);

  writeSettings();

  osd.setPanel(4,9);

  osd.openPanel();

  osd.printf_P(PSTR("OSD Initialized, reboot"));

  osd.closePanel();

  // run for ever so user resets

  for(;;) {}

}

// Write our latest FACTORY settings to EEPROM

void writeSettings() {

 // Writing all default parameters to EEPROM, ON = panel enabled

 // All panels have 3 values:

 //  - Enable/Disable

 //  - X coordinate on screen

 //  - Y coordinate on screen

 writeEEPROM(off, panCenter_en_ADDR);

 writeEEPROM(13, panCenter_x_ADDR);

 writeEEPROM(7,  panCenter_y_ADDR);

 writeEEPROM(on, panPitch_en_ADDR);

 writeEEPROM(22, panPitch_x_ADDR);

 writeEEPROM(9,  panPitch_y_ADDR);

 writeEEPROM(on, panRoll_en_ADDR);

 writeEEPROM(11, panRoll_x_ADDR);

 writeEEPROM(1,  panRoll_y_ADDR);

 writeEEPROM(on, panBatt_A_en_ADDR);

 writeEEPROM(21, panBatt_A_x_ADDR);

 writeEEPROM(1,  panBatt_A_y_ADDR);

 //writeEEPROM(on, panBatt_B_en_ADDR);

 //writeEEPROM(21, panBatt_B_x_ADDR);

 //writeEEPROM(3,  panBatt_B_y_ADDR);

 writeEEPROM(on, panGPSats_en_ADDR);

 writeEEPROM(2,  panGPSats_x_ADDR);

 writeEEPROM(13, panGPSats_y_ADDR);

 writeEEPROM(on, panGPL_en_ADDR);

 writeEEPROM(5,  panGPL_x_ADDR);

 writeEEPROM(13, panGPL_y_ADDR);

 writeEEPROM(on, panGPS_en_ADDR);

 writeEEPROM(2,  panGPS_x_ADDR);

 writeEEPROM(14, panGPS_y_ADDR);

 writeEEPROM(on, panRose_en_ADDR);

 writeEEPROM(16, panRose_x_ADDR);

 writeEEPROM(14, panRose_y_ADDR);

 writeEEPROM(on, panHeading_en_ADDR);

 writeEEPROM(24, panHeading_x_ADDR);

 writeEEPROM(13, panHeading_y_ADDR);

 writeEEPROM(on, panMavBeat_en_ADDR);

 writeEEPROM(2,  panMavBeat_x_ADDR);

 writeEEPROM(9, panMavBeat_y_ADDR);

 writeEEPROM(on, panHomeDir_en_ADDR);

 writeEEPROM(14, panHomeDir_x_ADDR);

 writeEEPROM(3,  panHomeDir_y_ADDR);

 writeEEPROM(on, panHomeDis_en_ADDR);

 writeEEPROM(2,  panHomeDis_x_ADDR);

 writeEEPROM(1,  panHomeDis_y_ADDR);

 writeEEPROM(off,panWPDir_en_ADDR);

 writeEEPROM(0,  panWPDir_x_ADDR);

 writeEEPROM(0,  panWPDir_y_ADDR);

 writeEEPROM(off,panWPDis_en_ADDR);

 writeEEPROM(0,  panWPDis_x_ADDR);

 writeEEPROM(0,  panWPDis_y_ADDR);

 //writeEEPROM(on, panRSSI_en_ADDR);

 //writeEEPROM(21, panRSSI_x_ADDR);

 //writeEEPROM(5,  panRSSI_y_ADDR);

 //writeEEPROM(on, panCur_A_en_ADDR);

 //writeEEPROM(21, panCur_A_x_ADDR);

 //writeEEPROM(2,  panCur_A_y_ADDR);

 //writeEEPROM(on, panCur_B_en_ADDR);

 //writeEEPROM(21, panCur_B_x_ADDR);

 //writeEEPROM(4,  panCur_B_y_ADDR);

 writeEEPROM(on, panAlt_en_ADDR);

 writeEEPROM(2,  panAlt_x_ADDR);

 writeEEPROM(2,  panAlt_y_ADDR);

 writeEEPROM(on, panVel_en_ADDR);

 writeEEPROM(2,  panVel_x_ADDR);

 writeEEPROM(3,  panVel_y_ADDR);

 writeEEPROM(on, panThr_en_ADDR);

 writeEEPROM(2,  panThr_x_ADDR);

 writeEEPROM(4,  panThr_y_ADDR);

 writeEEPROM(on, panFMod_en_ADDR);

 writeEEPROM(17,  panFMod_x_ADDR);

 writeEEPROM(13, panFMod_y_ADDR);

 writeEEPROM(on, panHorizon_en_ADDR);

 writeEEPROM(8,  panHorizon_x_ADDR);

 writeEEPROM(7,  panHorizon_y_ADDR);

}

void readSettings() {

  //****** First set of 8 Panels ******

  setBit(panA_REG, Cen_BIT, readEEPROM(panCenter_en_ADDR));

  panCenter_XY[0] = readEEPROM(panCenter_x_ADDR);

  panCenter_XY[1] = checkPAL(readEEPROM(panCenter_y_ADDR));

  setBit(panA_REG, Pit_BIT, readEEPROM(panPitch_en_ADDR));

  panPitch_XY[0] = readEEPROM(panPitch_x_ADDR);

  panPitch_XY[1] = checkPAL(readEEPROM(panPitch_y_ADDR));

  setBit(panA_REG, Rol_BIT, readEEPROM(panRoll_en_ADDR));

  panRoll_XY[0] = readEEPROM(panRoll_x_ADDR);

  panRoll_XY[1] = checkPAL(readEEPROM(panRoll_y_ADDR));

  setBit(panA_REG, BatA_BIT, readEEPROM(panBatt_A_en_ADDR));

  panBatt_A_XY[0] = readEEPROM(panBatt_A_x_ADDR);

  panBatt_A_XY[1] = checkPAL(readEEPROM(panBatt_A_y_ADDR));

  //setBit(panA_REG, BatB_BIT, readEEPROM(panBatt_B_en_ADDR));

  //panBatt_B_XY[0] = readEEPROM(panBatt_B_x_ADDR);

  //panBatt_B_XY[1] = checkPAL(readEEPROM(panBatt_B_y_ADDR));

  setBit(panA_REG, GPSats_BIT, readEEPROM(panGPSats_en_ADDR));

  panGPSats_XY[0] = readEEPROM(panGPSats_x_ADDR);

  panGPSats_XY[1] = checkPAL(readEEPROM(panGPSats_y_ADDR));

  setBit(panA_REG, GPL_BIT, readEEPROM(panGPL_en_ADDR));

  panGPL_XY[0] = readEEPROM(panGPL_x_ADDR);

  panGPL_XY[1] = checkPAL(readEEPROM(panGPL_y_ADDR));

  setBit(panA_REG, GPS_BIT, readEEPROM(panGPS_en_ADDR));

  panGPS_XY[0] = readEEPROM(panGPS_x_ADDR);

  panGPS_XY[1] = checkPAL(readEEPROM(panGPS_y_ADDR));

  //****** Second set of 8 Panels ******

  setBit(panB_REG, Rose_BIT, readEEPROM(panRose_en_ADDR));

  panRose_XY[0] = readEEPROM(panRose_x_ADDR);

  panRose_XY[1] = checkPAL(readEEPROM(panRose_y_ADDR));

  setBit(panB_REG, Head_BIT, readEEPROM(panHeading_en_ADDR));

  panHeading_XY[0] = readEEPROM(panHeading_x_ADDR);

  panHeading_XY[1] = checkPAL(readEEPROM(panHeading_y_ADDR));

  setBit(panB_REG, MavB_BIT, readEEPROM(panMavBeat_en_ADDR));

  panMavBeat_XY[0] = readEEPROM(panMavBeat_x_ADDR);

  panMavBeat_XY[1] = checkPAL(readEEPROM(panMavBeat_y_ADDR));

  setBit(panB_REG, HDis_BIT, readEEPROM(panHomeDis_en_ADDR));

  panHomeDis_XY[0] = readEEPROM(panHomeDis_x_ADDR);

  panHomeDis_XY[1] = checkPAL(readEEPROM(panHomeDis_y_ADDR));

  setBit(panB_REG, HDir_BIT, readEEPROM(panHomeDir_en_ADDR));

  panHomeDir_XY[0] = readEEPROM(panHomeDir_x_ADDR);

  panHomeDir_XY[1] = checkPAL(readEEPROM(panHomeDir_y_ADDR));

  //setBit(panB_REG, RSSI_BIT, readEEPROM(panRSSI_en_ADDR));

  //panRSSI_XY[0] = readEEPROM(panRSSI_x_ADDR);

  //panRSSI_XY[1] = checkPAL(readEEPROM(panRSSI_y_ADDR));

  //****** Third set of 8 Panels ******

  //setBit(panC_REG, CurA_BIT, readEEPROM(panCur_A_en_ADDR));

  //panCur_A_XY[0] = readEEPROM(panCur_A_x_ADDR);

  //panCur_A_XY[1] = checkPAL(readEEPROM(panCur_A_y_ADDR));

  //setBit(panC_REG, CurB_BIT, readEEPROM(panCur_B_en_ADDR));

  //panCur_B_XY[0] = readEEPROM(panCur_B_x_ADDR);

  //panCur_B_XY[1] = checkPAL(readEEPROM(panCur_B_y_ADDR));

  setBit(panC_REG, Alt_BIT, readEEPROM(panAlt_en_ADDR));

  panAlt_XY[0] = readEEPROM(panAlt_x_ADDR);

  panAlt_XY[1] = checkPAL(readEEPROM(panAlt_y_ADDR));

  setBit(panC_REG, Vel_BIT, readEEPROM(panVel_en_ADDR));

  panVel_XY[0] = readEEPROM(panVel_x_ADDR);

  panVel_XY[1] = checkPAL(readEEPROM(panVel_y_ADDR));

  setBit(panC_REG, Thr_BIT, readEEPROM(panThr_en_ADDR));

  panThr_XY[0] = readEEPROM(panThr_x_ADDR);

  panThr_XY[1] = checkPAL(readEEPROM(panThr_y_ADDR));

  setBit(panC_REG, FMod_BIT, readEEPROM(panFMod_en_ADDR));

  panFMod_XY[0] = readEEPROM(panFMod_x_ADDR);

  panFMod_XY[1] = checkPAL(readEEPROM(panFMod_y_ADDR));

  setBit(panC_REG, Hor_BIT, readEEPROM(panHorizon_en_ADDR));

  panHorizon_XY[0] = readEEPROM(panHorizon_x_ADDR);

  panHorizon_XY[1] = checkPAL(readEEPROM(panHorizon_y_ADDR));

}

int checkPAL(int line){

  if(line >= osd.getCenter() && osd.getMode() == 0){

    line -= 3;//Cutting lines offset after center if NTSC

  }

  return line;

}

void updateSettings(byte panel, byte panel_x, byte panel_y, byte panel_s ) {

  if(panel >= 1 && panel <= 32) {

    writeEEPROM(panel_s, (6 * panel) - 6 + 0);

    if(panel_s != 0) {

      writeEEPROM(panel_x, (6 * panel) - 6 + 2);

      writeEEPROM(panel_y, (6 * panel) - 6 + 4);

    }

    osd.clear();

    readSettings();

  }

}