initial commit
initial commit

file:b/ArduCAM_OSD.ino (new)
--- /dev/null
+++ b/ArduCAM_OSD.ino
@@ -1,1 +1,223 @@
-
+/*
+
+Copyright (c) 2011.  All rights reserved.
+An Open Source Arduino based OSD and Camera Control project.
+
+Program  : ArduCAM-OSD (Supports the variant: minimOSD)
+Version  : V2.1, 24 September 2012
+Author(s): Sandro Benigno
+Coauthor(s):
+Jani Hirvinen   (All the EEPROM routines)
+Michael Oborne  (OSD Configutator)
+Mike Smith      (BetterStream and Fast Serial libraries)
+Gábor Zoltán
+Pedro Santos
+Special Contribuitor:
+Andrew Tridgell by all the support on MAVLink
+Doug Weibel by his great orientation since the start of this project
+Contributors: James Goppert, Max Levine, Burt Green, Eddie Furey
+and all other members of DIY Drones Dev team
+Thanks to: Chris Anderson, Jordi Munoz
+
+
+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 3 of the License, or
+(at your option) any later version.
+
+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, see <http://www.gnu.org/licenses/>
+
+*/
+
+/* ************************************************************ */
+/* **************** MAIN PROGRAM - MODULES ******************** */
+/* ************************************************************ */
+
+#undef PROGMEM 
+#define PROGMEM __attribute__(( section(".progmem.data") )) 
+
+#undef PSTR 
+#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 
+
+#define isPAL 1
+
+/* **********************************************/
+/* ***************** INCLUDES *******************/
+
+//#define membug 
+//#define FORCEINIT  // You should never use this unless you know what you are doing 
+
+
+// AVR Includes
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>
+#include <math.h>
+#include <inttypes.h>
+#include <avr/pgmspace.h>
+// Get the common arduino functions
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "wiring.h"
+#endif
+#include <EEPROM.h>
+#include <SimpleTimer.h>
+#include <GCS_MAVLink.h>
+
+#ifdef membug
+#include <MemoryFree.h>
+#endif
+
+// Configurations
+#include "OSD_Config.h"
+#include "ArduCam_Max7456.h"
+#include "OSD_Vars.h"
+#include "OSD_Func.h"
+
+/* *************************************************/
+/* ***************** DEFINITIONS *******************/
+
+//OSD Hardware 
+//#define ArduCAM328
+#define MinimOSD
+
+#define TELEMETRY_SPEED  57600  // How fast our MAVLink telemetry is coming to Serial port
+#define BOOTTIME         2000   // Time in milliseconds that we show boot loading bar and wait user input
+
+// Objects and Serial definitions
+FastSerialPort0(Serial);
+OSD osd; //OSD object 
+
+SimpleTimer  mavlinkTimer;
+
+
+/* **********************************************/
+/* ***************** SETUP() *******************/
+
+void setup() 
+{
+#ifdef ArduCAM328
+    pinMode(10, OUTPUT); // USB ArduCam Only
+#endif
+    pinMode(MAX7456_SELECT,  OUTPUT); // OSD CS
+
+    Serial.begin(TELEMETRY_SPEED);
+    // setup mavlink port
+    mavlink_comm_0_port = &Serial;
+
+#ifdef membug
+    Serial.println(freeMem());
+#endif
+
+    // Prepare OSD for displaying 
+    unplugSlaves();
+    osd.init();
+
+    // Start 
+    startPanels();
+    delay(500);
+
+    // OSD debug for development (Shown at start)
+#ifdef membug
+    osd.setPanel(1,1);
+    osd.openPanel();
+    osd.printf("%i",freeMem()); 
+    osd.closePanel();
+#endif
+
+    // Just to easy up development things
+#ifdef FORCEINIT
+    InitializeOSD();
+#endif
+
+
+    // Check EEPROM to see if we have initialized it already or not
+    // also checks if we have new version that needs EEPROM reset
+//    if(readEEPROM(CHK1) + readEEPROM(CHK2) != VER) {
+//        osd.setPanel(6,9);
+//        osd.openPanel();
+//        osd.printf_P(PSTR("Missing/Old Config")); 
+//        osd.closePanel();
+        //InitializeOSD();
+//    }
+
+    // Get correct panel settings from EEPROM
+    readSettings();
+    for(panel = 0; panel < npanels; panel++) readPanelSettings();
+    panel = 0; //set panel to 0 to start in the first navigation screen
+    // Show bootloader bar
+//    loadBar();
+delay(2000);
+Serial.flush(); 
+    // Startup MAVLink timers  
+    mavlinkTimer.Set(&OnMavlinkTimer, 120);
+
+    // House cleaning, clear display and enable timers
+    osd.clear();
+    mavlinkTimer.Enable();
+
+} // END of setup();
+
+
+
+/* ***********************************************/
+/* ***************** MAIN LOOP *******************/
+
+// Mother of all happenings, The loop()
+// As simple as possible.
+void loop() 
+{
+
+    if(enable_mav_request == 1){//Request rate control
+        //osd.clear();
+        //osd.setPanel(3,10);
+        //osd.openPanel();
+        //osd.printf_P(PSTR("Requesting DataStreams...")); 
+        //osd.closePanel();
+        //for(int n = 0; n < 3; n++){
+        //    request_mavlink_rates();//Three times to certify it will be readed
+        //    delay(50);
+        //}
+        enable_mav_request = 0;
+        //delay(2000);
+        osd.clear();
+        waitingMAVBeats = 0;
+        lastMAVBeat = millis();//Preventing error from delay sensing
+    }
+
+    read_mavlink();
+    mavlinkTimer.Run();
+}
+
+/* *********************************************** */
+/* ******** functions used in main loop() ******** */
+void OnMavlinkTimer()
+{
+    setHeadingPatern();  // generate the heading patern
+
+    //  osd_battery_pic_A = setBatteryPic(osd_battery_remaining_A);     // battery A remmaning picture
+    //osd_battery_pic_B = setBatteryPic(osd_battery_remaining_B);     // battery B remmaning picture
+
+    setHomeVars(osd);   // calculate and set Distance from home and Direction to home
+    
+    writePanels();       // writing enabled panels (check OSD_Panels Tab)
+    
+    setFdataVars();
+}
+
+
+void unplugSlaves(){
+    //Unplug list of SPI
+#ifdef ArduCAM328
+    digitalWrite(10,  HIGH); // unplug USB HOST: ArduCam Only
+#endif
+    digitalWrite(MAX7456_SELECT,  HIGH); // unplug OSD
+}

+

--- /dev/null
+++ b/ArduCam_Max7456.cpp
@@ -1,1 +1,321 @@
 
+#include <FastSerial.h>
+
+#include "ArduCam_Max7456.h"
+// Get the common arduino functions
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "wiring.h"
+#endif
+#include "Spi.h"
+#include <EEPROM.h>
+#include "OSD_Config.h"
+
+volatile int16_t x;
+volatile int font_count;
+volatile byte character_bitmap[0x40];
+
+OSD::OSD()
+{
+}
+
+//------------------ init ---------------------------------------------------
+
+void OSD::init()
+{
+  pinMode(MAX7456_SELECT,OUTPUT);
+  pinMode(MAX7456_VSYNC, INPUT);
+  digitalWrite(MAX7456_VSYNC,HIGH); //enabling pull-up resistor
+
+  detectMode();
+
+  digitalWrite(MAX7456_SELECT,LOW);
+  //read black level register
+  Spi.transfer(MAX7456_OSDBL_reg_read);//black level read register
+  byte osdbl_r = Spi.transfer(0xff);
+  Spi.transfer(MAX7456_VM0_reg);
+  Spi.transfer(MAX7456_RESET | video_mode);
+  delay(50);
+  //set black level
+  byte osdbl_w = (osdbl_r & 0xef); //Set bit 4 to zero 11101111
+  Spi.transfer(MAX7456_OSDBL_reg); //black level write register
+  Spi.transfer(osdbl_w);
+
+  setBrightness();
+  // define sync (auto,int,ext) and
+  // making sure the Max7456 is enabled
+  control(1);
+}
+
+//------------------ Detect Mode (PAL/NTSC) ---------------------------------
+
+void OSD::detectMode()
+{
+  //read STAT and auto detect Mode PAL/NTSC
+  digitalWrite(MAX7456_SELECT,LOW);
+  Spi.transfer(MAX7456_STAT_reg_read);//status register
+  byte osdstat_r = Spi.transfer(0xff);
+
+  if ((B00000001 & osdstat_r) == 1){ //PAL
+      setMode(1);  
+  }
+  else if((B00000010 & osdstat_r) == 1){ //NTSC
+      setMode(0);
+  }
+  //If no signal was detected so it uses EEPROM config
+  else{
+      if (EEPROM.read(PAL_NTSC_ADDR) == 0){ //NTSC
+          setMode(0);
+      } 
+      else { //PAL
+          setMode(1);
+      }
+      digitalWrite(MAX7456_SELECT,LOW);
+  }
+}
+
+//------------------ Set Brightness  ---------------------------------
+void OSD::setBrightness()
+{
+
+    uint8_t blevel = EEPROM.read(OSD_BRIGHTNESS_ADDR);
+
+    if(blevel == 0) //low brightness
+        blevel = MAX7456_WHITE_level_80;
+    else if(blevel == 1) 
+        blevel = MAX7456_WHITE_level_90;
+    else if(blevel == 2)
+        blevel = MAX7456_WHITE_level_100;
+    else if(blevel == 3) //high brightness
+        blevel = MAX7456_WHITE_level_120;
+    else 
+        blevel = MAX7456_WHITE_level_80; //low brightness if bad value
+    
+    // set all rows to same charactor white level, 90%
+    for (x = 0x0; x < 0x10; x++)
+    {
+        Spi.transfer(x + 0x10);
+        Spi.transfer(blevel);
+    }
+}
+
+//------------------ Set Mode (PAL/NTSC) ------------------------------------
+
+void OSD::setMode(int themode)
+{
+  switch(themode){
+    case 0:
+      video_mode = MAX7456_MODE_MASK_NTCS;
+      video_center = MAX7456_CENTER_NTSC;
+      break;
+    case 1:
+      video_mode = MAX7456_MODE_MASK_PAL;
+      video_center = MAX7456_CENTER_PAL;
+      break;
+  }
+}
+
+//------------------ Get Mode (PAL 0/NTSC 1) --------------------------------
+
+int OSD::getMode()
+{
+  switch(video_mode){
+    case MAX7456_MODE_MASK_NTCS:
+      return 0;
+      break;
+    case MAX7456_MODE_MASK_PAL:
+      return 1;
+      break;
+  }
+  return 0;
+}
+
+//------------------ Get Center (PAL/NTSC) ----------------------------------
+
+int OSD::getCenter()
+{
+  return video_center; //first line for center panel
+}
+
+//------------------ plug ---------------------------------------------------
+
+void OSD::plug()
+{
+  digitalWrite(MAX7456_SELECT,LOW);
+}
+
+//------------------ clear ---------------------------------------------------
+
+void OSD::clear()
+{
+  // clear the screen
+  digitalWrite(MAX7456_SELECT,LOW);
+  Spi.transfer(MAX7456_DMM_reg);
+  Spi.transfer(MAX7456_CLEAR_display);
+  digitalWrite(MAX7456_SELECT,HIGH);
+}
+
+//------------------ set panel -----------------------------------------------
+
+void
+OSD::setPanel(uint8_t st_col, uint8_t st_row){
+  start_col = st_col;
+  start_row = st_row;
+  col = st_col;
+  row = st_row;
+}
+
+//------------------ open panel ----------------------------------------------
+
+void
+OSD::openPanel(void){
+  unsigned int linepos;
+  byte settings, char_address_hi, char_address_lo;
+ 
+  //find [start address] position
+  linepos = row*30+col;
+  
+  // divide 16 bits into hi & lo byte
+  char_address_hi = linepos >> 8;
+  char_address_lo = linepos;
+
+  //Auto increment turn writing fast (less SPI commands).
+  //No need to set next char address. Just send them
+  settings = MAX7456_INCREMENT_auto; //To Enable DMM Auto Increment
+  digitalWrite(MAX7456_SELECT,LOW);
+  Spi.transfer(MAX7456_DMM_reg); //dmm
+  Spi.transfer(settings);
+
+  Spi.transfer(MAX7456_DMAH_reg); // set start address high
+  Spi.transfer(char_address_hi);
+
+  Spi.transfer(MAX7456_DMAL_reg); // set start address low
+  Spi.transfer(char_address_lo);
+  //Serial.printf("setPos -> %d %d\n", col, row);
+}
+
+//------------------ close panel ---------------------------------------------
+
+void
+OSD::closePanel(void){  
+  Spi.transfer(MAX7456_DMDI_reg);
+  Spi.transfer(MAX7456_END_string); //This is needed "trick" to finish auto increment
+  digitalWrite(MAX7456_SELECT,HIGH);
+  //Serial.println("close");
+  row++; //only after finish the auto increment the new row will really act as desired
+}
+
+//------------------ write single char ---------------------------------------------
+
+void
+OSD::openSingle(uint8_t x, uint8_t y){
+  unsigned int linepos;
+  byte char_address_hi, char_address_lo;
+ 
+  //find [start address] position
+  linepos = y*30+x;
+  
+  // divide 16 bits into hi & lo byte
+  char_address_hi = linepos >> 8;
+  char_address_lo = linepos;
+  
+  digitalWrite(MAX7456_SELECT,LOW);
+  
+  Spi.transfer(MAX7456_DMAH_reg); // set start address high
+  Spi.transfer(char_address_hi);
+
+  Spi.transfer(MAX7456_DMAL_reg); // set start address low
+  Spi.transfer(char_address_lo);
+  //Serial.printf("setPos -> %d %d\n", col, row);
+}
+
+//------------------ write ---------------------------------------------------
+
+size_t
+OSD::write(uint8_t c){
+  if(c == '|'){
+    closePanel(); //It does all needed to finish auto increment and change current row
+    openPanel(); //It does all needed to re-enable auto increment
+  }
+  else{
+    Spi.transfer(MAX7456_DMDI_reg);
+    Spi.transfer(c);
+  }
+  return 1;
+}
+
+//---------------------------------
+
+void
+OSD::control(uint8_t ctrl){
+  digitalWrite(MAX7456_SELECT,LOW);
+  Spi.transfer(MAX7456_VM0_reg);
+  switch(ctrl){
+    case 0:
+      Spi.transfer(MAX7456_DISABLE_display | video_mode);
+      break;
+    case 1:
+      //Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_internal);
+      //Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_external);
+      Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_autosync); 
+      break;
+  }
+  digitalWrite(MAX7456_SELECT,HIGH);
+}
+
+void 
+OSD::write_NVM(int font_count, uint8_t *character_bitmap)
+{
+  byte x;
+  byte char_address_hi, char_address_lo;
+  byte screen_char;
+
+  char_address_hi = font_count;
+  char_address_lo = 0;
+ //Serial.println("write_new_screen");   
+
+  // disable display
+  digitalWrite(MAX7456_SELECT,LOW);
+  Spi.transfer(MAX7456_VM0_reg); 
+  Spi.transfer(MAX7456_DISABLE_display);
+
+  Spi.transfer(MAX7456_CMAH_reg); // set start address high
+  Spi.transfer(char_address_hi);
+
+  for(x = 0; x < NVM_ram_size; x++) // write out 54 (out of 64) bytes of character to shadow ram
+  {
+    screen_char = character_bitmap[x];
+    Spi.transfer(MAX7456_CMAL_reg); // set start address low
+    Spi.transfer(x);
+    Spi.transfer(MAX7456_CMDI_reg);
+    Spi.transfer(screen_char);
+  }
+
+  // transfer a 54 bytes from shadow ram to NVM
+  Spi.transfer(MAX7456_CMM_reg);
+  Spi.transfer(WRITE_nvr);
+  
+  // wait until bit 5 in the status register returns to 0 (12ms)
+  while ((Spi.transfer(MAX7456_STAT_reg_read) & STATUS_reg_nvr_busy) != 0x00);
+
+  Spi.transfer(MAX7456_VM0_reg); // turn on screen next vertical
+  Spi.transfer(MAX7456_ENABLE_display_vert);
+  digitalWrite(MAX7456_SELECT,HIGH);  
+}
+
+//------------------ pure virtual ones (just overriding) ---------------------
+
+int  OSD::available(void){
+	return 0;
+}
+int  OSD::read(void){
+	return 0;
+}
+int  OSD::peek(void){
+	return 0;
+}
+void OSD::flush(void){
+}
+

+

file:b/ArduCam_Max7456.h (new)
--- /dev/null
+++ b/ArduCam_Max7456.h
@@ -1,1 +1,107 @@
 
+#ifndef ArduCam_Max7456_h
+#define ArduCam_Max7456_h
+
+/******* FROM DATASHEET *******/
+
+#define MAX7456_SELECT 6//SS
+#define MAX7456_VSYNC 2//INT0
+
+#define NTSC 0
+#define PAL 1
+#define MAX7456_MODE_MASK_PAL 0x40 //PAL mask 01000000
+#define MAX7456_CENTER_PAL 0x8
+
+#define MAX7456_MODE_MASK_NTCS 0x00 //NTSC mask 00000000 ("|" will do nothing)
+#define MAX7456_CENTER_NTSC 0x6
+
+//MAX7456 reg read addresses
+#define MAX7456_OSDBL_reg_read 0xec //black level
+#define MAX7456_STAT_reg_read  0xa0 //0xa[X] Status
+
+//MAX7456 reg write addresses
+#define MAX7456_VM0_reg   0x00
+#define MAX7456_VM1_reg   0x01
+#define MAX7456_DMM_reg   0x04
+#define MAX7456_DMAH_reg  0x05
+#define MAX7456_DMAL_reg  0x06
+#define MAX7456_DMDI_reg  0x07
+#define MAX7456_OSDM_reg  0x0c //not used. Is to set mix
+#define MAX7456_OSDBL_reg 0x6c //black level
+
+//MAX7456 reg write addresses to recording NVM process
+#define MAX7456_CMM_reg   0x08
+#define MAX7456_CMAH_reg  0x09
+#define MAX7456_CMAL_reg  0x0a
+#define MAX7456_CMDI_reg  0x0b
+
+//DMM commands
+#define MAX7456_CLEAR_display 0x04
+#define MAX7456_CLEAR_display_vert 0x06
+
+#define MAX7456_INCREMENT_auto 0x03
+#define MAX7456_SETBG_local 0x20 //00100000 force local BG to defined brightness level VM1[6:4]
+
+#define MAX7456_END_string 0xff
+
+//VM0 commands mixed with mode NTSC or PAL mode
+#define MAX7456_ENABLE_display_vert 0x0c //mask with NTSC/PAL
+#define MAX7456_RESET 0x02 //mask with NTSC/PAL
+#define MAX7456_DISABLE_display 0x00 //mask with NTSC/PAL
+
+//VM0 command modifiers
+#define MAX7456_SYNC_autosync 0x10
+#define MAX7456_SYNC_internal 0x30
+#define MAX7456_SYNC_external 0x20
+//VM1 command modifiers
+#define MAX7456_WHITE_level_80 0x03
+#define MAX7456_WHITE_level_90 0x02
+#define MAX7456_WHITE_level_100 0x01
+#define MAX7456_WHITE_level_120 0x00
+
+#define NVM_ram_size 0x36
+#define WRITE_nvr 0xa0
+#define STATUS_reg_nvr_busy 0x20
+
+//If PAL
+#ifdef isPAL
+  #define MAX7456_screen_size 480 //16x30
+  #define MAX7456_screen_rows 0x10
+#else
+  #define MAX7456_screen_size 390 //13x30
+  #define MAX7456_screen_rows 0x0D
+#endif
+
+//------------------ the OSD class -----------------------------------------------
+
+class OSD: public BetterStream
+{
+  public:
+    OSD(void);
+    void init(void);
+    void clear(void);
+    void plug(void);
+    void setPanel(uint8_t start_col, uint8_t start_row);
+    void openPanel(void);
+    void closePanel(void);
+    void control(uint8_t ctrl);
+    void detectMode(void);
+    void setMode(int mode);
+    void setBrightness();
+    void openSingle(uint8_t x, uint8_t y);
+    int getMode(void);
+    int getCenter(void);
+    virtual int     available(void);
+    virtual int     read(void);
+    virtual int     peek(void);
+    virtual void    flush(void);
+    virtual size_t write(uint8_t c);
+    void write_NVM(int font_count, uint8_t *character_bitmap);
+    using BetterStream::write;
+  private:
+    uint8_t start_col, start_row, col, row, video_mode, video_center;
+};
+
+#endif
+

+

file:b/ArduNOTES.ino (new)
--- /dev/null
+++ b/ArduNOTES.ino
@@ -1,1 +1,83 @@
+/*
 
+  File       : ArduCAM-OSD Notes
+  Version    : v1.0.02
+  Author     : Jani Hirvinen
+
+  File to define all panels that can be used on ArduCAM-OSD hardware. Also their order numbers 
+  and other information.
+
+  Order number are also used when configuring their location. Look below
+
+  Register , Order number , Panel name, Data/Size , Definitions
+  -------------------------------------------------------
+  panA_REG.0    01    panCenter       "IIII" \r "IIII"  - Center xrosshair 
+  panA_REG.1    02    panPitch        "DDDDII"          - Pitch angle with symbols
+  panA_REG.2    03    panRoll         "DDDDII"          - Roll angle with symbols
+  panA_REG.3    04    panBattery1     "DD.DII"          - Voltage sensing #1 symbol with voltage reading
+  panA_REG.4    05    panBattery2     "DD.DII"          - Voltage sensing #2 symbol with voltage reading  (!Not Implemented)
+  panA_REG.5    06    panGPSats       "I  CC"           - Amount of locked satelliset with symbols
+  panA_REG.6    07    panGPL          "II"              - GPS Lock symbol
+  panA_REG.7    08    panGPS          "I DDD.DDDDDD" \r "I DDD.DDDDDD" - GPS location data (lat/lon)
+ 
+  panB_REG.0    09    panRose         "IIIIIIIIIIIII" \r "ICCCCCCCCCCCCI"  - Compass rose
+  panB_REG.1    10    panHeading      "IIDDDDI"         - Compass heading with symbols
+  panB_REG.2    11    panMavBeat      "II"              - MAVLink heartbeat
+  panB_REG.3    12    panHomeDir      "II"      N/Y     - Home location arrows
+  panB_REG.4    13    panHomeDis      "IDDDDDI" N/Y     - Distance to home
+  panB_REG.5    14    panWPDir        "II"      N/Y     - Waypoint location arrows (!Not Implemented)
+  panB_REG.6    15    panWPDis        "IDDDDDI"         - Distance to next waypoint (!Not Implemented)
+  panB_REG.7    16    panRSSI                           - RSSI data from RC 
+  
+  panC_REG.0    17    panCurrent1                       - Current sensing #1  
+  panC_REG.1    18    panCurrent2                       - Current sensing #2 (!Not Implemented)
+  panC_REG.2    19    panAlt          "IDDDDDI"         - Altitude information
+  panC_REG.3    20    panVel          "IDDDDDI"         - Velocity information
+  panC_REG.4    21    panThr          "IDDDDDI"         - MAVLink Throttle data
+  panC_REG.5    22    panFMod         "II"              - Flight mode display
+  panC_REG.6    05    panHorizon                        - Artificial Horizon
+  panC_REG.7    24    --
+
+
+I = Icon
+D = Digit
+C = Char
+
+N/Y = Not Yet working
+N/C = Not Confirmed
+
+Screen sizes: 
+PAL   15 Rows x 32 Chars
+NTSC  13 Rows x 30 Chars
+ 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+*/

+

file:b/BOOT_Func.ino (new)
--- /dev/null
+++ b/BOOT_Func.ino
@@ -1,1 +1,75 @@
+/* ******************************************************************/
+/* *********************** BOOT UP FUNCTIONS ********************** */
 
+
+///////////////////////////////////////////////////////
+// Function: loadBar(void)
+//
+// On bootup time we will show loading bar for defined BOOTTIME seconds
+// This is interesting to avoid writing to APM during bootup if OSD's TX is connected
+// After that, it continue in normal mode eg starting to listen MAVLink commands
+
+//#define barX 5
+//#define barY 12
+
+//void loadBar() { //change name due we don't have CLI anymore
+//  int waitTimer;
+//  byte barStep = 0;
+
+  // Write plain panel to let users know what to do
+//  panBoot(barX,barY);
+
+//  delay(500);    // To give small extra waittime to users
+//  Serial.flush(); 
+  
+  // Our main loop to wait input from user.  
+//  for(waitTimer = 0; waitTimer <= BOOTTIME; waitTimer++) {
+
+    // How often we update our progress bar is depending on modulo
+//    if(waitTimer % (BOOTTIME / 8) == 0) {
+//      barStep++;
+      
+      // Update bar it self
+//      osd.setPanel(barX + 12, barY);
+//      osd.openPanel();
+//      switch(barStep) {
+//        case 0:
+//         osd.printf_P(PSTR("\x8c\x8d\x8d\x8d\x8d\x8d\x8d"));
+//         break;
+//        case 1:
+//         osd.printf_P(PSTR("\x8a\x8d\x8d\x8d\x8d\x8d\x8d"));
+//         break;
+//        case 2:
+//         osd.printf_P(PSTR("\x89\x8b\x8d\x8d\x8d\x8d\x8d"));
+//         break;
+//        case 3:
+//         osd.printf_P(PSTR("\x89\x89\x8b\x8d\x8d\x8d\x8d"));
+//         break;
+//        case 4:
+//         osd.printf_P(PSTR("\x89\x89\x89\x8b\x8d\x8d\x8d"));
+//         break;
+//        case 5:
+//         osd.printf_P(PSTR("\x89\x89\x89\x89\x8b\x8d\x8d"));
+//         break;
+//        case 6:
+//         osd.printf_P(PSTR("\x89\x89\x89\x89\x89\x8b\x8d"));
+//         break;
+//        case 7:
+//         osd.printf_P(PSTR("\x89\x89\x89\x89\x89\x89\x8b"));
+//         break;
+//        case 8:
+//         osd.printf_P(PSTR("\x89\x89\x89\x89\x89\x89\x89"));
+//         break;
+//        case 9:
+//         osd.printf_P(PSTR("\x89\x89\x89\x89\x89\x89\x89\x89"));
+//         break;
+//      }
+//      osd.closePanel();
+//    }
+    
+//    delay(1);       // Minor delay to make sure that we stay here long enough
+//  }
+//}
+
+

+

file:b/Font.ino (new)
--- /dev/null
+++ b/Font.ino
@@ -1,1 +1,95 @@
 
+void uploadFont()
+{
+    uint16_t byte_count = 0;
+    byte bit_count;
+    byte ascii_binary[0x08];
+
+    // move these local to prevent ram usage
+    uint8_t character_bitmap[0x40];
+    int font_count = 0;
+
+    osd.clear();
+//    osd.setPanel(6,9);
+//    osd.openPanel();
+//    osd.printf_P(PSTR("Character Update"));
+//    delay(2000);
+//    osd.closePanel();
+
+
+    Serial.printf_P(PSTR("Ready for Font\n"));
+
+    while(font_count < 256) { 
+        int8_t incomingByte = Serial.read();
+        switch(incomingByte) // parse and decode mcm file
+        {
+        case 0x0d: // carridge return, end of line
+            //Serial.println("cr");
+            if (bit_count == 8 && (ascii_binary[0] == 0x30 || ascii_binary[0] == 0x31))
+            {
+                // turn 8 ascii binary bytes to single byte '01010101' = 0x55
+                // fill in 64 bytes of character data
+                // made this local to prevent needing a global
+                byte ascii_byte;
+
+                ascii_byte = 0;
+
+                if (ascii_binary[0] == 0x31) // ascii '1'
+                    ascii_byte = ascii_byte + 128;
+
+                if (ascii_binary[1] == 0x31)
+                    ascii_byte = ascii_byte + 64;
+
+                if (ascii_binary[2] == 0x31)
+                    ascii_byte = ascii_byte + 32;
+
+                if (ascii_binary[3] == 0x31)
+                    ascii_byte = ascii_byte + 16;
+
+                if (ascii_binary[4] == 0x31)
+                    ascii_byte = ascii_byte + 8;
+
+                if (ascii_binary[5] == 0x31)
+                    ascii_byte = ascii_byte + 4;
+
+                if (ascii_binary[6] == 0x31)
+                    ascii_byte = ascii_byte + 2;
+
+                if (ascii_binary[7] == 0x31)
+                    ascii_byte = ascii_byte + 1;
+
+                character_bitmap[byte_count] = ascii_byte;
+                byte_count++;
+                bit_count = 0;
+            }
+            else
+                bit_count = 0;
+            break;
+        case 0x0a: // line feed, ignore
+            //Serial.println("ln");   
+            break;
+        case 0x30: // ascii '0'
+        case 0x31: // ascii '1' 
+            ascii_binary[bit_count] = incomingByte;
+            bit_count++;
+            break;
+        default:
+            break;
+        }
+
+        // we have one completed character
+        // write the character to NVM 
+        if(byte_count == 64)
+        {
+            osd.write_NVM(font_count, character_bitmap);    
+            byte_count = 0;
+            font_count++;
+            Serial.printf_P(PSTR("Char Done\n"));
+        }
+    }
+
+    //  character_bitmap[]
+}
+
+

+

file:b/MAVLink.ino (new)
--- /dev/null
+++ b/MAVLink.ino
@@ -1,1 +1,168 @@
+#include "../GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h"
+#include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h"
 
+// true when we have received at least 1 MAVLink packet
+static bool mavlink_active;
+static uint8_t crlf_count = 0;
+
+static int packet_drops = 0;
+static int parse_error = 0;
+
+void request_mavlink_rates()
+{
+    const int  maxStreams = 6;
+    const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_RAW_SENSORS,
+        MAV_DATA_STREAM_EXTENDED_STATUS,
+        MAV_DATA_STREAM_RC_CHANNELS,
+        MAV_DATA_STREAM_POSITION,
+        MAV_DATA_STREAM_EXTRA1, 
+        MAV_DATA_STREAM_EXTRA2};
+    const uint16_t MAVRates[maxStreams] = {0x02, 0x02, 0x05, 0x02, 0x05, 0x02};
+    for (int i=0; i < maxStreams; i++) {
+        mavlink_msg_request_data_stream_send(MAVLINK_COMM_0,
+            apm_mav_system, apm_mav_component,
+            MAVStreams[i], MAVRates[i], 1);
+    }
+}
+
+void read_mavlink(){
+    mavlink_message_t msg; 
+    mavlink_status_t status;
+
+    //grabing data 
+    while(Serial.available() > 0) { 
+        uint8_t c = Serial.read();
+
+        /* allow CLI to be started by hitting enter 3 times, if no
+        heartbeat packets have been received */
+        if (mavlink_active == 0 && millis() < 20000 && millis() > 5000) {
+            if (c == '\n' || c == '\r') {
+                crlf_count++;
+            } else {
+                crlf_count = 0;
+            }
+            if (crlf_count == 3) {
+                uploadFont();
+            }
+        }
+
+        //trying to grab msg  
+        if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
+            mavlink_active = 1;
+            //handle msg
+            switch(msg.msgid) {
+            case MAVLINK_MSG_ID_HEARTBEAT:
+                {
+                    mavbeat = 1;
+                    apm_mav_system    = msg.sysid;
+                    apm_mav_component = msg.compid;
+//                    apm_mav_type      = mavlink_msg_heartbeat_get_type(&msg);            
+                 //   osd_mode = mavlink_msg_heartbeat_get_custom_mode(&msg);
+                    osd_mode = (uint8_t)mavlink_msg_heartbeat_get_custom_mode(&msg);
+                    //Mode (arducoper armed/disarmed)
+                    base_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
+                    if(getBit(base_mode,7)) motor_armed = 1;
+                    else motor_armed = 0;
+
+                    osd_nav_mode = 0;          
+                    lastMAVBeat = millis();
+                    if(waitingMAVBeats == 1){
+                        enable_mav_request = 1;
+                    }
+                }
+                break;
+            case MAVLINK_MSG_ID_SYS_STATUS:
+                {
+
+                    osd_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f); //Battery voltage, in millivolts (1 = 1 millivolt)
+                    osd_curr_A = mavlink_msg_sys_status_get_current_battery(&msg); //Battery current, in 10*milliamperes (1 = 10 milliampere)         
+                    osd_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg); //Remaining battery energy: (0%: 0, 100%: 100)
+                    //osd_mode = apm_mav_component;//Debug
+                    //osd_nav_mode = apm_mav_system;//Debug
+                }
+                break;
+
+            case MAVLINK_MSG_ID_GPS_RAW_INT:
+                {
+                    osd_lat = mavlink_msg_gps_raw_int_get_lat(&msg) / 10000000.0f;
+                    osd_lon = mavlink_msg_gps_raw_int_get_lon(&msg) / 10000000.0f;
+                    osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg);
+                    osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);
+                    osd_cog = mavlink_msg_gps_raw_int_get_cog(&msg);
+                }
+                break; 
+            case MAVLINK_MSG_ID_VFR_HUD:
+                {
+                    osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg);
+                    osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
+                    osd_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north
+                    osd_throttle = (uint8_t)mavlink_msg_vfr_hud_get_throttle(&msg);
+                    osd_alt = mavlink_msg_vfr_hud_get_alt(&msg);
+                    osd_climb = mavlink_msg_vfr_hud_get_climb(&msg);
+                }
+                break;
+            case MAVLINK_MSG_ID_ATTITUDE:
+                {
+                    osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
+                    osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
+//                    osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
+                }
+                break;
+            case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
+                {
+//                  nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(&msg);
+//                  nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(&msg);
+//                  nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(&msg);
+                  wp_target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(&msg);
+                  wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
+//                  alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
+//                  aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
+                  xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
+                }
+                break;
+            case MAVLINK_MSG_ID_MISSION_CURRENT:
+                {
+                    wp_number = (uint8_t)mavlink_msg_mission_current_get_seq(&msg);
+                }
+                break;
+            case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
+                {
+//                    chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg);
+//                    chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg);
+//                    chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
+//                    chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg);
+                    chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg);
+                    chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg);
+                    chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg);
+                    chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg);
+                    osd_rssi = mavlink_msg_rc_channels_raw_get_rssi(&msg);
+                }
+                break;           
+            case MAVLINK_MSG_ID_WIND:
+                {
+                  if (osd_climb < 1.66 && osd_climb > -1.66){
+                  osd_winddirection = mavlink_msg_wind_get_direction(&msg); // 0..360 deg, 0=north
+                  osd_windspeed = mavlink_msg_wind_get_speed(&msg); //m/s
+//                  osd_windspeedz = mavlink_msg_wind_get_speed_z(&msg); //m/s
+                  }
+                }
+                break;
+            case MAVLINK_MSG_ID_SCALED_PRESSURE:
+                {
+                    temperature = mavlink_msg_scaled_pressure_get_temperature(&msg);
+                }
+                break;
+            default:
+                //Do nothing
+                break;
+            }
+        }
+        delayMicroseconds(138);
+        //next one
+    }
+    // Update global packet drops counter
+    packet_drops += status.packet_rx_drop_count;
+    parse_error += status.parse_error;
+
+}

+

file:b/OSD_Config.h (new)
--- /dev/null
+++ b/OSD_Config.h
@@ -1,1 +1,215 @@
 
+#define on 1
+#define off 0
+
+// Version number, incrementing this will erase/upload factory settings.
+// Only devs should increment this
+#define VER 76
+
+// EEPROM Stepping, be careful not to overstep. 
+// We reserved floats for just to be sure if some values needs to be
+// changed in future.
+// byte  = 1
+// int   = 4
+// float = 8
+
+// Panel 8bit REGISTER with BIT positions
+// panA_REG Byte has:
+//#define Cen_BIT        0
+#define Pit_BIT        1
+#define Rol_BIT        2
+#define BatA_BIT       3
+#define Bp_BIT         4
+#define GPSats_BIT     5
+#define COG_BIT        6
+#define GPS_BIT        7
+
+// panB_REG Byte has:
+#define Rose_BIT       0
+#define Head_BIT       1
+//#define MavB_BIT       2
+#define HDir_BIT       3
+#define HDis_BIT       4
+//#define WDir_BIT       5 //
+#define WDis_BIT       6 //
+#define Time_BIT       7 
+
+// panC_REG Byte has:
+#define CurA_BIT       0
+#define As_BIT         1
+#define Alt_BIT        2
+#define Vel_BIT        3
+#define Thr_BIT        4
+#define FMod_BIT       5
+#define Hor_BIT        6
+#define Halt_BIT       7
+
+// panD_REG Byte has:
+#define Warn_BIT       0
+#define Off_BIT        1
+#define WindS_BIT      2
+#define Climb_BIT      3
+//#define Tune_BIT       4
+#define CALLSIGN_BIT   5
+#define RSSI_BIT       6
+#define Eff_BIT        7
+
+// panE_REG Byte has:
+
+//#define Ch_BIT         0
+#define TEMP_BIT       1
+#define DIST_BIT       2
+
+/* *********************************************** */
+// EEPROM Storage addresses
+
+#define OffsetBITpanel       250
+
+// First of 8 panels
+//#define panCenter_en_ADDR 0
+//#define panCenter_x_ADDR 2
+//#define panCenter_y_ADDR 4
+#define panPitch_en_ADDR 6
+#define panPitch_x_ADDR 8
+#define panPitch_y_ADDR 10
+#define panRoll_en_ADDR 12
+#define panRoll_x_ADDR 14
+#define panRoll_y_ADDR 16
+#define panBatt_A_en_ADDR 18
+#define panBatt_A_x_ADDR 20
+#define panBatt_A_y_ADDR 22
+#define panBatt_B_en_ADDR 24
+#define panBatt_B_x_ADDR 26
+#define panBatt_B_y_ADDR 28
+#define panGPSats_en_ADDR 30
+#define panGPSats_x_ADDR 32
+#define panGPSats_y_ADDR 34
+#define panCOG_en_ADDR 36
+#define panCOG_x_ADDR 38
+#define panCOG_y_ADDR 40
+#define panGPS_en_ADDR 42
+#define panGPS_x_ADDR 44
+#define panGPS_y_ADDR 46
+
+// Second set of 8 panels
+#define panRose_en_ADDR 48
+#define panRose_x_ADDR 50
+#define panRose_y_ADDR 52
+#define panHeading_en_ADDR 54
+#define panHeading_x_ADDR 56
+#define panHeading_y_ADDR 58
+//#define panMavBeat_en_ADDR 60
+//#define panMavBeat_x_ADDR 62
+//#define panMavBeat_y_ADDR 64
+#define panHomeDir_en_ADDR 66
+#define panHomeDir_x_ADDR 68
+#define panHomeDir_y_ADDR 70
+#define panHomeDis_en_ADDR 72
+#define panHomeDis_x_ADDR 74
+#define panHomeDis_y_ADDR 76
+//#define panWPDir_en_ADDR 80 
+//#define panWPDir_x_ADDR 82  
+//#define panWPDir_y_ADDR 84  
+#define panWPDis_en_ADDR 86 
+#define panWPDis_x_ADDR 88  
+#define panWPDis_y_ADDR 90  
+#define panRSSI_en_ADDR 92 
+#define panRSSI_x_ADDR 94  
+#define panRSSI_y_ADDR 96  
+
+// Third set of 8 panels
+#define panCur_A_en_ADDR 98 
+#define panCur_A_x_ADDR 100 
+#define panCur_A_y_ADDR 102 
+#define panCurB_en_ADDR 104 //(!Not implemented)
+#define panCurB_x_ADDR 106 //
+#define panCurB_y_ADDR 108 //
+#define panAlt_en_ADDR 110
+#define panAlt_x_ADDR 112
+#define panAlt_y_ADDR 114
+#define panVel_en_ADDR 116
+#define panVel_x_ADDR 118
+#define panVel_y_ADDR 120
+#define panThr_en_ADDR 122
+#define panThr_x_ADDR 124
+#define panThr_y_ADDR 126
+#define panFMod_en_ADDR 128
+#define panFMod_x_ADDR 130
+#define panFMod_y_ADDR 132
+#define panHorizon_en_ADDR 134
+#define panHorizon_x_ADDR 136
+#define panHorizon_y_ADDR 138
+#define panHomeAlt_en_ADDR 140
+#define panHomeAlt_x_ADDR 142
+#define panHomeAlt_y_ADDR 144
+#define panAirSpeed_en_ADDR 146
+#define panAirSpeed_x_ADDR 148
+#define panAirSpeed_y_ADDR 150
+#define panBatteryPercent_en_ADDR 152
+#define panBatteryPercent_x_ADDR 154
+#define panBatteryPercent_y_ADDR 156
+#define panTime_en_ADDR 158
+#define panTime_x_ADDR 160
+#define panTime_y_ADDR 162
+#define panWarn_en_ADDR 164
+#define panWarn_x_ADDR 166
+#define panWarn_y_ADDR 168
+#define panOff_en_ADDR 170
+//#define panOff_x_ADDR 172
+//#define panOff_y_ADDR 174
+#define panWindSpeed_en_ADDR 176
+#define panWindSpeed_x_ADDR 178
+#define panWindSpeed_y_ADDR 180
+#define panClimb_en_ADDR 182
+#define panClimb_x_ADDR 184
+#define panClimb_y_ADDR 186
+//#define panTune_en_ADDR 188
+//#define panTune_x_ADDR 190
+//#define panTune_y_ADDR 192
+#define panEff_en_ADDR 194
+#define panEff_x_ADDR 196
+#define panEff_y_ADDR 198
+#define panCALLSIGN_en_ADDR 200
+#define panCALLSIGN_x_ADDR 202
+#define panCALLSIGN_y_ADDR 204
+//#define panCh_en_ADDR 206
+//#define panCh_x_ADDR 208
+//#define panCh_y_ADDR 210
+#define panTemp_en_ADDR 212
+#define panTemp_x_ADDR 214
+#define panTemp_y_ADDR 216
+//#define panFdata_en_ADDR 218
+//#define panFdata_x_ADDR 220
+//#define panFdata_y_ADDR 222
+#define panDistance_en_ADDR 224
+#define panDistance_x_ADDR 226
+#define panDistance_y_ADDR 228
+
+#define OSD_BATT_SHOW_PERCENT_ADDR 888
+#define measure_ADDR 890
+#define overspeed_ADDR 892
+#define stall_ADDR 894
+#define battv_ADDR 896
+//#define battp_ADDR 898
+#define OSD_RSSI_HIGH_ADDR 900
+#define OSD_RSSI_LOW_ADDR 902
+#define RADIO_ON_ADDR 904
+#define ch_toggle_ADDR 906
+#define OSD_RSSI_RAW_ADDR 908
+#define switch_mode_ADDR 910
+#define PAL_NTSC_ADDR 912
+
+#define OSD_BATT_WARN_ADDR 914
+#define OSD_RSSI_WARN_ADDR 916
+
+#define OSD_BRIGHTNESS_ADDR 918
+
+#define OSD_CALL_SIGN_ADDR 920
+#define OSD_CALL_SIGN_TOTAL 8
+
+#define CHK1 1000
+#define CHK2 1006
+
+
+#define EEPROM_MAX_ADDR 1024 // this is 328 chip

+

--- /dev/null
+++ b/OSD_Config_Func.ino
@@ -1,1 +1,388 @@
-
+/* ******************************************************************/
+/* *********************** GENERAL FUNCTIONS ********************** */
+
+//Extract functions (get bits from the positioning bytes
+#define ISa(panel,whichBit) getBit(panA_REG[panel], whichBit)
+#define ISb(panel,whichBit) getBit(panB_REG[panel], whichBit)
+#define ISc(panel,whichBit) getBit(panC_REG[panel], whichBit)
+#define ISd(panel,whichBit) getBit(panD_REG[panel], whichBit)
+#define ISe(panel,whichBit) getBit(panE_REG[panel], 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);
+    for(panel = 0; panel < npanels; panel++) 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
+    uint16_t offset = OffsetBITpanel * panel;
+//    writeEEPROM(off, panCenter_en_ADDR + offset);
+//    writeEEPROM(13, panCenter_x_ADDR + offset);
+//    writeEEPROM(7,  panCenter_y_ADDR + offset);
+    writeEEPROM(on, panPitch_en_ADDR + offset);
+    writeEEPROM(23, panPitch_x_ADDR + offset);
+    writeEEPROM(11, panPitch_y_ADDR + offset);
+    writeEEPROM(on, panRoll_en_ADDR + offset);
+    writeEEPROM(12, panRoll_x_ADDR + offset);
+    writeEEPROM(1,  panRoll_y_ADDR + offset);
+    writeEEPROM(on, panBatt_A_en_ADDR + offset);
+    writeEEPROM(14, panBatt_A_x_ADDR + offset);
+    writeEEPROM(14,  panBatt_A_y_ADDR + offset);
+    //writeEEPROM(on, panBatt_B_en_ADDR);
+    //writeEEPROM(21, panBatt_B_x_ADDR);
+    //writeEEPROM(3,  panBatt_B_y_ADDR);
+    writeEEPROM(on, panGPSats_en_ADDR + offset);
+    writeEEPROM(1,  panGPSats_x_ADDR + offset);
+    writeEEPROM(11, panGPSats_y_ADDR + offset);
+    writeEEPROM(on, panCOG_en_ADDR + offset);
+    writeEEPROM(5,  panCOG_x_ADDR + offset);
+    writeEEPROM(11, panCOG_y_ADDR + offset);
+    writeEEPROM(on, panGPS_en_ADDR + offset);
+    writeEEPROM(1,  panGPS_x_ADDR + offset);
+    writeEEPROM(14, panGPS_y_ADDR + offset);
+    writeEEPROM(on, panRose_en_ADDR + offset);
+    writeEEPROM(21, panRose_x_ADDR + offset);
+    writeEEPROM(15, panRose_y_ADDR + offset);
+    writeEEPROM(on, panHeading_en_ADDR + offset);
+    writeEEPROM(24, panHeading_x_ADDR + offset);
+    writeEEPROM(14, panHeading_y_ADDR + offset);
+//    writeEEPROM(on, panMavBeat_en_ADDR + offset);
+//    writeEEPROM(2,  panMavBeat_x_ADDR + offset);
+//    writeEEPROM(9, panMavBeat_y_ADDR + offset);
+    writeEEPROM(on, panHomeDir_en_ADDR + offset);
+    writeEEPROM(14, panHomeDir_x_ADDR + offset);
+    writeEEPROM(3,  panHomeDir_y_ADDR + offset);
+    writeEEPROM(on, panHomeDis_en_ADDR + offset);
+    writeEEPROM(22, panHomeDis_x_ADDR + offset);
+    writeEEPROM(1,  panHomeDis_y_ADDR + offset);
+ //   writeEEPROM(off,panWPDir_en_ADDR);
+ //   writeEEPROM(27,  panWPDir_x_ADDR);
+ //   writeEEPROM(12,  panWPDir_y_ADDR);
+    writeEEPROM(off, panWPDis_en_ADDR);
+    writeEEPROM(9,  panWPDis_x_ADDR);
+    writeEEPROM(14, panWPDis_y_ADDR);
+    writeEEPROM(on, panRSSI_en_ADDR + offset);
+    writeEEPROM(8,  panRSSI_x_ADDR + offset);
+    writeEEPROM(13, panRSSI_y_ADDR + offset);
+    writeEEPROM(on, panCur_A_en_ADDR + offset);
+    writeEEPROM(14, panCur_A_x_ADDR + offset);
+    writeEEPROM(15, panCur_A_y_ADDR + offset);
+    //writeEEPROM(on, panCur_B_en_ADDR);
+    //writeEEPROM(21, panCur_B_x_ADDR);
+    //writeEEPROM(4,  panCur_B_y_ADDR);
+    writeEEPROM(on, panAlt_en_ADDR + offset);
+    writeEEPROM(22, panAlt_x_ADDR + offset);
+    writeEEPROM(3,  panAlt_y_ADDR + offset);
+    writeEEPROM(on, panHomeAlt_en_ADDR + offset);
+    writeEEPROM(22, panHomeAlt_x_ADDR + offset);
+    writeEEPROM(2,  panHomeAlt_y_ADDR + offset);
+    writeEEPROM(on, panVel_en_ADDR + offset);
+    writeEEPROM(1,  panVel_x_ADDR + offset);
+    writeEEPROM(2,  panVel_y_ADDR + offset);
+    writeEEPROM(on, panAirSpeed_en_ADDR + offset);
+    writeEEPROM(1,  panAirSpeed_x_ADDR + offset);
+    writeEEPROM(1,  panAirSpeed_y_ADDR + offset); 
+    writeEEPROM(on, panBatteryPercent_en_ADDR + offset);
+    writeEEPROM(1,  panBatteryPercent_x_ADDR + offset);
+    writeEEPROM(4,  panBatteryPercent_y_ADDR + offset); 
+    writeEEPROM(on, panTime_en_ADDR + offset);
+    writeEEPROM(23, panTime_x_ADDR + offset);
+    writeEEPROM(13, panTime_y_ADDR + offset);
+    writeEEPROM(on, panThr_en_ADDR + offset);
+    writeEEPROM(1,  panThr_x_ADDR + offset);
+    writeEEPROM(3,  panThr_y_ADDR + offset);
+    writeEEPROM(on, panFMod_en_ADDR + offset);
+    writeEEPROM(1,  panFMod_x_ADDR + offset);
+    writeEEPROM(13, panFMod_y_ADDR + offset);
+    writeEEPROM(on, panHorizon_en_ADDR + offset);
+    writeEEPROM(8,  panHorizon_x_ADDR + offset);
+    writeEEPROM(6,  panHorizon_y_ADDR + offset);
+    writeEEPROM(on, panWarn_en_ADDR + offset);
+    writeEEPROM(9,  panWarn_x_ADDR + offset);
+    writeEEPROM(4,  panWarn_y_ADDR + offset);
+    writeEEPROM(on, panOff_en_ADDR + offset);
+//    writeEEPROM(10, panOff_x_ADDR + offset);
+//    writeEEPROM(4,  panOff_y_ADDR + offset);
+    writeEEPROM(on, panWindSpeed_en_ADDR + offset);
+    writeEEPROM(24, panWindSpeed_x_ADDR + offset);
+    writeEEPROM(7,  panWindSpeed_y_ADDR + offset);
+    writeEEPROM(on, panClimb_en_ADDR + offset);
+    writeEEPROM(1,  panClimb_x_ADDR + offset);
+    writeEEPROM(8,  panClimb_y_ADDR + offset);
+//    writeEEPROM(on, panTune_en_ADDR + offset);
+//    writeEEPROM(10, panTune_x_ADDR + offset);
+//    writeEEPROM(4,  panTune_y_ADDR + offset);
+    writeEEPROM(on, panEff_en_ADDR + offset);
+    writeEEPROM(14, panEff_x_ADDR + offset);
+    writeEEPROM(13, panEff_y_ADDR + offset);
+ //   writeEEPROM(on, panCh_en_ADDR + offset);
+ //   writeEEPROM(10, panCh_x_ADDR + offset);
+ //   writeEEPROM(4,  panCh_y_ADDR + offset);
+    writeEEPROM(off, panTemp_en_ADDR + offset);
+    writeEEPROM(22, panTemp_x_ADDR + offset);
+    writeEEPROM(14, panTemp_y_ADDR + offset);
+//    writeEEPROM(on, panFata_en_ADDR + offset);
+//    writeEEPROM(10, panFdata_x_ADDR + offset);
+//    writeEEPROM(4,  panFdata_y_ADDR + offset);
+    writeEEPROM(off, panDistance_en_ADDR + offset);
+    writeEEPROM(22, panDistance_x_ADDR + offset);
+    writeEEPROM(2,  panDistance_y_ADDR + offset);
+ 
+    writeEEPROM(30,overspeed_ADDR);
+    writeEEPROM(0,stall_ADDR);
+    writeEEPROM(100,battv_ADDR); //10Volts
+    writeEEPROM(6,ch_toggle_ADDR);
+
+}
+void readSettings() {
+    overspeed = EEPROM.read(overspeed_ADDR);
+    stall = EEPROM.read(stall_ADDR);
+    battv = EEPROM.read(battv_ADDR);
+    switch_mode = EEPROM.read(switch_mode_ADDR);
+//    if (EEPROM.read(ch_toggle_ADDR) < 4 || EEPROM.read(ch_toggle_ADDR) > 8){
+//     	EEPROM.write(ch_toggle_ADDR, 5);
+//	}
+    ch_toggle = EEPROM.read(ch_toggle_ADDR);
+    //  battp = EEPROM.read(battp_ADDR);
+    rssical = EEPROM.read(OSD_RSSI_HIGH_ADDR);
+    rssipersent = EEPROM.read(OSD_RSSI_LOW_ADDR);
+    rssiraw_on = EEPROM.read(OSD_RSSI_RAW_ADDR);
+
+    batt_warn_level = EEPROM.read(OSD_BATT_WARN_ADDR);
+    rssi_warn_level = EEPROM.read(OSD_RSSI_WARN_ADDR);
+    int i;
+    for(i=0;i < OSD_CALL_SIGN_TOTAL;i++) 
+    {
+        char_call[i] = EEPROM.read(OSD_CALL_SIGN_ADDR + i);
+        if(char_call[i] == 0) break;
+    }
+    char_call[i+1] ='\0'; //null terminate the string 
+}
+
+void readPanelSettings() {
+
+    //****** First set of 8 Panels ******
+    uint16_t offset = OffsetBITpanel * panel;
+
+ //   setBit(panA_REG[panel], Cen_BIT, readEEPROM(panCenter_en_ADDR + offset));
+ //   panCenter_XY[0][panel] = readEEPROM(panCenter_x_ADDR + offset);
+ //   panCenter_XY[1][panel] = checkPAL(readEEPROM(panCenter_y_ADDR + offset));
+
+    setBit(panA_REG[panel], Bp_BIT, readEEPROM(panBatteryPercent_en_ADDR + offset));
+    panBatteryPercent_XY[0][panel] = readEEPROM(panBatteryPercent_x_ADDR + offset);
+    panBatteryPercent_XY[1][panel] = checkPAL(readEEPROM(panBatteryPercent_y_ADDR + offset));
+
+    setBit(panA_REG[panel], Pit_BIT, readEEPROM(panPitch_en_ADDR + offset));
+    panPitch_XY[0][panel] = readEEPROM(panPitch_x_ADDR + offset);
+    panPitch_XY[1][panel] = checkPAL(readEEPROM(panPitch_y_ADDR + offset));
+
+    setBit(panA_REG[panel], Rol_BIT, readEEPROM(panRoll_en_ADDR + offset));
+    panRoll_XY[0][panel] = readEEPROM(panRoll_x_ADDR + offset);
+    panRoll_XY[1][panel] = checkPAL(readEEPROM(panRoll_y_ADDR + offset));
+
+    setBit(panA_REG[panel], BatA_BIT, readEEPROM(panBatt_A_en_ADDR + offset));
+    panBatt_A_XY[0][panel] = readEEPROM(panBatt_A_x_ADDR + offset);
+    panBatt_A_XY[1][panel] = checkPAL(readEEPROM(panBatt_A_y_ADDR + offset));
+
+    //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[panel], GPSats_BIT, readEEPROM(panGPSats_en_ADDR + offset));
+    panGPSats_XY[0][panel] = readEEPROM(panGPSats_x_ADDR + offset);
+    panGPSats_XY[1][panel] = checkPAL(readEEPROM(panGPSats_y_ADDR + offset));
+
+    setBit(panA_REG[panel], COG_BIT, readEEPROM(panCOG_en_ADDR + offset));
+    panCOG_XY[0][panel] = readEEPROM(panCOG_x_ADDR + offset);
+    panCOG_XY[1][panel] = checkPAL(readEEPROM(panCOG_y_ADDR + offset));
+
+    setBit(panA_REG[panel], GPS_BIT, readEEPROM(panGPS_en_ADDR + offset));
+    panGPS_XY[0][panel] = readEEPROM(panGPS_x_ADDR + offset);
+    panGPS_XY[1][panel] = checkPAL(readEEPROM(panGPS_y_ADDR + offset));
+
+    //****** Second set of 8 Panels ******
+
+    setBit(panB_REG[panel], Rose_BIT, readEEPROM(panRose_en_ADDR + offset));
+    panRose_XY[0][panel] = readEEPROM(panRose_x_ADDR + offset);
+    panRose_XY[1][panel] = checkPAL(readEEPROM(panRose_y_ADDR + offset));
+
+    setBit(panB_REG[panel], Head_BIT, readEEPROM(panHeading_en_ADDR + offset));
+    panHeading_XY[0][panel] = readEEPROM(panHeading_x_ADDR + offset);
+    panHeading_XY[1][panel] = checkPAL(readEEPROM(panHeading_y_ADDR + offset));
+
+//    setBit(panB_REG[panel], MavB_BIT, readEEPROM(panMavBeat_en_ADDR + offset));
+//    panMavBeat_XY[0][panel] = readEEPROM(panMavBeat_x_ADDR + offset);
+//    panMavBeat_XY[1][panel] = checkPAL(readEEPROM(panMavBeat_y_ADDR + offset));
+
+    setBit(panB_REG[panel], HDis_BIT, readEEPROM(panHomeDis_en_ADDR + offset));
+    panHomeDis_XY[0][panel] = readEEPROM(panHomeDis_x_ADDR + offset);
+    panHomeDis_XY[1][panel] = checkPAL(readEEPROM(panHomeDis_y_ADDR + offset));
+
+    setBit(panB_REG[panel], HDir_BIT, readEEPROM(panHomeDir_en_ADDR + offset));
+    panHomeDir_XY[0][panel] = readEEPROM(panHomeDir_x_ADDR + offset);
+    panHomeDir_XY[1][panel] = checkPAL(readEEPROM(panHomeDir_y_ADDR + offset));
+
+//    setBit(panB_REG[panel], WDir_BIT, readEEPROM(panWPDir_en_ADDR + offset));
+//    panWPDir_XY[0][panel] = readEEPROM(panWPDir_x_ADDR + offset);
+//    panWPDir_XY[1][panel] = checkPAL(readEEPROM(panWPDir_y_ADDR + offset));
+
+    setBit(panB_REG[panel], WDis_BIT, readEEPROM(panWPDis_en_ADDR + offset));
+    panWPDis_XY[0][panel] = readEEPROM(panWPDis_x_ADDR + offset);
+    panWPDis_XY[1][panel] = checkPAL(readEEPROM(panWPDis_y_ADDR + offset));
+
+    setBit(panB_REG[panel], Time_BIT, readEEPROM(panTime_en_ADDR + offset));
+    panTime_XY[0][panel] = readEEPROM(panTime_x_ADDR + offset);
+    panTime_XY[1][panel] = checkPAL(readEEPROM(panTime_y_ADDR + offset));
+
+    //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[panel], CurA_BIT, readEEPROM(panCur_A_en_ADDR + offset));
+    panCur_A_XY[0][panel] = readEEPROM(panCur_A_x_ADDR + offset);
+    panCur_A_XY[1][panel] = checkPAL(readEEPROM(panCur_A_y_ADDR + offset));
+
+    //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[panel], Alt_BIT, readEEPROM(panAlt_en_ADDR + offset));
+    panAlt_XY[0][panel] = readEEPROM(panAlt_x_ADDR + offset);
+    panAlt_XY[1][panel] = checkPAL(readEEPROM(panAlt_y_ADDR + offset));
+
+    setBit(panC_REG[panel], Halt_BIT, readEEPROM(panHomeAlt_en_ADDR + offset));
+    panHomeAlt_XY[0][panel] = readEEPROM(panHomeAlt_x_ADDR + offset);
+    panHomeAlt_XY[1][panel] = checkPAL(readEEPROM(panHomeAlt_y_ADDR + offset));
+
+    setBit(panC_REG[panel], As_BIT, readEEPROM(panAirSpeed_en_ADDR + offset));
+    panAirSpeed_XY[0][panel] = readEEPROM(panAirSpeed_x_ADDR + offset);
+    panAirSpeed_XY[1][panel] = checkPAL(readEEPROM(panAirSpeed_y_ADDR + offset));
+
+    setBit(panC_REG[panel], Vel_BIT, readEEPROM(panVel_en_ADDR + offset));
+    panVel_XY[0][panel] = readEEPROM(panVel_x_ADDR + offset);
+    panVel_XY[1][panel] = checkPAL(readEEPROM(panVel_y_ADDR + offset));
+
+    setBit(panC_REG[panel], Thr_BIT, readEEPROM(panThr_en_ADDR + offset));
+    panThr_XY[0][panel] = readEEPROM(panThr_x_ADDR + offset);
+    panThr_XY[1][panel] = checkPAL(readEEPROM(panThr_y_ADDR + offset));
+
+    setBit(panC_REG[panel], FMod_BIT, readEEPROM(panFMod_en_ADDR + offset));
+    panFMod_XY[0][panel] = readEEPROM(panFMod_x_ADDR + offset);
+    panFMod_XY[1][panel] = checkPAL(readEEPROM(panFMod_y_ADDR + offset));
+
+    setBit(panC_REG[panel], Hor_BIT, readEEPROM(panHorizon_en_ADDR + offset));
+    panHorizon_XY[0][panel] = readEEPROM(panHorizon_x_ADDR + offset);
+    panHorizon_XY[1][panel] = checkPAL(readEEPROM(panHorizon_y_ADDR + offset));
+
+    setBit(panD_REG[panel], Warn_BIT, readEEPROM(panWarn_en_ADDR + offset));
+    panWarn_XY[0][panel] = readEEPROM(panWarn_x_ADDR + offset);
+    panWarn_XY[1][panel] = checkPAL(readEEPROM(panWarn_y_ADDR + offset));
+
+    setBit(panD_REG[panel], WindS_BIT, readEEPROM(panWindSpeed_en_ADDR + offset));
+    panWindSpeed_XY[0][panel] = readEEPROM(panWindSpeed_x_ADDR + offset);
+    panWindSpeed_XY[1][panel] = checkPAL(readEEPROM(panWindSpeed_y_ADDR + offset));
+
+    setBit(panD_REG[panel], Climb_BIT, readEEPROM(panClimb_en_ADDR + offset));
+    panClimb_XY[0][panel] = readEEPROM(panClimb_x_ADDR + offset);
+    panClimb_XY[1][panel] = checkPAL(readEEPROM(panClimb_y_ADDR + offset));
+
+//    setBit(panD_REG[panel], Tune_BIT, readEEPROM(panTune_en_ADDR + offset));
+//    panTune_XY[0][panel] = readEEPROM(panTune_x_ADDR + offset);
+//    panTune_XY[1][panel] = checkPAL(readEEPROM(panTune_y_ADDR + offset));
+
+    setBit(panD_REG[panel], RSSI_BIT, readEEPROM(panRSSI_en_ADDR + offset));
+    panRSSI_XY[0][panel] = readEEPROM(panRSSI_x_ADDR + offset);
+    panRSSI_XY[1][panel] = checkPAL(readEEPROM(panRSSI_y_ADDR + offset));
+    
+    setBit(panD_REG[panel], Eff_BIT, readEEPROM(panEff_en_ADDR + offset));
+    panEff_XY[0][panel] = readEEPROM(panEff_x_ADDR + offset);
+    panEff_XY[1][panel] = checkPAL(readEEPROM(panEff_y_ADDR + offset));
+
+    setBit(panD_REG[panel], CALLSIGN_BIT, readEEPROM(panCALLSIGN_en_ADDR + offset));
+    panCALLSIGN_XY[0][panel] = readEEPROM(panCALLSIGN_x_ADDR + offset);
+    panCALLSIGN_XY[1][panel] = checkPAL(readEEPROM(panCALLSIGN_y_ADDR + offset));
+
+//   setBit(panE_REG[panel], Ch_BIT, readEEPROM(panCh_en_ADDR + offset));
+//    panCh_XY[0][panel] = readEEPROM(panCh_x_ADDR + offset);
+//    panCh_XY[1][panel] = checkPAL(readEEPROM(panCh_y_ADDR + offset));
+
+    setBit(panE_REG[panel], TEMP_BIT, readEEPROM(panTemp_en_ADDR + offset));
+    panTemp_XY[0][panel] = readEEPROM(panTemp_x_ADDR + offset);
+    panTemp_XY[1][panel] = checkPAL(readEEPROM(panTemp_y_ADDR + offset));
+
+    setBit(panE_REG[panel], DIST_BIT, readEEPROM(panDistance_en_ADDR + offset));
+    panDistance_XY[0][panel] = readEEPROM(panDistance_x_ADDR + offset);
+    panDistance_XY[1][panel] = checkPAL(readEEPROM(panDistance_y_ADDR + offset));
+}
+
+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 panelu, byte panel_x, byte panel_y, byte panel_s ) {
+    if(panel >= 1 && panel <= 32) {
+
+        writeEEPROM(panel_s, (6 * panelu) - 6 + 0);
+        if(panel_s != 0) {
+            writeEEPROM(panel_x, (6 * panelu) - 6 + 2);
+            writeEEPROM(panel_y, (6 * panelu) - 6 + 4);
+        }
+        osd.clear();
+        readSettings();
+        for(panel = 0; panel < npanels; panel++) readPanelSettings();
+    } 
+}
+

+

file:b/OSD_Func.h (new)
--- /dev/null
+++ b/OSD_Func.h
@@ -1,1 +1,127 @@
 
+//------------------ Heading and Compass ----------------------------------------
+
+static char buf_show[12];
+const char buf_Rule[36] = {0x82,0x80,0x81,0x80,0x81,0x80,
+                           0x84,0x80,0x81,0x80,0x81,0x80,
+                           0x83,0x80,0x81,0x80,0x81,0x80,
+                           0x85,0x80,0x81,0x80,0x81,0x80};
+void setHeadingPatern()
+{
+  int start;
+  start = round((osd_heading * 24)/360);
+  start -= 3;
+  if(start < 0) start += 24;
+  for(int x=0; x <= 10; x++){
+    buf_show[x] = buf_Rule[start];
+    if(++start > 23) start = 0;
+  }
+  buf_show[7] = '\0';
+}
+
+//------------------ Battery Remaining Picture ----------------------------------
+
+char setBatteryPic(uint16_t bat_level)
+{
+  if(bat_level <= 100){
+    return 0xb4;
+  }
+  else if(bat_level <= 300){
+    return 0xb5;
+  }
+  else if(bat_level <= 400){
+    return 0xb6;
+  }
+  else if(bat_level <= 500){
+    return 0xb7;
+  }
+  else if(bat_level <= 800){
+    return 0xb8;
+  }
+  else return 0xb9;
+}
+
+//------------------ Home Distance and Direction Calculation ----------------------------------
+
+void setHomeVars(OSD &osd)
+{
+  float dstlon, dstlat;
+  long bearing;
+  
+  if(osd_throttle > 3 && takeoff_heading == -400)
+    takeoff_heading = osd_heading;
+  osd_alt_to_home = (osd_alt - osd_home_alt);
+  if(osd_got_home == 0 && osd_fix_type > 1){
+    osd_home_lat = osd_lat;
+    osd_home_lon = osd_lon;
+    //osd_home_alt = osd_alt;
+    osd_got_home = 1;
+  }
+  else if(osd_got_home == 1){
+    // JRChange: osd_home_alt: check for stable osd_alt (must be stable for 25*120ms = 3s)
+    if(osd_alt_cnt < 25){
+      if(fabs(osd_alt_prev - osd_alt) > 0.5){
+        osd_alt_cnt = 0;
+        osd_alt_prev = osd_alt;
+      }
+      else
+      {
+        if(++osd_alt_cnt >= 25){
+          osd_home_alt = osd_alt;  // take this stable osd_alt as osd_home_alt
+          haltset = 1;
+        }
+      }
+    }
+    // shrinking factor for longitude going to poles direction
+    float rads = fabs(osd_home_lat) * 0.0174532925;
+    double scaleLongDown = cos(rads);
+    double scaleLongUp   = 1.0f/cos(rads);
+
+    //DST to Home
+    dstlat = fabs(osd_home_lat - osd_lat) * 111319.5;
+    dstlon = fabs(osd_home_lon - osd_lon) * 111319.5 * scaleLongDown;
+    osd_home_distance = sqrt(sq(dstlat) + sq(dstlon));
+
+    //DIR to Home
+    dstlon = (osd_home_lon - osd_lon); //OffSet_X
+    dstlat = (osd_home_lat - osd_lat) * scaleLongUp; //OffSet Y
+    bearing = 90 + (atan2(dstlat, -dstlon) * 57.295775); //absolut home direction
+    if(bearing < 0) bearing += 360;//normalization
+    bearing = bearing - 180;//absolut return direction
+    if(bearing < 0) bearing += 360;//normalization
+    bearing = bearing - osd_heading;//relative home direction
+    if(bearing < 0) bearing += 360; //normalization
+    osd_home_direction = round((float)(bearing/360.0f) * 16.0f) + 1;//array of arrows =)
+    if(osd_home_direction > 16) osd_home_direction = 0;
+
+  }
+
+}
+
+void setFdataVars(){
+
+  if (haltset == 1 && takeofftime == 0 && osd_alt_to_home > 5 && osd_throttle > 10){
+    takeofftime = 1;
+    tdistance = 0;
+    FTime = (millis()/1000);
+  }
+  
+//  if ((millis() - dt) >= 1000){
+//    if (osd_groundspeed > 1.0) tdistance = tdistance + (((millis() - dt) / 1000) * osd_groundspeed); 
+//  dt = millis();
+//  }
+
+  if (osd_groundspeed > 1.0) tdistance += (osd_groundspeed * (millis() - runt) / 1000.0);
+  mah_used += (osd_curr_A * 10.0 * (millis() - runt) / 3600000.0);
+  runt = millis();
+    
+  if (takeofftime == 1){
+    if (osd_home_distance > max_home_distance) max_home_distance = osd_home_distance;
+    if (osd_airspeed > max_osd_airspeed) max_osd_airspeed = osd_airspeed;
+    if (osd_groundspeed > max_osd_groundspeed) max_osd_groundspeed = osd_groundspeed;
+    if (osd_alt_to_home > max_osd_home_alt) max_osd_home_alt = osd_alt_to_home;
+    if (osd_windspeed > max_osd_windspeed) max_osd_windspeed = osd_windspeed;
+  }
+}
+

+

file:b/OSD_Panels.ino (new)
--- /dev/null
+++ b/OSD_Panels.ino
@@ -1,1 +1,1227 @@
-
+/******* STARTUP PANEL *******/
+
+void startPanels(){
+//    osd.clear();
+    //osd_clear = 3;
+    panLogo(); // Display our logo  
+    do_converts(); // load the unit conversion preferences
+}
+
+//------------------ Panel: Startup ArduCam OSD LOGO -------------------------------
+
+void panLogo(){
+    osd.setPanel(5, 5);
+    osd.openPanel();
+    osd.printf_P(PSTR("\xb0\xb1\xb2\xb3\xb4|\xb5\xb6\xb7\xb8\xb9|MinimOSD-Extra 2.4|Plane r628"));
+    osd.closePanel();
+}
+
+
+/******* PANELS - POSITION *******/
+
+void writePanels(){ 
+
+  if(millis() < (lastMAVBeat + 2200)){
+  
+    if (osd_alt_to_home <= 10 && osd_groundspeed <= 1 && osd_throttle <= 1 && takeofftime == 1 && osd_home_distance <= 100) {
+      landing = 1;
+          }else{
+          landed = millis();
+          }
+    if (landing == 1 && (millis() - 7000) > landed){ 
+       if (osd_clear == 0){
+         osd.clear(); 
+         osd_clear = 1;
+         panFdata();
+       }
+//        if ((millis() - 10000) > landing) panFdata();     
+        }else{
+//         landed = millis();
+         
+         if (osd_clear == 1){
+         osd.clear(); 
+         osd_clear = 0;                    
+       }
+      if(ch_toggle > 3) panOff(); // This must be first so you can always toggle
+            if(panel != npanels)
+            {
+                
+                //Testing bits from 8 bit register A 
+ //               if(ISa(panel,Cen_BIT)) panCenter(panCenter_XY[0][panel], panCenter_XY[1][panel]);   //4x2
+                if(ISd(panel,Warn_BIT)) panWarn(panWarn_XY[0][panel], panWarn_XY[1][panel]); // this must be here so warnings are always checked
+                if(ISa(panel,Pit_BIT)) panPitch(panPitch_XY[0][panel], panPitch_XY[1][panel]); //5x1
+                if(ISa(panel,Rol_BIT)) panRoll(panRoll_XY[0][panel], panRoll_XY[1][panel]); //5x1
+                if(ISa(panel,BatA_BIT)) panBatt_A(panBatt_A_XY[0][panel], panBatt_A_XY[1][panel]); //7x1
+                //  if(ISa(panel,BatB_BIT)) panBatt_B(panBatt_B_XY[0], panBatt_B_XY[1][panel]); //7x1
+                if(ISa(panel,GPSats_BIT)) panGPSats(panGPSats_XY[0][panel], panGPSats_XY[1][panel]); //5x1
+                if(ISa(panel,GPS_BIT)) panGPS(panGPS_XY[0][panel], panGPS_XY[1][panel]); //12x3
+                if(ISa(panel,Bp_BIT)) panBatteryPercent(panBatteryPercent_XY[0][panel], panBatteryPercent_XY[1][panel]); //
+                if(ISa(panel,COG_BIT)) panCOG(panCOG_XY[0][panel], panCOG_XY[1][panel]); //
+
+                //Testing bits from 8 bit register B
+                if(ISb(panel,Rose_BIT)) panRose(panRose_XY[0][panel], panRose_XY[1][panel]);        //13x3
+                if(ISb(panel,Head_BIT)) panHeading(panHeading_XY[0][panel], panHeading_XY[1][panel]); //13x3
+ //               if(ISb(panel,MavB_BIT)) panMavBeat(panMavBeat_XY[0][panel], panMavBeat_XY[1][panel]); //13x3
+
+                if(osd_got_home == 1){
+                    if(ISb(panel,HDis_BIT)) panHomeDis(panHomeDis_XY[0][panel], panHomeDis_XY[1][panel]); //13x3
+                    if(ISb(panel,HDir_BIT)) panHomeDir(panHomeDir_XY[0][panel], panHomeDir_XY[1][panel]); //13x3
+                }
+
+                if(ISb(panel,Time_BIT)) panTime(panTime_XY[0][panel], panTime_XY[1][panel]);
+ //               if(ISb(panel,WDir_BIT)) panWPDir(panWPDir_XY[0][panel], panWPDir_XY[1][panel]); //??x??
+                if(wp_number > 0){
+                if(ISb(panel,WDis_BIT)) panWPDis(panWPDis_XY[0][panel], panWPDis_XY[1][panel]); //??x??
+                }
+                //Testing bits from 8 bit register C 
+                //if(osd_got_home == 1){
+                if(ISc(panel,Alt_BIT)) panAlt(panAlt_XY[0][panel], panAlt_XY[1][panel]); //
+                if(ISc(panel,Halt_BIT)) panHomeAlt(panHomeAlt_XY[0][panel], panHomeAlt_XY[1][panel]); //
+                if(ISc(panel,Vel_BIT)) panVel(panVel_XY[0][panel], panVel_XY[1][panel]); //
+                if(ISc(panel,As_BIT)) panAirSpeed(panAirSpeed_XY[0][panel], panAirSpeed_XY[1][panel]); //
+
+                //}
+                if(ISc(panel,Thr_BIT)) panThr(panThr_XY[0][panel], panThr_XY[1][panel]); //
+                if(ISc(panel,FMod_BIT)) panFlightMode(panFMod_XY[0][panel], panFMod_XY[1][panel]);  //
+                if(ISc(panel,Hor_BIT)) panHorizon(panHorizon_XY[0][panel], panHorizon_XY[1][panel]); //14x5
+                if(ISc(panel,CurA_BIT)) panCur_A(panCur_A_XY[0][panel], panCur_A_XY[1][panel]);
+
+                //Testing bits from 8 bit register D 
+                //if(ISd(Off_BIT)) panOff(panOff_XY[0], panOff_XY[1]);
+                if(ISd(panel,WindS_BIT)) panWindSpeed(panWindSpeed_XY[0][panel], panWindSpeed_XY[1][panel]);
+                if(ISd(panel,Climb_BIT)) panClimb(panClimb_XY[0][panel], panClimb_XY[1][panel]);
+//                if(ISd(panel,Tune_BIT)) panTune(panTune_XY[0][panel], panTune_XY[1][panel]);
+                if(ISd(panel,RSSI_BIT)) panRSSI(panRSSI_XY[0][panel], panRSSI_XY[1][panel]); //??x??
+                if(ISd(panel,Eff_BIT)) panEff(panEff_XY[0][panel], panEff_XY[1][panel]);
+                if(ISd(panel,CALLSIGN_BIT)) panCALLSIGN(panCALLSIGN_XY[0][panel], panCALLSIGN_XY[1][panel]);
+                if(ISe(panel,TEMP_BIT)) panTemp(panTemp_XY[0][panel], panTemp_XY[1][panel]);
+//                if(ISe(panel,Ch_BIT)) panCh(panCh_XY[0][panel], panCh_XY[1][panel]);
+                if(ISe(panel,DIST_BIT)) panDistance(panDistance_XY[0][panel], panDistance_XY[1][panel]);
+            } else { //panel == npanels
+//                if(ISd(0,Warn_BIT)) panWarn(panWarn_XY[0][0], panWarn_XY[1][0]); // this must be here so warnings are always checked
+                if(ISd(0,CALLSIGN_BIT)) panCALLSIGN(panCALLSIGN_XY[0][panel], panCALLSIGN_XY[1][panel]); //call sign even in off panel
+            }
+ //       } else { // if (osd_on > 0)
+ //           panSetup();
+ //       }
+        }
+    //}
+   } else { // if no mavlink update for 2 secs
+    
+        // this could be replaced with a No Mavlink warning so the last seen values still show
+
+        osd.clear();
+        waitingMAVBeats = 1;
+        // Display our logo and wait... 
+    //    panWaitMAVBeats(5,10); //Waiting for MAVBeats...
+    panLogo();
+  }
+  
+    // OSD debug for development (Shown on top-middle panels) 
+#ifdef membug
+    osd.setPanel(13,4);
+    osd.openPanel();
+    osd.printf("%i",freeMem()); 
+    osd.closePanel();
+#endif
+  
+
+}
+/******* PANELS - DEFINITION *******/
+
+/* **************************************************************** */
+// Panel  : COG Course Over Ground
+// Needs  : X, Y locations
+// Output : 
+// Size   : 
+// Staus  : done
+
+void panCOG(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    
+//    osd_COG_arrow_rotate_int = (((int)osd_cog / 100 - (int)osd_heading)/360 * 16 + 16) % 16 + 1; // [1, 16]
+//    osd_COG_arrow_rotate_int = (osd_cog / 100 - osd_heading) / 360 * 16 + 1;
+    osd_COG_arrow_rotate_int = round(((osd_cog / 100) - osd_heading)/360.0 * 16.0 +1); //Convert to int 1-16 
+    if(osd_COG_arrow_rotate_int < 0 ) osd_COG_arrow_rotate_int += 16;
+    if(osd_COG_arrow_rotate_int == 0) osd_COG_arrow_rotate_int = 16;    
+    if(osd_COG_arrow_rotate_int == 17) osd_COG_arrow_rotate_int = 1;
+    
+    if (((osd_cog / 100) - osd_heading) > 180){
+       off_course = (osd_cog / 100 - osd_heading) - 360;
+    }else if (((osd_cog / 100) - osd_heading) < -180){
+       off_course = (osd_cog / 100 - osd_heading) + 360;
+    }else{
+       off_course = (osd_cog / 100 - osd_heading);
+    }
+    
+    showArrow((uint8_t)osd_COG_arrow_rotate_int,2);
+
+    osd.closePanel();
+}
+
+
+/* **************************************************************** */
+// Panel  : ODO
+// Needs  : X, Y locations
+// Output : 
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panDistance(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    //do_converts();
+    if ((tdistance * converth) > 1000.0) {
+    osd.printf("%c%5.2f%c", 0x8f, ((tdistance * converth) / distconv), distchar);
+    }else{
+    osd.printf("%c%5.0f%c", 0x8f, (tdistance * converth), high);
+    }
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panFdata
+// Needs  : X, Y locations
+// Output : 
+// Size   : 
+// Staus  : done
+void panFdata(){
+     osd.setPanel(11, 3);
+    osd.openPanel();                          
+//    osd.printf("%c%3i%c%02i|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60, 0x0b, ((max_home_distance) * converth), high, 0x1b, ((tdistance) * converth), high, 0x13,(max_osd_airspeed * converts), spe,0x14,(max_osd_groundspeed * converts),spe,0x12, (max_osd_home_alt * converth), high,0x1d,(max_osd_windspeed * converts),spe);
+    osd.printf("%c%3i%c%02i|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5.0f%c|%c%10.6f|%c%10.6f", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60, 0x0b, (int)((max_home_distance) * converth), high, 0x8f, (int)((tdistance) * converth), high, 0x13,(int)(max_osd_airspeed * converts), spe,0x14,(int)(max_osd_groundspeed * converts),spe,0x12, (int)(max_osd_home_alt * converth), high,0x1d,(int)(max_osd_windspeed * converts),spe, 0x17, mah_used, 0x01, 0x03, (double)osd_lat, 0x04, (double)osd_lon);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : pantemp
+// Needs  : X, Y locations
+// Output : 
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panTemp(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%c%5.1f%c", 0x0a, (float(temperature * tempconv + tempconvAdd) / 100), temps);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : efficiency
+// Needs  : X, Y locations
+// Output : 
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panEff(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    if (osd_throttle >= 1){
+      if (ma == 0) {
+              ma = 1;
+            }
+        if (osd_groundspeed != 0) eff = (float(osd_curr_A * 10) / (osd_groundspeed * converts))* 0.5 + eff * 0.5;
+//        eff = eff * 0.2 + eff * 0.8;
+          if (eff > 0 && eff <= 9999) {
+            osd.printf("%c%4.0f%c", 0x16, (double)eff, 0x01);
+          }else{
+          osd.printf_P(PSTR("\x16\x20\x20\x20\x20\x20")); 
+          }
+          
+    }else{
+         
+        if ((osd_throttle < 1)){
+            if (ma == 1) {
+              palt = osd_alt_to_home;
+//              descendt = millis();
+              ddistance = tdistance;
+              ma = 0;
+            }
+          }
+            if (osd_climb < -0.05){ 
+//            glide = ((osd_alt_to_home / (palt - osd_alt_to_home)) * ((millis() - descendt) / 1000)) * osd_groundspeed;
+            glide = (osd_alt_to_home / (palt - osd_alt_to_home)) * (tdistance - ddistance);
+            if (glide > 9999) glide = 9999;
+             if (glide != 'inf' && glide > -0){
+            osd.printf("%c%4.0f%c", 0x18, glide, 0x0c);
+             }
+            }
+            else if (osd_climb > 0.0 && osd_pitch <= 0) {
+              osd.printf_P(PSTR("\x18\x20\x20\x90\x91\x20"));   
+            }else{
+              osd.printf_P(PSTR("\x18\x20\x20\x20\x20\x20")); 
+            }
+            
+        
+    }
+
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panCh
+// Needs  : X, Y locations
+// Output : Scaled channel values from MAVLink
+// Size   
+// Staus  : done
+
+//void panCh(int first_col, int first_line){
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+    
+//    osd.printf("%c%c%5i|%c%c%5i|%c%c%5i|%c%c%5i|%c%c%5i|%c%c%5i", 0x43, 0x31, chan1_raw, 0x43, 0x32, chan2_raw, 0x43, 0x33, chan3_raw, 0x43, 0x34, chan4_raw, 0x43, 0x35, chan5_raw, 0x43, 0x36, chan6_raw); 
+//    osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panRSSI
+// Needs  : X, Y locations
+// Output : Alt symbol and altitude value in meters from MAVLink
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panRSSI(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    rssi = (int16_t)osd_rssi;
+    //if (rssi > rssical) rssi = rssical;
+    //else if (rssi < rssipersent) rssi = rssipersent;
+
+    if(!rssiraw_on) rssi = (int16_t)((float)(rssi - rssipersent)/(float)(rssical-rssipersent)*100.0f);
+//    if (rssi < -99) rssi = -99;
+    osd.printf("%c%3i%c", 0x09, rssi, 0x25);
+//    osd.printf("%c%3i%c", 0x09, osd_clear, 0x25); 
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panCALLSIGN
+// Needs  : X, Y locations
+// Output : Call sign identification
+// Size   : 1 x 6Hea  (rows x chars)
+// Staus  : done
+
+void panCALLSIGN(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    //osd.printf("%c%c%c%c%c%c", char_call[0], char_call[1], char_call[2], char_call[3], char_call[4], char_call[5]); 
+    //During the first 1000 miliseconds of each minute show callsign
+//    if(millis() <= 1000){
+//      osd.printf("%s", char_call); 
+//    }else{
+//    osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
+//      osd.printf("%s",strclear);
+    
+//    if(millis() % 60000
+    if ((millis() - 60000) > CallSignBlink){
+      if (millis() - 61000 > CallSignBlink){
+        CallSignBlink = millis();
+          }
+      osd.printf("%s", char_call); 
+    }else{
+//    osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
+      osd.printf("%s",strclear);
+    }
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panSetup
+// Needs  : Nothing, uses whole screen
+// Output : The settings menu
+// Size   : 3 x ?? (rows x chars)
+// Staus  : done
+
+//void panSetup(){
+
+//    if (millis() > text_timer){
+//        text_timer = millis() + 500;
+
+//        osd.clear();
+//        osd.setPanel(5, 7);
+//        osd.openPanel();
+
+//        if (chan1_raw_middle == 0 && chan2_raw_middle == 0){
+//            chan1_raw_middle = chan1_raw;
+//            chan2_raw_middle = chan2_raw;
+//        }
+
+//        if ((chan2_raw - 100) > chan2_raw_middle ) setup_menu++;  //= setup_menu + 1;
+//        else if ((chan2_raw + 100) < chan2_raw_middle ) setup_menu--;  //= setup_menu - 1;
+//        if (setup_menu < 0) setup_menu = 0;
+//        else if (setup_menu > 2) setup_menu = 2;
+
+
+//        switch (setup_menu){
+//        case 0:
+//            {
+//                osd.printf_P(PSTR("    Overspeed    "));
+//                osd.printf("%3.0i%c", overspeed, spe);
+//                overspeed = change_val(overspeed, overspeed_ADDR);
+//                break;
+//            }
+//        case 1:
+//            {
+//                osd.printf_P(PSTR("   Stall Speed   "));
+//                osd.printf("%3.0i%c", stall , spe);
+//                //overwritedisplay();
+//                stall = change_val(stall, stall_ADDR);
+//                break;
+//            }
+//        case 2:
+//            {
+//                osd.printf_P(PSTR("Battery warning "));
+//                osd.printf("%3.1f%c", float(battv)/10.0 , 0x76, 0x20);
+//                battv = change_val(battv, battv_ADDR);
+//                break;
+//            }
+            //      case 4:
+            //        osd.printf_P(PSTR("Battery warning "));
+            //        osd.printf("%3.0i%c", battp , 0x25);
+            //        if ((chan1_raw - 100) > chan1_raw_middle ){
+            //        battp = battp - 1;}
+            //        if ((chan1_raw + 100) < chan1_raw_middle ){
+            //        battp = battp + 1;} 
+            //        EEPROM.write(208, battp);
+            //        break;
+//        }
+//}
+//    osd.closePanel();
+//}
+
+//int change_val(int value, int address)
+//{
+//    uint8_t value_old = value;
+//    if (chan1_raw > chan1_raw_middle + 100) value--;
+//    if (chan1_raw  < chan1_raw_middle - 100) value++;
+
+//    if(value != value_old && setup_menu ) EEPROM.write(address, value);
+//    return value;
+//}
+
+/* **************************************************************** */
+// Panel  : pan wind speed
+// Needs  : X, Y locations
+// Output : Wind direction symbol (arrow) and velocity
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panWindSpeed(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+
+//    osd_wind_arrow_rotate_int = round((osd_winddirection - osd_heading)/360.0 * 16.0) + 1; //Convert to int 1-16 
+//    if(osd_wind_arrow_rotate_int < -7 ) {
+//    osd_wind_arrow_rotate_int += 24;
+//    }else if(osd_wind_arrow_rotate_int > 8 ) {
+//    osd_wind_arrow_rotate_int -= 8;
+//    }else{
+//    osd_wind_arrow_rotate_int += 8;
+//    }
+//    nor_osd_windspeed = osd_windspeed * 0.005 + nor_osd_windspeed * 0.995;
+    
+    if (osd_winddirection < 0){
+    osd_wind_arrow_rotate_int = round(((osd_winddirection + 360) - osd_heading)/360.0 * 16.0) + 9; //Convert to int 1-16
+    }else{
+    osd_wind_arrow_rotate_int = round((osd_winddirection - osd_heading)/360.0 * 16.0) + 9; //Convert to int 1-16
+    }
+    if(osd_wind_arrow_rotate_int > 16 ) osd_wind_arrow_rotate_int -= 16; //normalize
+    if(osd_wind_arrow_rotate_int < 1 ) osd_wind_arrow_rotate_int += 16; //normalize
+    nor_osd_windspeed = osd_windspeed * 0.010 + nor_osd_windspeed * 0.990;    
+    
+    showArrow((uint8_t)osd_wind_arrow_rotate_int,1); //print data to OSD
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panOff
+// Needs  : X, Y locations
+// Output : OSD off
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panOff(){
+    if (ch_toggle == 4){
+        if ((osd_mode != 11) && (osd_mode != 1)){
+            if (osd_off_switch != osd_mode){ 
+                osd_off_switch = osd_mode;
+                osd_switch_time = millis();
+
+                if (osd_off_switch == osd_switch_last){
+                    if(panel == 0)
+                        {
+                            panel = 1;                                                        
+//                            if (millis() <= 60000){
+//                                osd_set = 1;
+//                            }else{
+//                                osd_set = 0;
+//                            }                            
+                        }
+                    else if(panel == 1)
+                        {
+                            panel = npanels;
+//                            osd_set = 0;                            
+                        }
+                    else if(panel == npanels)
+                        {
+                            panel = 0;
+                        }
+                    osd.clear();
+                }
+            }
+            if ((millis() - osd_switch_time) > 2000){
+                osd_switch_last = osd_mode;
+            }
+        }
+    }
+    else {
+        if(ch_toggle == 5) ch_raw = chan5_raw;
+        else if(ch_toggle == 6) ch_raw = chan6_raw;
+        else if(ch_toggle == 7) ch_raw = chan7_raw;
+        else if(ch_toggle == 8) ch_raw = chan8_raw;
+
+        if (switch_mode == 0){
+            if (ch_raw > 1800 && warning != 1) {
+//                if (millis() <= 60000){
+//                    osd_set = 1;
+//                }
+//                else if (osd_set != 1 && warning != 1){
+//              if (warning != 1){
+                osd.clear();
+//                }
+                panel = npanels; //off panel
+            }
+            else if (ch_raw < 1200 && panel != 0) { //first panel
+ //               osd_set = 0;
+                osd.clear();
+                panel = 0;
+            }    
+
+ // else if (ch_raw >= 1200 && ch_raw <= 1800 && setup_menu != 6 && panel != 1 && warning != 1) { //second panel
+    else if (ch_raw >= 1200 && ch_raw <= 1800 && panel != 1 && warning != 1) { //second panel
+ //               osd_set = 0;
+                osd.clear();
+                panel = 1;
+            }        
+        } else {
+
+            if (ch_raw > 1200)
+ //               if (millis() <= 60000 && osd_set != 1){
+ //                   if (osd_switch_time + 1000 < millis()){
+ //                       osd_set = 1;
+ //                       osd_switch_time = millis();
+ //                   }
+ //               } else {
+                    if (osd_switch_time + 1000 < millis()){
+ //                       osd_set = 0;
+                        osd.clear();
+                        if (panel == npanels) {
+                            panel = 0;
+                        } else {
+                            panel++;
+                        }
+                        if (panel > 1) panel = npanels;
+                        osd_switch_time = millis();
+                    }
+  //              }
+        }    
+    }
+}
+//* **************************************************************** */
+// Panel  : panTune
+// Needs  : X, Y locations
+// Output : Current symbol and altitude value in meters from MAVLink
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+    
+//  void panTune(int first_col, int first_line){
+//  osd.setPanel(first_col, first_line);
+//  osd.openPanel();
+
+//  osd.printf("%c%c%2.0f%c|%c%c%2.0f%c|%c%c%4.0i%c|%c%c%4.0i%c|%c%c%3.0f%c|%c%c%3.0f%c|%c%c%3.0f%c", 0x4E, 0x52, (nav_roll), 0x05, 0x4E, 0x50, (nav_pitch), 0x05, 0x4E, 0x48, (nav_bearing), 0x05, 0x54, 0x42, (wp_target_bearing), 0x05, 0x41, 0x45, (alt_error * converth), high, 0x58, 0x45, (xtrack_error), 0x6D, 0x41, 0x45, ((aspd_error / 100.0) * converts), spe);
+
+//  osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panCur_A
+// Needs  : X, Y locations
+// Output : Current symbol and altitude value in meters from MAVLink
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panCur_A(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%c%5.2f%c", 0xbd, (float(osd_curr_A) * 0.01), 0x0e);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panAlt
+// Needs  : X, Y locations
+// Output : Alt symbol and altitude value in meters from MAVLink
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panAlt(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    //osd.printf("%c%5.0f%c",0x85, (double)osd_alt_to_home, 0x0c);
+    osd.printf("%c%5.0f%c",0x11, (double)(osd_alt * converth), high);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panClimb
+// Needs  : X, Y locations
+// Output : Alt symbol and altitude value in meters from MAVLink
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panClimb(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    vs = (osd_climb * converth * 60) * 0.1 + vs * 0.9;
+    osd.printf("%c%4.0f%c%c",0x15, int(vs / 10.0) * 10.0, climbchar, 0x20);
+    osd.closePanel();
+    
+}
+
+/* **************************************************************** */
+// Panel  : panHomeAlt
+// Needs  : X, Y locations
+// Output : Alt symbol and home altitude value in meters from MAVLink
+// Size   : 1 x 7Hea  (rows x chars)
+// Staus  : done
+
+void panHomeAlt(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();   
+    osd.printf("%c%5.0f%c",0x12, (double)(osd_alt_to_home * converth), high);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panVel
+// Needs  : X, Y locations
+// Output : Velocity value from MAVlink with symbols
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panVel(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    
+    osd.printf("%c%3.0f%c",0x14,(double)(osd_groundspeed * converts),spe);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panAirSpeed
+// Needs  : X, Y locations
+// Output : Airspeed value from MAVlink with symbols
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panAirSpeed(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%c%3.0f%c", 0x13, (double)(osd_airspeed * converts), spe);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panWarn
+// Needs  : X, Y locations
+// Output : Airspeed value from MAVlink with symbols
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panWarn(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+
+    if (millis() > text_timer){ // if the text has been shown for a while
+        if (warning_type != 0) {
+            last_warning = warning_type; // save the warning type for cycling
+            warning_type = 0; // blank the text
+            warning = 1;
+            warning_timer = millis();            
+        } else {
+            if ((millis() - 10000) > warning_timer ) warning = 0;
+
+            int x = last_warning; // start the warning checks where we left it last time
+            while (warning_type == 0) { // cycle through the warning checks
+                x++;
+                if (x > 5) x = 1; // change the 6 if you add more warning types
+                if(x == 1) {if ((osd_fix_type) < 2) warning_type = 1;} // No GPS Fix
+                else if(x == 2) {if (osd_airspeed * converts < stall && osd_airspeed > 1.12) warning_type = 2;}
+                else if(x == 3) {if ((osd_airspeed * converts) > (float)overspeed) warning_type = 3;}
+                else if(x == 4) {if (osd_vbat_A < float(battv)/10.0 || (osd_battery_remaining_A < batt_warn_level && batt_warn_level != 0)) warning_type = 4;}
+                else if(x == 5) {if (rssi < rssi_warn_level && rssi != -99 && !rssiraw_on) warning_type = 5;}
+                if (x == last_warning) break; // if we've done a full cycle then there mustn't be any warnings
+            }
+        }
+
+        text_timer = millis() + 1000; // blink every 1 secs
+        if (warning == 1){ 
+            if (panel == 1) osd.clear();
+            panel = 0; // turn OSD on if there is a warning                  
+        }
+        char* warning_string;
+//        if (motor_armed == 0){
+//            warning_string = "\x20\x20\x44\x49\x53\x41\x52\x4d\x45\x44\x20\x20";      
+//        }else{
+            if (warning_type == 0) warning_string = "\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20";
+            else if (warning_type == 1) warning_string = "\x20\x4E\x6F\x20\x47\x50\x53\x20\x66\x69\x78\x21";
+            else if (warning_type == 2) warning_string = "\x20\x20\x20\x53\x74\x61\x6c\x6c\x21\x20\x20\x20";
+            else if (warning_type == 3) warning_string = "\x20\x4f\x76\x65\x72\x53\x70\x65\x65\x64\x21\x20";
+            else if (warning_type == 4) warning_string = "\x42\x61\x74\x74\x65\x72\x79\x20\x4c\x6f\x77\x21";
+            else if (warning_type == 5) warning_string = "\x20\x20\x4c\x6f\x77\x20\x52\x73\x73\x69\x20\x20";
+                //        case 6:
+                //osd.printf_P(PSTR("\x42\x61\x74\x74\x65\x72\x79\x20\x4c\x6f\x77\x21"));
+                //            warning_string = "\x20\x20\x44\x49\x53\x41\x52\x4d\x45\x44\x20\x20";
+                //            break;
+//            }
+        osd.printf("%s",warning_string);
+    }
+    osd.closePanel();
+}
+
+  
+/* **************************************************************** */
+// Panel  : panThr
+// Needs  : X, Y locations
+// Output : Throttle value from MAVlink with symbols
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panThr(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%c%3.0i%c",0x02,osd_throttle,0x25);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panBatteryPercent
+// Needs  : X, Y locations
+// Output : Battery state from MAVlink with symbols
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panBatteryPercent(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    
+    if (EEPROM.read(OSD_BATT_SHOW_PERCENT_ADDR) == 1){
+    osd.printf("%c%3.0i%c", 0x17, osd_battery_remaining_A, 0x25);
+    }else{
+    osd.printf("%c%5.0f%c",0x17, mah_used, 0x01);
+    }
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panTime
+// Needs  : X, Y locations
+// Output : Time from start with symbols
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panTime(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    
+    start_Time = (millis()/1000) - FTime;
+    osd.printf("%c%2i%c%02i", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panHomeDis
+// Needs  : X, Y locations
+// Output : Home Symbol with distance to home in meters
+// Size   : 1 x 7  (rows x chars)
+// Staus  : done
+
+void panHomeDis(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%c%5.0f%c", 0x0b, (double)((osd_home_distance) * converth), high);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panCenter
+// Needs  : X, Y locations
+// Output : 2 row croshair symbol created by 2 x 4 chars
+// Size   : 2 x 4  (rows x chars)
+// Staus  : done
+
+//void panCenter(int first_col, int first_line){
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+//    osd.printf_P(PSTR("\x05\x03\x04\x05|\x15\x13\x14\x15"));
+//    osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panHorizon
+// Needs  : X, Y locations
+// Output : 12 x 4 Horizon line surrounded by 2 cols (left/right rules)
+// Size   : 14 x 4  (rows x chars)
+// Staus  : done
+
+void panHorizon(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20|\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20|\xc6\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc5|\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20|\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20|"));
+    osd.closePanel();
+    showHorizon((first_col + 1), first_line);
+    //Show ILS on  HUD
+//    showILS(first_col, first_line);
+}
+
+/* **************************************************************** */
+// Panel  : panPitch
+// Needs  : X, Y locations
+// Output : -+ value of current Pitch from vehicle with degree symbols and pitch symbol
+// Size   : 1 x 6  (rows x chars)
+// Staus  : done
+
+void panPitch(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%4i%c%c",osd_pitch,0x05,0x07);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panRoll
+// Needs  : X, Y locations
+// Output : -+ value of current Roll from vehicle with degree symbols and roll symbol
+// Size   : 1 x 6  (rows x chars)
+// Staus  : done
+
+void panRoll(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%4i%c%c",osd_roll,0x05,0x06);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panBattery A (Voltage 1)
+// Needs  : X, Y locations
+// Output : Voltage value as in XX.X and symbol of over all battery status
+// Size   : 1 x 8  (rows x chars)
+// Staus  : done
+
+void panBatt_A(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    /*************** This commented code is for the next ArduPlane Version
+    if(osd_battery_remaining_A > 100){
+        osd.printf(" %c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
+    else osd.printf("%c%5.2f%c%c", 0xbc, (double)osd_vbat_A, 0x0d, osd_battery_pic_A);
+    */
+    osd.printf("%c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
+    osd.closePanel();
+}
+
+//------------------ Panel: Waiting for MAVLink HeartBeats -------------------------------
+
+//void panWaitMAVBeats(int first_col, int first_line){
+//    panLogo();
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+//    osd.printf_P(PSTR("Waiting for|MAVLink heartbeats..."));
+//    osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panGPL
+// Needs  : X, Y locations
+// Output : 1 static symbol with changing FIX symbol
+// Size   : 1 x 2  (rows x chars)
+// Staus  : done
+
+//void panGPL(int first_col, int first_line){
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+//    char* gps_str;
+//    if(osd_fix_type == 0 || osd_fix_type == 1) gps_str = "\x10\x20"; 
+        //osd.printf_P(PSTR("\x10\x20"));
+//    else if(osd_fix_type == 2 || osd_fix_type == 3) gps_str = "\x11\x20";
+        //osd.printf_P(PSTR("\x11\x20"));
+//    osd.printf("%s",gps_str);
+
+    /*  if(osd_fix_type <= 1) {
+    osd.printf_P(PSTR("\x10"));
+    } else {
+    osd.printf_P(PSTR("\x11"));
+    }  */
+//    osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panGPSats
+// Needs  : X, Y locations
+// Output : 1 symbol and number of locked satellites
+// Size   : 1 x 5  (rows x chars)
+// Staus  : done
+
+void panGPSats(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    
+    byte gps_str = 0x1f;
+    if(osd_fix_type >= 2)
+      gps_str = 0x0f;
+    
+    osd.printf("%c%2i", gps_str, osd_satellites_visible);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panGPS
+// Needs  : X, Y locations
+// Output : two row numeric value of current GPS location with LAT/LON symbols as on first char
+// Size   : 2 x 12  (rows x chars)
+// Staus  : done
+
+void panGPS(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%c%10.6f|%c%10.6f", 0x03, (double)osd_lat, 0x04, (double)osd_lon);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panHeading
+// Needs  : X, Y locations
+// Output : Symbols with numeric compass heading value
+// Size   : 1 x 5  (rows x chars)
+// Staus  : not ready
+
+void panHeading(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    osd.printf("%4.0f%c", (double)osd_heading, 0x05);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panRose
+// Needs  : X, Y locations
+// Output : a dynamic compass rose that changes along the heading information
+// Size   : 2 x 13  (rows x chars)
+// Staus  : done
+
+void panRose(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    //osd_heading  = osd_yaw;
+    //if(osd_yaw < 0) osd_heading = 360 + osd_yaw;
+//    osd.printf("%s|%c%s%c", "\x20\xc0\xc0\xc0\xc0\xc0\xc7\xc0\xc0\xc0\xc0\xc0\x20", 0xc3, buf_show, 0x87);
+    osd.printf("%c%s%c", 0xc3, buf_show, 0x87);
+    osd.closePanel();
+}
+
+
+/* **************************************************************** */
+// Panel  : panBoot
+// Needs  : X, Y locations
+// Output : Booting up text and empty bar after that
+// Size   : 1 x 21  (rows x chars)
+// Staus  : done
+
+//void panBoot(int first_col, int first_line){
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+//    osd.printf_P(PSTR("Booting up:\x88\x8d\x8d\x8d\x8d\x8d\x8d\x8d\x8e")); 
+//    osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panMavBeat
+// Needs  : X, Y locations
+// Output : 2 symbols, one static and one that blinks on every 50th received 
+//          mavlink packet.
+// Size   : 1 x 2  (rows x chars)
+// Staus  : done
+
+//void panMavBeat(int first_col, int first_line){
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+//    if(mavbeat == 1){
+//        osd.printf_P(PSTR("\xEA\xEC"));
+//        mavbeat = 0;
+//    }
+//    else{
+//        osd.printf_P(PSTR("\xEA\xEB"));
+//    }
+//    osd.closePanel();
+//}
+
+
+/* **************************************************************** */
+// Panel  : panWPDir
+// Needs  : X, Y locations
+// Output : 2 symbols that are combined as one arrow, shows direction to next waypoint
+// Size   : 1 x 2  (rows x chars)
+// Staus  : not ready
+
+//void panWPDir(int first_col, int first_line){
+//    osd.setPanel(first_col, first_line);
+//    osd.openPanel();
+   
+//    wp_target_bearing_rotate_int = round(((float)wp_target_bearing - osd_heading)/360.0 * 16.0) + 1; //Convert to int 0-16 
+//    if(wp_target_bearing_rotate_int < 0 ) wp_target_bearing_rotate_int += 16; //normalize  
+
+//    showArrow((uint8_t)wp_target_bearing_rotate_int,0);
+//    osd.closePanel();
+//}
+
+/* **************************************************************** */
+// Panel  : panWPDis
+// Needs  : X, Y locations
+// Output : W then distance in Km - Distance to next waypoint
+// Size   : 1 x 2  (rows x chars)
+// Staus  : not ready TODO - CHANGE the Waypoint symbol - Now only a W!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+void panWPDis(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    
+    wp_target_bearing_rotate_int = round(((float)wp_target_bearing - osd_heading)/360.0 * 16.0) + 1; //Convert to int 0-16 
+    if(wp_target_bearing_rotate_int < 0 ) wp_target_bearing_rotate_int += 16; //normalize
+    else if(wp_target_bearing_rotate_int > 16 ) wp_target_bearing_rotate_int -= 16; //normalize
+      if (xtrack_error > 999) xtrack_error = 999;
+      else if (xtrack_error < -999) xtrack_error = -999;
+
+      osd.printf("%c%c%2i%c%4.0f%c|",0x57, 0x70, wp_number,0x0,(double)((float)(wp_dist) * converth),high);
+      showArrow((uint8_t)wp_target_bearing_rotate_int,0);
+      
+      if (osd_mode == 10 || osd_mode == 15){     
+        osd.printf("%c%c%c%4.0f%c", 0x20, 0x58, 0x65, (xtrack_error* converth), high);
+      }else{
+        osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
+          }
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panHomeDir
+// Needs  : X, Y locations
+// Output : 2 symbols that are combined as one arrow, shows direction to home
+// Size   : 1 x 2  (rows x chars)
+// Status : not tested
+
+void panHomeDir(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    showArrow((uint8_t)osd_home_direction,0);
+    osd.closePanel();
+}
+
+/* **************************************************************** */
+// Panel  : panFlightMode 
+// Needs  : X, Y locations
+// Output : 2 symbols, one static name symbol and another that changes by flight modes
+// Size   : 1 x 2  (rows x chars)
+// Status : done
+
+void panFlightMode(int first_col, int first_line){
+    osd.setPanel(first_col, first_line);
+    osd.openPanel();
+    //char c1 = 0x7f ;//"; char c2; char c3; char c4; char c5; 
+    char* mode_str="";
+    if (osd_mode == 0) mode_str = "manu"; //Manual 
+    if (osd_mode == 1) mode_str = "circ"; //CIRCLE 
+    if (osd_mode == 2) mode_str = "stab"; //Stabilize
+    if (osd_mode == 3) mode_str = "trai"; //Training
+    if (osd_mode == 4) mode_str = "acro"; //ACRO
+    if (osd_mode == 5) mode_str = "fbwa"; //FLY_BY_WIRE_A
+    if (osd_mode == 6) mode_str = "fbwb"; //FLY_BY_WIRE_B
+    if (osd_mode == 7) mode_str = "crui"; //Cruise
+    if (osd_mode == 10) mode_str = "auto"; //AUTO
+    if (osd_mode == 11) mode_str = "retl"; //Return to Launch 
+    if (osd_mode == 12) mode_str = "loit"; //Loiter
+    if (osd_mode == 15) mode_str = "guid"; //GUIDED
+    
+    
+    osd.printf("%c%s", 0x7f, mode_str);
+//      osd.printf("%c%i", 0x7f, osd_mode);
+    osd.closePanel();
+}
+
+
+// ---------------- EXTRA FUNCTIONS ----------------------
+// Show those fancy 2 char arrows
+
+void showArrow(uint8_t rotate_arrow,uint8_t method) {  
+    char arrow_set1 = 0x0;
+  
+    if(rotate_arrow == 0){
+      rotate_arrow = 1;
+    }
+    arrow_set1 = rotate_arrow * 2 + 0x8E;
+
+//    if(method == 1) osd.printf("%c%3.0f%c|%c%c%2.0f%c",0x1d,(double)(osd_windspeed * converts),spe, arrow_set1, arrow_set2,(double)(osd_windspeedz * converts),spe);
+    if(method == 1) osd.printf("%c%3.0f%c|%c%c%2.0f%c",0x1d,(double)(osd_windspeed * converts),spe, arrow_set1, arrow_set1 + 1,(double)(nor_osd_windspeed * converts),spe);
+    else if(method == 2) osd.printf("%c%c%4i%c", arrow_set1, arrow_set1 + 1, off_course, 0x05);   
+    else osd.printf("%c%c", arrow_set1, arrow_set1 + 1);
+}
+
+// Calculate and shows Artificial Horizon
+// Smooth horizon by Jörg Rothfuchs
+							// with different factors we can adapt do different cam optics
+#define AH_PITCH_FACTOR		0.010471976		// conversion factor for pitch
+#define AH_ROLL_FACTOR		0.017453293		// conversion factor for roll
+#define AH_COLS			12			// number of artificial horizon columns
+#define AH_ROWS			5			// number of artificial horizon rows
+#define CHAR_COLS		12			// number of MAX7456 char columns
+#define CHAR_ROWS		18			// number of MAX7456 char rows
+#define CHAR_SPECIAL		9			// number of MAX7456 special chars for the artificial horizon
+#define AH_TOTAL_LINES		AH_ROWS * CHAR_ROWS	// helper define
+
+
+#define LINE_SET_STRAIGHT__	(0xC7 - 1)		// code of the first MAX7456 straight char -1
+#define LINE_SET_STRAIGHT_O	(0xD0 - 3)		// code of the first MAX7456 straight overflow char -3
+#define LINE_SET_P___STAG_1	(0xD1 - 1)		// code of the first MAX7456 positive staggered set 1 char -1
+#define LINE_SET_P___STAG_2	(0xDA - 1)		// code of the first MAX7456 positive staggered set 2 char -1
+#define LINE_SET_N___STAG_1	(0xE3 - 1)		// code of the first MAX7456 negative staggered set 1 char -1
+#define LINE_SET_N___STAG_2	(0xEC - 1)		// code of the first MAX7456 negative staggered set 2 char -1
+#define LINE_SET_P_O_STAG_1	(0xF5 - 2)		// code of the first MAX7456 positive overflow staggered set 1 char -2
+#define LINE_SET_P_O_STAG_2	(0xF9 - 1)		// code of the first MAX7456 positive overflow staggered set 2 char -1
+#define LINE_SET_N_O_STAG_1	(0xF7 - 2)		// code of the first MAX7456 negative overflow staggered set 1 char -2
+#define LINE_SET_N_O_STAG_2	(0xFC - 1)		// code of the first MAX7456 negative overflow staggered set 2 char -1
+
+
+#define OVERFLOW_CHAR_OFFSET	6			// offset for the overflow subvals
+
+
+#define ANGLE_1			9			// angle above we switch to line set 1
+#define ANGLE_2			25			// angle above we switch to line set 2
+
+
+// Calculate and show artificial horizon
+// used formula: y = m * x + n <=> y = tan(a) * x + n
+void showHorizon(int start_col, int start_row) {
+    int col, row, pitch_line, middle, hit, subval;
+    int roll;
+    int line_set = LINE_SET_STRAIGHT__;
+    int line_set_overflow = LINE_SET_STRAIGHT_O;
+    int subval_overflow = 9;
+    
+    // preset the line char attributes
+    roll = osd_roll;
+    if ((roll >= 0 && roll < 90) || (roll >= -179 && roll < -90)) {	// positive angle line chars
+	roll = roll < 0 ? roll + 179 : roll;
+        if (abs(roll) > ANGLE_2) {
+	    line_set = LINE_SET_P___STAG_2;
+	    line_set_overflow = LINE_SET_P_O_STAG_2;
+            subval_overflow = 7;
+	} else if (abs(roll) > ANGLE_1) {
+	    line_set = LINE_SET_P___STAG_1;
+	    line_set_overflow = LINE_SET_P_O_STAG_1;
+            subval_overflow = 8;
+	}
+    } else {								// negative angle line chars
+	roll = roll > 90 ? roll - 179 : roll;
+        if (abs(roll) > ANGLE_2) {
+	    line_set = LINE_SET_N___STAG_2;
+	    line_set_overflow = LINE_SET_N_O_STAG_2;
+            subval_overflow = 7;
+	} else if (abs(roll) > ANGLE_1) {
+	    line_set = LINE_SET_N___STAG_1;
+	    line_set_overflow = LINE_SET_N_O_STAG_1;
+            subval_overflow = 8;
+	}
+    }
+    
+    pitch_line = round(tan(-AH_PITCH_FACTOR * osd_pitch) * AH_TOTAL_LINES) + AH_TOTAL_LINES/2;	// 90 total lines
+    for (col=1; col<=AH_COLS; col++) {
+        middle = col * CHAR_COLS - (AH_COLS/2 * CHAR_COLS) - CHAR_COLS/2;	  // -66 to +66	center X point at middle of each column
+        hit = tan(AH_ROLL_FACTOR * osd_roll) * middle + pitch_line;	          // 1 to 90	calculating hit point on Y plus offset
+        if (hit >= 1 && hit <= AH_TOTAL_LINES) {
+	    row = (hit-1) / CHAR_ROWS;						  // 0 to 4 bottom-up
+	    subval = (hit - (row * CHAR_ROWS) + 1) / (CHAR_ROWS / CHAR_SPECIAL);  // 1 to 9
+	    
+	    // print the line char
+            osd.openSingle(start_col + col - 1, start_row + AH_ROWS - row - 1);
+            osd.printf("%c", line_set + subval);
+	    
+	    // check if we have to print an overflow line char
+	    if (subval >= subval_overflow && row < 4) {	// only if it is a char which needs overflow and if it is not the upper most row
+                osd.openSingle(start_col + col - 1, start_row + AH_ROWS - row - 2);
+                osd.printf("%c", line_set_overflow + subval - OVERFLOW_CHAR_OFFSET);
+	    }
+        }
+    }
+}
+
+// Calculate and shows ILS
+void showILS(int start_col, int start_row) { 
+    //Vertical calculation
+    int currentAngleDisplacement = atan(osd_alt_to_home / osd_home_distance) * 57.2957795 - 10;
+    //Calc current char position 
+    //int numberOfPixels = CHAR_ROWS * AH_ROWS;
+    int totalNumberOfLines = 9 * AH_ROWS; //9 chars in chartset for vertical line
+    int linePosition = totalNumberOfLines * currentAngleDisplacement / 10 + (totalNumberOfLines / 2); //+-5 degrees
+    int charPosition = linePosition / 9;
+    int selectedChar = 9 - (linePosition % 9) + 0xC7;
+    if(charPosition >= 0 && charPosition <= CHAR_ROWS)
+    {
+      osd.openSingle(start_col + AH_COLS + 2, start_row + charPosition);
+      osd.printf("%c", selectedChar);
+    }
+    
+    //Horizontal calculation
+    currentAngleDisplacement = osd_home_direction - takeoff_heading;
+    //Horizontal calculation
+    totalNumberOfLines = 6 * AH_COLS; //6 chars in chartset for vertical line
+    linePosition = totalNumberOfLines * currentAngleDisplacement / 10 + (totalNumberOfLines / 2); //+-5 degrees
+    charPosition = linePosition / 6;
+    selectedChar = (linePosition % 6) + 0xBF;
+    if(charPosition >= 0 && charPosition <= CHAR_COLS)
+    {
+      osd.openSingle(start_col + charPosition, start_row + AH_ROWS + 1);
+      osd.printf("%c", selectedChar);
+    }
+}
+
+void do_converts()
+{
+    if (EEPROM.read(measure_ADDR) == 0) {
+        converts = 3.6;
+        converth = 1.0;
+        spe = 0x10;
+        high = 0x0c;
+        temps = 0xba;
+        tempconv = 10;
+        tempconvAdd = 0;
+        distchar = 0x1b;
+        distconv = 1000;
+        climbchar = 0x1a;
+    } else {
+        converts = 2.23;
+        converth = 3.28;
+        spe = 0x19;
+        high = 0x66;
+        temps = 0xbb;
+        tempconv = 18;
+        tempconvAdd = 3200;
+        distchar = 0x1c;
+        distconv = 5280;
+        climbchar = 0x1e;
+    }
+}
+
+

+

file:b/OSD_Vars.h (new)
--- /dev/null
+++ b/OSD_Vars.h
@@ -1,1 +1,220 @@
-
+/*Panels variables*/
+//Will come from APM telem port
+
+static float        max_home_distance = 0;
+static float        max_osd_airspeed = 0;
+static float        max_osd_groundspeed = 0; 
+static float        max_osd_home_alt = 0;
+static float        max_osd_windspeed = 0;
+static float        nor_osd_windspeed = 0;
+static float        vs = 0;
+
+static float tdistance = 0;
+static float ddistance = 0;
+static char strclear[]="\x20\x20\x20\x20\x20\x20\x20\x20";
+
+
+//static float	      nav_roll = 0; // Current desired roll in degrees
+//static float        nav_pitch = 0; // Current desired pitch in degrees
+//static int16_t      nav_bearing = 0; // Current desired heading in degrees
+static int16_t	    wp_target_bearing = 0; // Bearing to current MISSION/target in degrees
+static int8_t       wp_target_bearing_rotate_int = 0;
+static uint16_t     wp_dist = 0; // Distance to active MISSION in meters
+static uint8_t      wp_number = 0; // Current waypoint number
+//static float	      alt_error = 0; // Current altitude error in meters
+//static float        aspd_error = 0; // Current airspeed error in meters/second
+static float	    xtrack_error = 0; // Current crosstrack error on x-y plane in meters
+static float        eff = 0; //Efficiency
+
+static uint8_t      base_mode=0;
+static bool         motor_armed = 0;
+static bool         ma = 0;
+static bool         osd_clear = 0;
+static uint16_t     ch_raw = 0;
+//static uint16_t     chan1_raw = 0;
+//static uint16_t     chan2_raw = 0;
+//static uint16_t     chan3_raw = 0;
+//static uint16_t     chan4_raw = 0;
+static uint16_t     chan5_raw = 0;
+static uint16_t     chan6_raw = 0;
+static uint16_t     chan7_raw = 0;
+static uint16_t     chan8_raw = 0;
+//static uint16_t     chan1_raw_middle = 0;
+//static uint16_t     chan2_raw_middle = 0;
+
+static uint8_t      ch_toggle = 0;
+//static boolean      osd_set = 0;
+static boolean      switch_mode = 0;
+static boolean      takeofftime = 0;
+static boolean      haltset = 0;
+//static boolean      pal_ntsc = 0;
+
+//static int8_t       setup_menu = 0;
+static float        converts = 0;
+static float        converth = 0;
+static uint8_t      overspeed = 0;
+static uint8_t      stall = 0;
+static uint8_t      battv = 0; //Battery warning voltage - units Volt *10 
+static uint16_t     distconv = 0;
+//static int        battp = 0;
+
+static uint8_t      spe = 0;
+static uint8_t      high = 0;
+static uint8_t      temps = 0;
+static float        osd_vbat_A = 0;                 // Battery A voltage in milivolt
+static int16_t      osd_curr_A = 0;                 // Battery A current
+static float        mah_used = 0;
+static int8_t       osd_battery_remaining_A = 0;    // 0 to 100 <=> 0 to 1000
+static uint8_t      batt_warn_level = 0;
+
+//static uint8_t    osd_battery_pic_A = 0xb4;       // picture to show battery remaining
+//static float      osd_vbat_B = 0;               // voltage in milivolt
+//static float    timer_B = 0;                 // Battery B current
+//static uint16_t   osd_battery_remaining_B = 0;  // 0 to 100 <=> 0 to 1000
+//static uint8_t    osd_battery_pic_B = 0xb4;     // picture to show battery remaining
+static float        start_Time = -1.0;
+static uint8_t      osd_mode = 0;                   // Navigation mode from RC AC2 = CH5, APM = CH8
+static uint8_t      osd_nav_mode = 0;               // Navigation mode from RC AC2 = CH5, APM = CH8
+static unsigned long text_timer = 0;
+static unsigned long warning_timer =0;
+static unsigned long runt =0;
+static unsigned long FTime = 0;
+static unsigned long CallSignBlink = 0;
+static unsigned long landed = 0;
+
+static uint8_t      warning_type = 0;
+static uint8_t      last_warning = 0;
+static uint8_t      warning = 0;
+static uint8_t      osd_off_switch = 0;
+static uint8_t      osd_switch_last = 100;
+static unsigned long         osd_switch_time = 0;
+//static unsigned long         descendt = 0;
+static float         palt = 0;
+static float        osd_climb = 0;
+//static float        descend = 0;
+
+static float        osd_lat = 0;                    // latidude
+static float        osd_lon = 0;                    // longitude
+static uint8_t      osd_satellites_visible = 0;     // number of satelites
+static uint8_t      osd_fix_type = 0;               // GPS lock 0-1=no fix, 2=2D, 3=3D
+static uint16_t      osd_cog;                        // Course over ground
+static uint16_t        off_course;
+static uint8_t      osd_got_home = 0;               // tels if got home position or not
+static float        osd_home_lat = 0;               // home latidude
+static float        osd_home_lon = 0;               // home longitude
+static float        osd_home_alt = 0; 
+static float        osd_alt_to_home = 0; 
+static long         osd_home_distance = 0;          // distance from home
+static uint8_t      osd_home_direction;             // Arrow direction pointing to home (1-16 to CW loop)
+static int          takeoff_heading = -400;         // Calculated takeoff heading
+
+static int16_t       osd_pitch = 0;                  // pitch from DCM
+static int16_t       osd_roll = 0;                   // roll from DCM
+//static int8_t       osd_yaw = 0;                    // relative heading form DCM
+static float        osd_heading = 0;                // ground course heading from GPS
+static float        glide = 0;
+
+static float        osd_alt = 0;                    // altitude
+static float        osd_airspeed = 0;              // airspeed
+static float        osd_windspeed = 0;
+static float        osd_windspeedz = 0;
+static float        osd_winddirection = 0;
+static int8_t       osd_wind_arrow_rotate_int;
+static int8_t       osd_COG_arrow_rotate_int;
+
+static uint8_t      osd_alt_cnt = 0;              // counter for stable osd_alt
+static float        osd_alt_prev = 0;             // previous altitude
+
+static float        osd_groundspeed = 0;            // ground speed
+static uint8_t     osd_throttle = 0;               // throtle
+static uint16_t     temperature = 0;
+static uint8_t      tempconv = 1;
+static uint16_t     tempconvAdd = 0;
+static uint16_t     distchar = 0;
+static uint16_t     climbchar = 0;
+
+
+static float     convertt = 0;
+//Call sign variables
+static char         char_call[OSD_CALL_SIGN_TOTAL+1] = {0};
+
+//MAVLink session control
+static boolean      mavbeat = 0;
+static boolean      landing = 0;
+static float        lastMAVBeat = 0;
+static boolean      waitingMAVBeats = 1;
+//static uint8_t      apm_mav_type;
+static uint8_t      apm_mav_system; 
+static uint8_t      apm_mav_component;
+static boolean      enable_mav_request = 0;
+
+static const uint8_t npanels = 2;
+static uint8_t panel = 0; 
+// Panel BIT registers
+byte panA_REG[npanels] = {0b00000000};
+byte panB_REG[npanels] = {0b00000000};
+byte panC_REG[npanels] = {0b00000000};
+byte panD_REG[npanels] = {0b00000000};
+byte panE_REG[npanels] = {0b00000000};
+
+byte modeScreen = 0; //NTSC:0, PAL:1
+
+//byte SerCMD1 = 0;
+//byte SerCMD2 = 0;
+
+// First 8 panels and their X,Y coordinate holders
+//byte panCenter_XY[2][npanels]; // = { 13,7,0 };
+byte panPitch_XY[2][npanels]; // = { 11,1 };
+byte panRoll_XY[2][npanels]; // = { 23,7 };
+byte panBatt_A_XY[2][npanels]; // = { 23,1 };
+//byte panBatt_B_XY[2]; // = { 23,3 };
+byte panGPSats_XY[2][npanels]; // = { 2,12 };
+byte panCOG_XY[2][npanels]; // = { 2,11 };
+byte panGPS_XY[2][npanels]; // = { 2,13 };
+byte panBatteryPercent_XY[2][npanels];
+
+
+//Second 8 set of panels and their X,Y coordinate holders
+byte panRose_XY[2][npanels]; // = { 16,13 };
+byte panHeading_XY[2][npanels]; // = { 16,12 };
+//byte panMavBeat_XY[2][npanels]; // = { 2,10 };
+byte panHomeDir_XY[2][npanels]; // = { 0,0 };
+byte panHomeDis_XY[2][npanels]; // = { 0,0 };
+//byte panWPDir_XY[2][npanels]; // = { 27,12 };
+byte panWPDis_XY[2][npanels]; // = { 23,11 };
+byte panTime_XY[2][npanels];
+
+
+// Third set of panels and their X,Y coordinate holders
+byte panCur_A_XY[2][npanels]; // = { 23,1 };
+//byte panCur_B_XY[2]; // = { 23,3 };
+byte panAlt_XY[2][npanels]; // = { 0,0 };
+byte panHomeAlt_XY[2][npanels]; // = { 0,0 };
+byte panVel_XY[2][npanels]; // = { 0,0 };
+byte panAirSpeed_XY[2][npanels]; // = { 0,0 };
+byte panThr_XY[2][npanels]; // = { 0,0 };
+byte panFMod_XY[2][npanels]; // = { 0,0 };
+byte panHorizon_XY[2][npanels]; // = {8,centercalc}
+
+// Third set of panels and their X,Y coordinate holders
+byte panWarn_XY[2][npanels];
+byte panWindSpeed_XY[2][npanels];
+byte panClimb_XY[2][npanels];
+//byte panTune_XY[2][npanels];
+byte panRSSI_XY[2][npanels];
+byte panEff_XY[2][npanels];
+byte panCALLSIGN_XY[2][npanels];
+// byte panCh_XY[2][npanels];
+byte panTemp_XY[2][npanels];
+byte panDistance_XY[2][npanels];
+
+//*************************************************************************************************************
+//rssi varables
+static uint8_t      rssipersent = 0;
+static uint8_t      rssical = 0;
+static uint8_t      osd_rssi = 0; //raw value from mavlink
+static int16_t      rssi = -99; // scaled value 0-100%
+static bool         rssiraw_on = false; // 0- display scale value | 1- display raw value
+static uint8_t      rssi_warn_level = 0;
+

+

file:b/Spi.cpp (new)
--- /dev/null
+++ b/Spi.cpp
@@ -1,1 +1,57 @@
+// Get the common arduino functions
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "wiring.h"
+#endif
+#include "Spi.h"
 
+//---------- constructor ----------------------------------------------------
+
+SPI::SPI()
+{
+  // initialize the SPI pins
+  pinMode(SCK_PIN, OUTPUT);
+  pinMode(MOSI_PIN, OUTPUT);
+  pinMode(MISO_PIN, INPUT);
+  pinMode(SS_PIN, OUTPUT); // <------- !!! (Remember! This pin will select USB host chip Max3421)
+
+  // enable SPI Master, MSB, SPI mode 0, FOSC/4
+  mode(0);
+}
+
+//------------------ mode ---------------------------------------------------
+
+void SPI::mode(byte config)
+{
+  byte tmp;
+
+  // enable SPI master with configuration byte specified
+  SPCR = 0;
+  SPCR = (config & 0x7F) | (1<<SPE) | (1<<MSTR);
+  tmp = SPSR;
+  tmp = SPDR;
+}
+
+//------------------ transfer -----------------------------------------------
+
+byte SPI::transfer(byte value)
+{
+  SPDR = value;
+  while (!(SPSR & (1<<SPIF))) ;
+  return SPDR;
+}
+
+byte SPI::transfer(byte value, byte period)
+{
+  SPDR = value;
+  if (period > 0) delayMicroseconds(period);
+  while (!(SPSR & (1<<SPIF))) ;
+  return SPDR;
+}
+
+
+//---------- preinstantiate SPI object --------------------------------------
+
+SPI Spi = SPI();

+

file:b/Spi.h (new)
--- /dev/null
+++ b/Spi.h
@@ -1,1 +1,28 @@
+#ifndef Spi_h
+#define Spi_h
 
+// Get the common arduino functions
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "wiring.h"
+#endif
+
+#define SCK_PIN   13
+#define MISO_PIN  12
+#define MOSI_PIN  11
+#define SS_PIN    10  // <------- !!! (Remember! This pin will select USB host chip Max3421)
+
+class SPI
+{
+  public:
+    SPI(void);
+    void mode(byte);
+    byte transfer(byte);
+    byte transfer(byte, byte);
+};
+
+extern SPI Spi;
+
+#endif

+

--- /dev/null
+++ b/libraries/AP_Common/AP_Common.cpp
@@ -1,1 +1,18 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+/// @file		AP_Common.cpp

+/// @brief		Common utility routines for the ArduPilot libraries.

+///

+/// @note		Exercise restraint adding code here; everything in this

+///				library will be linked with any sketch using it.

+///

+

+#include "AP_Common.h"

+

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Common.h
@@ -1,1 +1,256 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+///

+/// @file 		AP_Common.h

+/// @brief		Common definitions and utility routines for the ArduPilot

+///				libraries.

+///

+

+#ifndef _AP_COMMON_H

+#define _AP_COMMON_H

+

+// Get the common arduino functions

+#if defined(ARDUINO) && ARDUINO >= 100

+	#include "Arduino.h"

+#else

+	#include "wiring.h"

+#endif

+// ... and remove some of their stupid macros

+#undef round

+#undef abs

+

+// prog_char_t is used as a wrapper type for prog_char, which is

+// a character stored in flash. By using this wrapper type we can

+// auto-detect at compile time if a call to a string function is using

+// a flash-stored string or not

+typedef struct {

+    char c;

+} prog_char_t;

+

+#include <stdint.h>

+#include "include/menu.h"		/// simple menu subsystem

+#include "c++.h" // c++ additions

+//#include "AP_Vector.h"

+//#include "AP_Loop.h"

+

+// default to AP_Param system, unless USE_AP_VAR is defined

+#ifdef USE_AP_VAR

+#include "AP_Var.h"

+#else

+#include "AP_Param.h"

+#endif

+

+////////////////////////////////////////////////////////////////////////////////

+/// @name	Warning control

+//@{

+//

+// Turn on/off warnings of interest.

+//

+// These warnings are normally suppressed by the Arduino IDE,

+// but with some minor hacks it's possible to have warnings

+// emitted.  This helps greatly when diagnosing subtle issues.

+//

+#pragma GCC diagnostic warning "-Wall"

+#pragma GCC diagnostic warning "-Wextra"

+#pragma GCC diagnostic warning "-Wlogical-op"

+#pragma GCC diagnostic ignored "-Wredundant-decls"

+

+// Make some dire warnings into errors

+//

+// Some warnings indicate questionable code; rather than let

+// these slide, we force them to become errors so that the

+// developer has to find a safer alternative.

+//

+//#pragma GCC diagnostic error "-Wfloat-equal"

+

+// The following is strictly for type-checking arguments to printf_P calls

+// in conjunction with a suitably modified Arduino IDE; never define for

+// production as it generates bad code.

+//

+#if PRINTF_FORMAT_WARNING_DEBUG

+# undef PSTR

+# define PSTR(_x)		_x		// help the compiler with printf_P

+# define float double			// silence spurious format warnings for %f

+#else

+// This is a workaround for GCC bug c++/34734.

+//

+// The C++ compiler normally emits many spurious warnings for the use

+// of PSTR (even though it generates correct code).  This workaround

+// has an equivalent effect but avoids the warnings, which otherwise

+// make finding real issues difficult.

+//

+#ifdef DESKTOP_BUILD

+# undef PROGMEM

+# define PROGMEM __attribute__(())

+#else

+# undef PROGMEM

+# define PROGMEM __attribute__(( section(".progmem.data") ))

+#endif

+

+# undef PSTR

+# define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \

+                (prog_char_t *)&__c[0];}))

+#endif

+

+// a varient of PSTR() for progmem strings passed to %S in printf()

+// this gets the gcc __format__ checking right

+#define FPSTR(s) (wchar_t *)(s)

+

+

+static inline int strcasecmp_P(const char *str1, const prog_char_t *pstr)

+{

+    return strcasecmp_P(str1, (const prog_char *)pstr);

+}

+

+static inline int strcmp_P(const char *str1, const prog_char_t *pstr)

+{

+    return strcmp_P(str1, (const prog_char *)pstr);

+}

+

+static inline size_t strlen_P(const prog_char_t *pstr)

+{

+    return strlen_P((const prog_char *)pstr);

+}

+

+static inline void *memcpy_P(void *dest, const prog_char_t *src, size_t n)

+{

+    return memcpy_P(dest, (const prog_char *)src, n);

+}

+

+// strlcat_P() in AVR libc seems to be broken 

+static inline size_t strlcat_P(char *d, const prog_char_t *s, size_t bufsize)

+{

+	size_t len1 = strlen(d);

+	size_t len2 = strlen_P(s);

+	size_t ret = len1 + len2;

+    

+	if (len1+len2 >= bufsize) {

+		if (bufsize < (len1+1)) {

+			return ret;

+		}

+		len2 = bufsize - (len1+1);

+	}

+	if (len2 > 0) {

+		memcpy_P(d+len1, s, len2);

+		d[len1+len2] = 0;

+	}

+	return ret;

+}

+

+static inline char *strncpy_P(char *buffer, const prog_char_t *pstr, size_t buffer_size)

+{

+    return strncpy_P(buffer, (const prog_char *)pstr, buffer_size);

+}

+

+

+// read something the size of a pointer. This makes the menu code more

+// portable

+static inline uintptr_t pgm_read_pointer(const void *s)

+{

+    if (sizeof(uintptr_t) == sizeof(uint16_t)) {

+        return (uintptr_t)pgm_read_word(s);

+    } else {

+        union {

+            uintptr_t p;

+            uint8_t a[sizeof(uintptr_t)];

+        } u;

+        uint8_t i;

+        for (i=0; i< sizeof(uintptr_t); i++) {

+            u.a[i] = pgm_read_byte(i + (const prog_char *)s);

+        }

+        return u.p;

+    }

+}

+

+//@}

+

+

+///

+/// @name Macros

+/// @{

+

+/// Define a constant string in program memory.  This is a little more obvious

+/// and less error-prone than typing the declaration out by hand.  It's required

+/// when passing PROGMEM strings to static object constructors because the PSTR

+/// hack can't be used at global scope.

+///

+#define PROGMEM_STRING(_v, _s)	static const char _v[] PROGMEM = _s

+

+#define ToRad(x) (x*0.01745329252)	// *pi/180

+#define ToDeg(x) (x*57.2957795131)	// *180/pi

+// @}

+

+

+////////////////////////////////////////////////////////////////////////////////

+/// @name	Types

+///

+/// Data structures and types used throughout the libraries and applications. 0 = default

+/// bit 0: Altitude is stored 			    0: Absolute,	1: Relative

+/// bit 1: Chnage Alt between WP 		    0: Gradually,	1: ASAP

+/// bit 2:

+/// bit 3: Req.to hit WP.alt to continue    0: No,          1: Yes

+/// bit 4: Relative to Home					0: No, 			1: Yes

+/// bit 5:

+/// bit 6:

+/// bit 7: Move to next Command 		    0: YES, 		1: Loiter until commanded

+

+//@{

+

+struct Location {

+    uint8_t		id;					///< command id

+    uint8_t		options;			///< options bitmask (1<<0 = relative altitude)

+    uint8_t		p1;					///< param 1

+    int32_t		alt;				///< param 2 - Altitude in centimeters (meters * 100)

+    int32_t		lat;				///< param 3 - Lattitude * 10**7

+    int32_t		lng;				///< param 4 - Longitude * 10**7

+};

+

+//@}

+

+////////////////////////////////////////////////////////////////////////////////

+/// @name	Conversions

+///

+/// Conversion macros and factors.

+///

+//@{

+

+/// XXX this should probably be replaced with radians()/degrees(), but their

+/// inclusion in wiring.h makes doing that here difficult.

+#define ToDeg(x) (x*57.2957795131)	// *180/pi

+#define ToRad(x) (x*0.01745329252)	// *pi/180

+

+//@}

+

+#ifdef DESKTOP_BUILD

+// used to report serious errors in autotest

+# define SITL_debug(fmt, args...)  fprintf(stdout, "%s:%u " fmt, __FUNCTION__, __LINE__, ##args)

+#else

+# define SITL_debug(fmt, args...)

+#endif

+

+/*  Product IDs for all supported products follow */

+

+#define AP_PRODUCT_ID_NONE 			0x00 	// Hardware in the loop

+#define AP_PRODUCT_ID_APM1_1280 	0x01 	// APM1 with 1280 CPUs

+#define AP_PRODUCT_ID_APM1_2560 	0x02 	// APM1 with 2560 CPUs

+#define AP_PRODUCT_ID_SITL		 	0x03 	// Software in the loop

+#define AP_PRODUCT_ID_APM2ES_REV_C4 0x14 	// APM2 with MPU6000ES_REV_C4

+#define AP_PRODUCT_ID_APM2ES_REV_C5	0x15 	// APM2 with MPU6000ES_REV_C5

+#define AP_PRODUCT_ID_APM2ES_REV_D6	0x16	// APM2 with MPU6000ES_REV_D6

+#define AP_PRODUCT_ID_APM2ES_REV_D7	0x17	// APM2 with MPU6000ES_REV_D7

+#define AP_PRODUCT_ID_APM2ES_REV_D8	0x18	// APM2 with MPU6000ES_REV_D8	

+#define AP_PRODUCT_ID_APM2_REV_C4	0x54	// APM2 with MPU6000_REV_C4 	

+#define AP_PRODUCT_ID_APM2_REV_C5	0x55	// APM2 with MPU6000_REV_C5 	

+#define AP_PRODUCT_ID_APM2_REV_D6	0x56	// APM2 with MPU6000_REV_D6 		

+#define AP_PRODUCT_ID_APM2_REV_D7	0x57	// APM2 with MPU6000_REV_D7 	

+#define AP_PRODUCT_ID_APM2_REV_D8	0x58	// APM2 with MPU6000_REV_D8 	

+#define AP_PRODUCT_ID_APM2_REV_D9	0x59	// APM2 with MPU6000_REV_D9 	

+

+#endif // _AP_COMMON_H

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Loop.cpp
@@ -1,1 +1,53 @@
+/*

+ * AP_Loop.pde

+ * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#include "AP_Loop.h"

+

+Loop::Loop(float _frequency, void (*fptr)(void *), void * data) :

+    _fptr(fptr),

+    _data(data),

+    _period(1.0e6/_frequency),

+    _subLoops(),

+    _timeStamp(micros()),

+    _load(0),

+    _dt(0)

+{

+}

+

+void Loop::update()

+{

+    // quick exit if not ready

+    if (micros() - _timeStamp < _period) return;

+

+    // update time stamp

+    uint32_t timeStamp0 = _timeStamp;

+    _timeStamp = micros();

+    _dt = (_timeStamp - timeStamp0)/1.0e6;

+

+    // update sub loops

+    for (uint8_t i=0; i<_subLoops.getSize(); i++) _subLoops[i]->update();

+

+    // callback function

+    if (_fptr) _fptr(_data);

+

+    // calculated load with a low pass filter

+    _load = 0.9*_load + 10*(float(micros()-_timeStamp)/(_timeStamp-timeStamp0));

+}

+

+// vim:ts=4:sw=4:expandtab

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Loop.h
@@ -1,1 +1,61 @@
+/*

+ * AP_Loop.h

+ * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#ifndef AP_Loop_H

+#define AP_Loop_H

+

+#include "AP_Vector.h"

+

+class Loop

+{

+public:

+    Loop() : _fptr(), _data(), _period(), _subLoops(), _timeStamp(), _load(), _dt() {};

+    Loop(float frequency, void (*fptr)(void *) = NULL, void * data = NULL);

+    void update();

+    Vector<Loop *> & subLoops() {

+        return _subLoops;

+    }

+    float frequency() {

+        return 1.0e6/_period;

+    }

+    void frequency(float _frequency) {

+        _period = 1e6/_frequency;

+    }

+    uint32_t timeStamp() {

+        return _timeStamp;

+    }

+    float dt() {

+        return _dt;

+    }

+    uint8_t load() {

+        return _load;

+    }

+protected:

+    void (*_fptr)(void *);

+    void * _data;

+    uint32_t _period;

+    Vector<Loop *> _subLoops;

+    uint32_t _timeStamp;

+    uint8_t _load;

+    float _dt;

+};

+

+#endif

+

+// vim:ts=4:sw=4:expandtab

 

--- /dev/null
+++ b/libraries/AP_Common/AP_MetaClass.cpp
@@ -1,1 +1,37 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+/// @file	AP_MetaClass.cpp

+///			Abstract meta-class from which other AP classes may inherit.

+///			Provides type introspection and some basic protocols that can

+///			be implemented by subclasses.

+

+#include "AP_MetaClass.h"

+

+// Default ctor, currently does nothing

+AP_Meta_class::AP_Meta_class(void)

+{

+}

+

+// Default dtor, currently does nothing but must be defined in order to ensure that

+// subclasses not overloading the default virtual dtor still have something in their

+// vtable.

+AP_Meta_class::~AP_Meta_class()

+{

+}

+

+size_t AP_Meta_class::serialize(void *buf, size_t bufSize) const

+{

+    return 0;

+}

+

+size_t AP_Meta_class::unserialize(void *buf, size_t bufSize)

+{

+    return 0;

+}

 

--- /dev/null
+++ b/libraries/AP_Common/AP_MetaClass.h
@@ -1,1 +1,260 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+/// @file	AP_Meta_class.h

+///	@brief	An abstract base class from which other classes can inherit.

+///

+/// This abstract base class declares and implements functions that are

+/// useful to code that wants to know things about a class, or to operate

+/// on the class without knowing precisely what it is.

+///

+/// All classes that inherit from this class can be assumed to have these

+/// basic functions.

+///

+

+#ifndef AP_META_CLASS_H

+#define AP_META_CLASS_H

+

+#include <stddef.h>			// for size_t

+#include <inttypes.h>

+

+#include <avr/io.h>			// for RAMEND

+///

+/// Basic meta-class from which other AP_* classes can derive.

+///

+/// Functions that form the public API to the metaclass are prefixed meta_.

+///

+/// Note that classes inheriting from AP_Meta_class *must* have a default

+/// constructor and destructor in order for meta_cast to work.

+///

+class AP_Meta_class

+{

+public:

+    /// Default constructor does nothing.

+    AP_Meta_class(void);

+

+    /// Default destructor is virtual, to ensure that all subclasses'

+    /// destructors are virtual.  This guarantees that all destructors

+    /// in the inheritance chain are called at destruction time.

+    ///

+    virtual ~AP_Meta_class();

+

+    /// Typedef for the ID unique to all instances of a class.

+    ///

+    /// See ::meta_type_id for a discussion of class type IDs.

+    ///

+    typedef uint16_t Type_id;

+

+    /// Obtain a value unique to all instances of a specific subclass.

+    ///

+    /// The value can be used to determine whether two class pointers

+    /// refer to the same exact class type.  The value can also be cached

+    /// and then used to detect objects of a given type at a later point.

+    ///

+    /// This is similar to the basic functionality of the C++ typeid

+    /// keyword, but does not depend on std::type_info or any compiler-

+    /// generated RTTI.

+    ///

+    /// The value is derived from the vtable address, so it is guaranteed

+    /// to be unique but cannot be known until the program has been compiled

+    /// and linked.  Thus, the only way to know the type ID of a given

+    /// type is to construct an object at runtime.  To cache the type ID

+    /// of a class Foo, see the templated version below:

+    ///

+    /// @return					A type-unique value for this.

+    ///

+    Type_id meta_type_id(void) const {

+        return *(Type_id *)this;

+    }

+

+    /// Obtain a value unique to all instances of a named subclass.

+    ///

+    /// This is similar to ::meta_type_id, but is a template taking a class name.

+    /// Use this function to cache the Type_id for a class when you don't need

+    /// or cannot afford the constructor cost associated with meta_cast.

+    ///

+    /// @tparam T               A subclass of AP_Meta_class.

+    /// @return                 The Type_id value for T.

+    ///

+    template<typename T>

+    static Type_id meta_type_id(void) {

+        T tmp;

+        return tmp.meta_type_id();

+    }

+

+    /// External handle for an instance of an AP_Meta_class subclass, contains

+    /// enough information to construct and validate a pointer to the instance

+    /// when passed back from an untrusted source.

+    ///

+    /// Handles are useful when passing a reference to an object to a client outside

+    /// the system, as they can be validated by the system when the client hands

+    /// them back.

+    ///

+    typedef uint32_t Meta_handle;

+

+    /// Return a value that can be used as an external pointer to an instance

+    /// of a subclass.

+    ///

+    /// The value can be passed to an untrusted agent, and validated on its return.

+    ///

+    /// The value contains the 16-bit type ID of the actual class and

+    /// a pointer to the class instance.

+    ///

+    /// @return					An opaque handle

+    ///

+    Meta_handle meta_get_handle(void) const {

+        return ((Meta_handle)meta_type_id() << 16) | (uintptr_t)this;

+    }

+

+    /// Validates an AP_Meta_class handle.

+    ///

+    /// The value of the handle is not required to be valid; in particular the

+    /// pointer encoded in the handle is validated before being dereferenced.

+    ///

+    /// The handle is considered good if the pointer is valid and the object

+    /// it points to has a type ID that matches the ID in the handle.

+    ///

+    /// @param	handle			A possible AP_Meta_class handle

+    /// @return					The instance pointer if the handle is good,

+    ///							or NULL if it is bad.

+    ///

+    static AP_Meta_class *meta_validate_handle(Meta_handle handle) {

+        AP_Meta_class *candidate = (AP_Meta_class *)(handle & 0xffff); // extract object pointer

+        uint16_t id = handle >> 16; // and claimed type

+

+        // Sanity-check the pointer to ensure it lies within the device RAM, so that

+        // a bad handle won't cause ::meta_type_id to read outside of SRAM.

+        // Assume that RAM (or addressable storage of some sort, at least) starts at zero.

+        //

+        // Note that this implies that we cannot deal with objects in ROM or EEPROM,

+        // but the constructor wouldn't be able to populate a vtable pointer there anyway...

+        //

+        if ((uintptr_t)candidate >= (RAMEND - 2)) { // -2 to account for the type_id

+            return NULL;

+        }

+

+        // Compare the typeid of the object that candidate points to with the typeid

+        // from the handle.  Note that it's safe to call meta_type_id() off the untrusted

+        // candidate pointer because meta_type_id is non-virtual (and will in fact be

+        // inlined here).

+        //

+        if (candidate->meta_type_id() == id) {

+            return candidate;

+        }

+

+        return NULL;

+    }

+

+    /// Tests whether two objects are of precisely the same class.

+    ///

+    /// Note that in the case where p2 inherits from p1, or vice-versa, this will return

+    /// false as we cannot detect these inheritance relationships at runtime.

+    ///

+    /// In the caller's context, p1 and p2 may be pointers to any type, but we require

+    /// that they be passed as pointers to AP_Meta_class in order to make it clear that

+    /// they should be pointers to classes derived from AP_Meta_class.

+    ///

+    /// No attempt is made to validate whether p1 and p2 are actually derived from

+    /// AP_Meta_class.  If p1 and p2 are equal, or if they point to non-class objects with

+    /// similar contents, or to non-AP_Meta_class derived classes with no virtual functions

+    /// this function may return true.

+    ///

+    /// @param	p1				The first object to be compared.

+    /// @param	p2				The second object to be compared.

+    /// @return					True if the two objects are of the same class, false

+    ///							if they are not.

+    ///

+    static bool meta_type_equivalent(AP_Meta_class *p1, AP_Meta_class *p2) {

+        return p1->meta_type_id() == p2->meta_type_id();

+    }

+

+    /// Cast a pointer to an expected class type.

+    ///

+    /// This function is used when a pointer is expected to be a pointer to a

+    /// subclass of AP_Meta_class, but the caller is not certain.  It will return the pointer

+    /// if it is, or NULL if it is not a pointer to the expected class.

+    ///

+    /// This should be used with caution, as T's default constructor and

+    /// destructor will be run, possibly introducing undesired side-effects.

+    ///

+    /// @todo	Consider whether we should make it difficult to have a default constructor

+    ///			with appreciable side-effects.

+    ///

+    /// @param	p				An AP_Meta_class subclass that may be of type T.

+    /// @tparam	T		        The name of a type to which p is to be cast.

+    /// @return					NULL if p is not of precisely type T, otherwise p cast to T.

+    ///

+    template<typename T>

+    static T *meta_cast(AP_Meta_class *p) {

+        T tmp;

+        if (meta_type_equivalent(p, &tmp)) {

+            return (T *)p;

+        }

+        return NULL;

+    }

+

+    /// Cast this to an expected class type.

+    ///

+    /// This is equivalent to meta_cast<T>(this)

+    ///

+    /// @tparam T               The name of a type to which this is to be cast.

+    /// @return                 NULL if this is not of precisely type T, otherwise this cast to T.

+    ///

+    template<typename T>

+    T *meta_cast(void) {

+        T tmp;

+        if (meta_type_equivalent(this, &tmp)) {

+            return (T*)this;

+        }

+        return NULL;

+    }

+

+    /// Serialize the class.

+    ///

+    /// Serialization stores the state of the class in an external buffer in such a

+    /// fashion that it can later be restored by unserialization.

+    ///

+    /// AP_Meta_class subclasses should only implement these functions if saving and

+    /// restoring their state makes sense.

+    ///

+    /// Serialization provides a mechanism for exporting the state of the class to an

+    /// external consumer, either for external introspection or for subsequent restoration.

+    ///

+    /// Classes that wrap variables should define the format of their serialiaed data

+    /// so that external consumers can reliably interpret it.

+    ///

+    /// @param	buf				Buffer into which serialised data should be placed.

+    /// @param	bufSize			The size of the buffer provided.

+    /// @return					The size of the serialised data, even if that data would

+    ///							have overflowed the buffer.  If the return value is zero,

+    ///							the class does not support serialization.

+    ///

+    virtual size_t serialize(void *buf, size_t bufSize) const;

+

+    /// Unserialize the class.

+    ///

+    /// Unserializing a class from a buffer into which the class previously serialized

+    /// itself restores the instance to an identical state, where "identical" is left

+    /// up to the class itself to define.

+    ///

+    /// Classes that wrap variables should define the format of their serialized data so

+    /// that external providers can reliably encode it.

+    ///

+    /// @param	buf				Buffer containing serialized data.

+    /// @param	bufSize			The size of the buffer.

+    /// @return					The number of bytes from the buffer that would be consumed

+    ///							unserializing the data.  If the value is less than or equal

+    ///							to bufSize, unserialization was successful.  If the return

+    ///							value is zero the class does not support unserialisation or

+    ///							the data in the buffer is invalid.

+    ///

+    virtual size_t unserialize(void *buf, size_t bufSize);

+};

+

+#endif // AP_Meta_class_H

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Param.cpp
@@ -1,1 +1,893 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+// total up and check overflow

+// check size of group var_info

+

+/// @file   AP_Param.cpp

+/// @brief  The AP variable store.

+

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+#include <AP_Math.h>

+

+#include <math.h>

+#include <string.h>

+

+// #define ENABLE_FASTSERIAL_DEBUG

+

+#ifdef ENABLE_FASTSERIAL_DEBUG

+# define serialDebug(fmt, args...)  if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0)

+#else

+# define serialDebug(fmt, args...)

+#endif

+

+// some useful progmem macros

+#define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr)

+#define PGM_UINT16(addr) pgm_read_word((const uint16_t *)addr)

+#define PGM_POINTER(addr) pgm_read_pointer((const void *)addr)

+

+// the 'GROUP_ID' of a element of a group is the 8 bit identifier used

+// to distinguish between this element of the group and other elements

+// of the same group. It is calculated using a bit shift per level of

+// nesting, so the first level of nesting gets 4 bits, and the next

+// level gets the next 4 bits. This limits groups to having at most 16

+// elements.

+#define GROUP_ID(grpinfo, base, i, shift) ((base)+(((uint16_t)PGM_UINT8(&grpinfo[i].idx))<<(shift)))

+

+// Note about AP_Vector3f handling.

+// The code has special cases for AP_Vector3f to allow it to be viewed

+// as both a single 3 element vector and as a set of 3 AP_Float

+// variables. This is done to make it possible for MAVLink to see

+// vectors as parameters, which allows users to save their compass

+// offsets in MAVLink parameter files. The code involves quite a few

+// special cases which could be generalised to any vector/matrix type

+// if we end up needing this behaviour for other than AP_Vector3f

+

+

+// static member variables for AP_Param.

+//

+

+// max EEPROM write size. This is usually less than the physical

+// size as only part of the EEPROM is reserved for parameters

+uint16_t AP_Param::_eeprom_size;

+

+// number of rows in the _var_info[] table

+uint8_t AP_Param::_num_vars;

+

+// storage and naming information about all types that can be saved

+const AP_Param::Info *AP_Param::_var_info;

+

+// write to EEPROM, checking each byte to avoid writing

+// bytes that are already correct

+void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)

+{

+    const uint8_t *b = (const uint8_t *)ptr;

+    while (size--) {

+        uint8_t v = eeprom_read_byte((const uint8_t *)ofs);

+        if (v != *b) {

+            eeprom_write_byte((uint8_t *)ofs, *b);

+        }

+        b++;

+        ofs++;

+    }

+}

+

+// write a sentinal value at the given offset

+void AP_Param::write_sentinal(uint16_t ofs)

+{

+    struct Param_header phdr;

+    phdr.type = _sentinal_type;

+    phdr.key  = _sentinal_key;

+    phdr.group_element = _sentinal_group;

+    eeprom_write_check(&phdr, ofs, sizeof(phdr));

+}

+

+// erase all EEPROM variables by re-writing the header and adding

+// a sentinal

+void AP_Param::erase_all(void)

+{

+    struct EEPROM_header hdr;

+

+    serialDebug("erase_all");

+

+    // write the header

+    hdr.magic[0] = k_EEPROM_magic0;

+    hdr.magic[1] = k_EEPROM_magic1;

+    hdr.revision = k_EEPROM_revision;

+    hdr.spare    = 0;

+    eeprom_write_check(&hdr, 0, sizeof(hdr));

+

+    // add a sentinal directly after the header

+    write_sentinal(sizeof(struct EEPROM_header));

+}

+

+// validate a group info table

+bool AP_Param::check_group_info(const struct AP_Param::GroupInfo *group_info,

+                                uint16_t *total_size,

+                                uint8_t group_shift)

+{

+    uint8_t type;

+    int8_t max_idx = -1;

+    for (uint8_t i=0;

+         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;

+         i++) {

+#ifdef AP_NESTED_GROUPS_ENABLED

+        if (type == AP_PARAM_GROUP) {

+            // a nested group

+            const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);

+            if (group_shift + _group_level_shift >= _group_bits) {

+                // double nesting of groups is not allowed

+                return false;

+            }

+            if (ginfo == NULL ||

+                !check_group_info(ginfo, total_size, group_shift + _group_level_shift)) {

+                return false;

+            }

+            continue;

+        }

+#endif // AP_NESTED_GROUPS_ENABLED

+        uint8_t idx = PGM_UINT8(&group_info[i].idx);

+        if (idx >= (1<<_group_level_shift)) {

+            // passed limit on table size

+            return false;

+        }

+        if ((int8_t)idx <= max_idx) {

+            // the indexes must be in increasing order

+            return false;

+        }

+        max_idx = (int8_t)idx;

+        uint8_t size = type_size((enum ap_var_type)type);

+        if (size == 0) {

+            // not a valid type

+            return false;

+        }

+        (*total_size) += size + sizeof(struct Param_header);

+    }

+    return true;

+}

+

+// check for duplicate key values

+bool AP_Param::duplicate_key(uint8_t vindex, uint8_t key)

+{

+    for (uint8_t i=vindex+1; i<_num_vars; i++) {

+        uint8_t key2 = PGM_UINT8(&_var_info[i].key);

+        if (key2 == key) {

+            // no duplicate keys allowed

+            return true;

+        }

+    }

+    return false;

+}

+

+// validate the _var_info[] table

+bool AP_Param::check_var_info(void)

+{

+    uint16_t total_size = sizeof(struct EEPROM_header);

+

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

+        uint8_t type = PGM_UINT8(&_var_info[i].type);

+        uint8_t key = PGM_UINT8(&_var_info[i].key);

+        if (type == AP_PARAM_GROUP) {

+            if (i == 0) {

+                // first element can't be a group, for first() call

+                return false;

+            }

+            const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);

+            if (group_info == NULL ||

+                !check_group_info(group_info, &total_size, 0)) {

+                return false;

+            }

+        } else {

+            uint8_t size = type_size((enum ap_var_type)type);

+            if (size == 0) {

+                // not a valid type - the top level list can't contain

+                // AP_PARAM_NONE

+                return false;

+            }

+            total_size += size + sizeof(struct Param_header);

+        }

+        if (duplicate_key(i, key)) {

+            return false;

+        }

+    }

+    if (total_size > _eeprom_size) {

+        serialDebug("total_size %u exceeds _eeprom_size %u",

+                    total_size, _eeprom_size);

+        return false;

+    }

+    return true;

+}

+

+

+// setup the _var_info[] table

+bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eeprom_size)

+{

+    struct EEPROM_header hdr;

+

+    _eeprom_size = eeprom_size;

+    _var_info = info;

+    _num_vars = num_vars;

+

+    if (!check_var_info()) {

+        return false;

+    }

+

+    serialDebug("setup %u vars", (unsigned)num_vars);

+

+    // check the header

+    eeprom_read_block(&hdr, 0, sizeof(hdr));

+    if (hdr.magic[0] != k_EEPROM_magic0 ||

+        hdr.magic[1] != k_EEPROM_magic1 ||

+        hdr.revision != k_EEPROM_revision) {

+        // header doesn't match. We can't recover any variables. Wipe

+        // the header and setup the sentinal directly after the header

+        serialDebug("bad header in setup - erasing");

+        erase_all();

+    }

+

+    return true;

+}

+

+// check if AP_Param has been initialised

+bool AP_Param::initialised(void)

+{

+    return _var_info != NULL;

+}

+

+// find the info structure given a header and a group_info table

+// return the Info structure and a pointer to the variables storage

+const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,

+                                                            uint8_t vindex,

+                                                            const struct GroupInfo *group_info,

+                                                            uint8_t group_base,

+                                                            uint8_t group_shift)

+{

+    uint8_t type;

+    for (uint8_t i=0;

+         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;

+         i++) {

+#ifdef AP_NESTED_GROUPS_ENABLED

+        if (type == AP_PARAM_GROUP) {

+            // a nested group

+            if (group_shift + _group_level_shift >= _group_bits) {

+                // too deeply nested - this should have been caught by

+                // setup() !

+                return NULL;

+            }

+            const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);

+            const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,

+                                                                    GROUP_ID(group_info, group_base, i, group_shift),

+                                                                    group_shift + _group_level_shift);

+            if (ret != NULL) {

+                return ret;

+            }

+            continue;

+        }

+#endif // AP_NESTED_GROUPS_ENABLED

+        if (GROUP_ID(group_info, group_base, i, group_shift) == phdr.group_element) {

+            // found a group element

+            *ptr = (void*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));

+            return &_var_info[vindex];

+        }

+    }

+    return NULL;

+}

+

+// find the info structure given a header

+// return the Info structure and a pointer to the variables storage

+const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr)

+{

+    // loop over all named variables

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

+        uint8_t type = PGM_UINT8(&_var_info[i].type);

+        uint8_t key = PGM_UINT8(&_var_info[i].key);

+        if (key != phdr.key) {

+            // not the right key

+            continue;

+        }

+        if (type != AP_PARAM_GROUP) {

+            // if its not a group then we are done

+            *ptr = (void*)PGM_POINTER(&_var_info[i].ptr);

+            return &_var_info[i];

+        }

+

+        const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);

+        return find_by_header_group(phdr, ptr, i, group_info, 0, 0);

+    }

+    return NULL;

+}

+

+// find the info structure for a variable in a group

+const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo *group_info,

+                                                           uint8_t vindex,

+                                                           uint8_t group_base,

+                                                           uint8_t group_shift,

+                                                           uint8_t *group_element,

+                                                           const struct GroupInfo **group_ret,

+                                                           uint8_t *idx)

+{

+    uintptr_t base = PGM_POINTER(&_var_info[vindex].ptr);

+    uint8_t type;

+    for (uint8_t i=0;

+         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;

+         i++) {

+        uintptr_t ofs = PGM_POINTER(&group_info[i].offset);

+#ifdef AP_NESTED_GROUPS_ENABLED

+        if (type == AP_PARAM_GROUP) {

+            const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);

+            // a nested group

+            if (group_shift + _group_level_shift >= _group_bits) {

+                // too deeply nested - this should have been caught by

+                // setup() !

+                return NULL;

+            }

+            const struct AP_Param::Info *info;

+            info = find_var_info_group(ginfo, vindex,

+                                       GROUP_ID(group_info, group_base, i, group_shift),

+                                       group_shift + _group_level_shift,

+                                       group_element,

+                                       group_ret,

+                                       idx);

+            if (info != NULL) {

+                return info;

+            }

+        } else // Forgive the poor formatting - if continues below.

+#endif // AP_NESTED_GROUPS_ENABLED

+        if ((uintptr_t)this == base + ofs) {

+            *group_element = GROUP_ID(group_info, group_base, i, group_shift);

+            *group_ret = &group_info[i];

+            *idx = 0;

+            return &_var_info[vindex];

+        } else if (type == AP_PARAM_VECTOR3F &&

+                   (base+ofs+sizeof(float) == (uintptr_t)this ||

+                    base+ofs+2*sizeof(float) == (uintptr_t)this)) {

+            // we are inside a Vector3f. We need to work out which

+            // element of the vector the current object refers to.

+            *idx = (((uintptr_t)this) - (base+ofs))/sizeof(float);

+            *group_element = GROUP_ID(group_info, group_base, i, group_shift);

+            *group_ret = &group_info[i];

+            return &_var_info[vindex];

+        }

+    }

+    return NULL;

+}

+

+// find the info structure for a variable

+const struct AP_Param::Info *AP_Param::find_var_info(uint8_t *group_element,

+                                                     const struct GroupInfo **group_ret,

+                                                     uint8_t *idx)

+{

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

+        uint8_t type = PGM_UINT8(&_var_info[i].type);

+        uintptr_t base = PGM_POINTER(&_var_info[i].ptr);

+        if (type == AP_PARAM_GROUP) {

+            const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);

+            const struct AP_Param::Info *info;

+            info = find_var_info_group(group_info, i, 0, 0, group_element, group_ret, idx);

+            if (info != NULL) {

+                return info;

+            }

+        } else if (base == (uintptr_t)this) {

+            *group_element = 0;

+            *group_ret = NULL;

+            *idx = 0;

+            return &_var_info[i];

+        } else if (type == AP_PARAM_VECTOR3F &&

+                   (base+sizeof(float) == (uintptr_t)this ||

+                    base+2*sizeof(float) == (uintptr_t)this)) {

+            // we are inside a Vector3f. Work out which element we are

+            // referring to.

+            *idx = (((uintptr_t)this) - base)/sizeof(float);

+            *group_element = 0;

+            *group_ret = NULL;

+            return &_var_info[i];

+        }

+    }

+    return NULL;

+}

+

+// return the storage size for a AP_PARAM_* type

+const uint8_t AP_Param::type_size(enum ap_var_type type)

+{

+    switch (type) {

+    case AP_PARAM_NONE:

+    case AP_PARAM_GROUP:

+        return 0;

+    case AP_PARAM_INT8:

+        return 1;

+    case AP_PARAM_INT16:

+        return 2;

+    case AP_PARAM_INT32:

+        return 4;

+    case AP_PARAM_FLOAT:

+        return 4;

+    case AP_PARAM_VECTOR3F:

+        return 3*4;

+    case AP_PARAM_VECTOR6F:

+        return 6*4;

+    case AP_PARAM_MATRIX3F:

+        return 3*3*4;

+    }

+    serialDebug("unknown type %u\n", type);

+    return 0;

+}

+

+// scan the EEPROM looking for a given variable by header content

+// return true if found, along with the offset in the EEPROM where

+// the variable is stored

+// if not found return the offset of the sentinal, or

+bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs)

+{

+    struct Param_header phdr;

+    uint16_t ofs = sizeof(AP_Param::EEPROM_header);

+    while (ofs < _eeprom_size) {

+        eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr));

+        if (phdr.type == target->type &&

+            phdr.key == target->key &&

+            phdr.group_element == target->group_element) {

+            // found it

+            *pofs = ofs;

+            return true;

+        }

+        // note that this is an ||, not an &&, as this makes us more

+        // robust to power off while adding a variable to EEPROM

+        if (phdr.type == _sentinal_type ||

+            phdr.key == _sentinal_key ||

+            phdr.group_element == _sentinal_group) {

+            // we've reached the sentinal

+            *pofs = ofs;

+            return false;

+        }

+        ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);

+    }

+    *pofs = ~0;

+    serialDebug("scan past end of eeprom");

+    return false;

+}

+

+// add a X,Y,Z suffix to the name of a Vector3f element

+void AP_Param::add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx)

+{

+    uint8_t len = strnlen(buffer, buffer_size);

+    if ((size_t)(len+3) >= buffer_size) {

+        // the suffix doesn't fit

+        return;

+    }

+    buffer[len] = '_';

+    if (idx == 0) {

+        buffer[len+1] = 'X';

+    } else if (idx == 1) {

+        buffer[len+1] = 'Y';

+    } else if (idx == 2) {

+        buffer[len+1] = 'Z';

+    }

+    buffer[len+2] = 0;

+}

+

+// Copy the variable's whole name to the supplied buffer.

+//

+// If the variable is a group member, prepend the group name.

+//

+void AP_Param::copy_name(char *buffer, size_t buffer_size, bool force_scalar)

+{

+    uint8_t group_element;

+    const struct GroupInfo *ginfo;

+    uint8_t idx;

+    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);

+    if (info == NULL) {

+        *buffer = 0;

+        serialDebug("no info found");

+        return;

+    }

+    strncpy_P(buffer, info->name, buffer_size);

+    if (ginfo != NULL) {

+        uint8_t len = strnlen(buffer, buffer_size);

+        if (len < buffer_size) {

+            strncpy_P(&buffer[len], ginfo->name, buffer_size-len);

+        }

+        if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&ginfo->type)) {

+            // the caller wants a specific element in a Vector3f

+            add_vector3f_suffix(buffer, buffer_size, idx);

+        }

+    } else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&info->type)) {

+            add_vector3f_suffix(buffer, buffer_size, idx);

+    }

+}

+

+// Find a variable by name in a group

+AP_Param *

+AP_Param::find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype)

+{

+    uint8_t type;

+    for (uint8_t i=0;

+         (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;

+         i++) {

+#ifdef AP_NESTED_GROUPS_ENABLED

+        if (type == AP_PARAM_GROUP) {

+            const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);

+            AP_Param *ap = find_group(name, vindex, ginfo, ptype);

+            if (ap != NULL) {

+                return ap;

+            }

+        } else

+#endif // AP_NESTED_GROUPS_ENABLED

+        if (strcasecmp_P(name, group_info[i].name) == 0) {

+            uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr);

+            *ptype = (enum ap_var_type)type;

+            return (AP_Param *)(p + PGM_POINTER(&group_info[i].offset));

+        } else if (type == AP_PARAM_VECTOR3F) {

+            // special case for finding Vector3f elements

+            uint8_t suffix_len = strlen_P(group_info[i].name);

+            if (strncmp_P(name, group_info[i].name, suffix_len) == 0 &&

+                name[suffix_len] == '_' &&

+                name[suffix_len+1] != 0 &&

+                name[suffix_len+2] == 0) {

+                uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr);

+                AP_Float *v = (AP_Float *)(p + PGM_POINTER(&group_info[i].offset));

+                *ptype = AP_PARAM_FLOAT;

+                switch (name[suffix_len+1]) {

+                case 'X':

+                    return (AP_Float *)&v[0];

+                case 'Y':

+                    return (AP_Float *)&v[1];

+                case 'Z':

+                    return (AP_Float *)&v[2];

+                }

+            }

+        }

+    }

+    return NULL;

+}

+

+

+// Find a variable by name.

+//

+AP_Param *

+AP_Param::find(const char *name, enum ap_var_type *ptype)

+{

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

+        uint8_t type = PGM_UINT8(&_var_info[i].type);

+        if (type == AP_PARAM_GROUP) {

+            uint8_t len = strnlen_P(_var_info[i].name, AP_MAX_NAME_SIZE);

+            if (strncmp_P(name, _var_info[i].name, len) != 0) {

+                continue;

+            }

+            const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);

+            AP_Param *ap = find_group(name + len, i, group_info, ptype);

+            if (ap != NULL) {

+                return ap;

+            }

+            // we continue looking as we want to allow top level

+            // parameter to have the same prefix name as group

+            // parameters, for example CAM_P_G

+        } else if (strcasecmp_P(name, _var_info[i].name) == 0) {

+            *ptype = (enum ap_var_type)type;

+            return (AP_Param *)PGM_POINTER(&_var_info[i].ptr);

+        }

+    }

+    return NULL;

+}

+

+// Save the variable to EEPROM, if supported

+//

+bool AP_Param::save(void)

+{

+    uint8_t group_element = 0;

+    const struct GroupInfo *ginfo;

+    uint8_t idx;

+    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);

+    const AP_Param *ap;

+

+    if (info == NULL) {

+        // we don't have any info on how to store it

+        return false;

+    }

+

+    struct Param_header phdr;

+

+    // create the header we will use to store the variable

+    if (ginfo != NULL) {

+        phdr.type = PGM_UINT8(&ginfo->type);

+    } else {

+        phdr.type = PGM_UINT8(&info->type);

+    }

+    phdr.key  = PGM_UINT8(&info->key);

+    phdr.group_element = group_element;

+

+    ap = this;

+    if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {

+        // only vector3f can have non-zero idx for now

+        return false;

+    }

+    if (idx != 0) {

+        ap = (const AP_Param *)((uintptr_t)ap) - (idx*sizeof(float));

+    }

+

+    // scan EEPROM to find the right location

+    uint16_t ofs;

+    if (scan(&phdr, &ofs)) {

+        // found an existing copy of the variable

+        eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));

+        return true;

+    }

+    if (ofs == (uint16_t)~0) {

+        return false;

+    }

+

+    // write a new sentinal, then the data, then the header

+    write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));

+    eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));

+    eeprom_write_check(&phdr, ofs, sizeof(phdr));

+    return true;

+}

+

+// Load the variable from EEPROM, if supported

+//

+bool AP_Param::load(void)

+{

+    uint8_t group_element = 0;

+    const struct GroupInfo *ginfo;

+    uint8_t idx;

+    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);

+    if (info == NULL) {

+        // we don't have any info on how to load it

+        return false;

+    }

+

+    struct Param_header phdr;

+

+    // create the header we will use to match the variable

+    if (ginfo != NULL) {

+        phdr.type = PGM_UINT8(&ginfo->type);

+    } else {

+        phdr.type = PGM_UINT8(&info->type);

+    }

+    phdr.key  = PGM_UINT8(&info->key);

+    phdr.group_element = group_element;

+

+    // scan EEPROM to find the right location

+    uint16_t ofs;

+    if (!scan(&phdr, &ofs)) {

+        return false;

+    }

+

+    if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {

+        // only vector3f can have non-zero idx for now

+        return false;

+    }

+

+    AP_Param *ap;

+    ap = this;

+    if (idx != 0) {

+        ap = (AP_Param *)((uintptr_t)ap) - (idx*sizeof(float));

+    }

+

+    // found it

+    eeprom_read_block(ap, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type));

+    return true;

+}

+

+// Load all variables from EEPROM

+//

+bool AP_Param::load_all(void)

+{

+    struct Param_header phdr;

+    uint16_t ofs = sizeof(AP_Param::EEPROM_header);

+    while (ofs < _eeprom_size) {

+        eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr));

+        // note that this is an || not an && for robustness

+        // against power off while adding a variable

+        if (phdr.type == _sentinal_type ||

+            phdr.key == _sentinal_key ||

+            phdr.group_element == _sentinal_group) {

+            // we've reached the sentinal

+            return true;

+        }

+

+        const struct AP_Param::Info *info;

+        void *ptr;

+

+        info = find_by_header(phdr, &ptr);

+        if (info != NULL) {

+            eeprom_read_block(ptr, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type));

+        }

+

+        ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);

+    }

+

+    // we didn't find the sentinal

+    serialDebug("no sentinal in load_all");

+    return false;

+}

+

+

+// return the first variable in _var_info

+AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)

+{

+    token->key = 0;

+    token->group_element = 0;

+    token->idx = 0;

+    if (_num_vars == 0) {

+        return NULL;

+    }

+    if (ptype != NULL) {

+        *ptype = (enum ap_var_type)PGM_UINT8(&_var_info[0].type);

+    }

+    return (AP_Param *)(PGM_POINTER(&_var_info[0].ptr));

+}

+

+/// Returns the next variable in a group, recursing into groups

+/// as needed

+AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_info,

+                               bool *found_current,

+                               uint8_t group_base,

+                               uint8_t group_shift,

+                               ParamToken *token,

+                               enum ap_var_type *ptype)

+{

+    enum ap_var_type type;

+    for (uint8_t i=0;

+         (type=(enum ap_var_type)PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;

+         i++) {

+#ifdef AP_NESTED_GROUPS_ENABLED

+        if (type == AP_PARAM_GROUP) {

+            // a nested group

+            const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);

+            AP_Param *ap;

+            ap = next_group(vindex, ginfo, found_current, GROUP_ID(group_info, group_base, i, group_shift),

+                            group_shift + _group_level_shift, token, ptype);

+            if (ap != NULL) {

+                return ap;

+            }

+        } else

+#endif // AP_NESTED_GROUPS_ENABLED

+        {

+            if (*found_current) {

+                // got a new one

+                token->key = vindex;

+                token->group_element = GROUP_ID(group_info, group_base, i, group_shift);

+                token->idx = 0;

+                if (ptype != NULL) {

+                    *ptype = type;

+                }

+                return (AP_Param*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));

+            }

+            if (GROUP_ID(group_info, group_base, i, group_shift) == token->group_element) {

+                *found_current = true;

+                if (type == AP_PARAM_VECTOR3F && token->idx < 3) {

+                    // return the next element of the vector as a

+                    // float

+                    token->idx++;

+                    if (ptype != NULL) {

+                        *ptype = AP_PARAM_FLOAT;

+                    }

+                    uintptr_t ofs = (uintptr_t)PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset);

+                    ofs += sizeof(float)*(token->idx-1);

+                    return (AP_Param *)ofs;

+                }

+            }

+        }

+    }

+    return NULL;

+}

+

+/// Returns the next variable in _var_info, recursing into groups

+/// as needed

+AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype)

+{

+    uint8_t i = token->key;

+    bool found_current = false;

+    if (i >= _num_vars) {

+        // illegal token

+        return NULL;

+    }

+    enum ap_var_type type = (enum ap_var_type)PGM_UINT8(&_var_info[i].type);

+

+    // allow Vector3f to be seen as 3 variables. First as a vector,

+    // then as 3 separate floats

+    if (type == AP_PARAM_VECTOR3F && token->idx < 3) {

+        token->idx++;

+        if (ptype != NULL) {

+            *ptype = AP_PARAM_FLOAT;

+        }

+        return (AP_Param *)(((token->idx-1)*sizeof(float))+(uintptr_t)PGM_POINTER(&_var_info[i].ptr));

+    }

+

+    if (type != AP_PARAM_GROUP) {

+        i++;

+        found_current = true;

+    }

+    for (; i<_num_vars; i++) {

+        type = (enum ap_var_type)PGM_UINT8(&_var_info[i].type);

+        if (type == AP_PARAM_GROUP) {

+            const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info);

+            AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, token, ptype);

+            if (ap != NULL) {

+                return ap;

+            }

+        } else {

+            // found the next one

+            token->key = i;

+            token->group_element = 0;

+            token->idx = 0;

+            if (ptype != NULL) {

+                *ptype = type;

+            }

+            return (AP_Param *)(PGM_POINTER(&_var_info[i].ptr));

+        }

+    }

+    return NULL;

+}

+

+/// Returns the next scalar in _var_info, recursing into groups

+/// as needed

+AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype)

+{

+    AP_Param *ap;

+    enum ap_var_type type;

+    while ((ap = next(token, &type)) != NULL && type > AP_PARAM_FLOAT) ;

+    if (ap != NULL && ptype != NULL) {

+        *ptype = type;

+    }

+    return ap;

+}

+

+

+/// cast a variable to a float given its type

+float AP_Param::cast_to_float(enum ap_var_type type)

+{

+    switch (type) {

+    case AP_PARAM_INT8:

+        return ((AP_Int8 *)this)->cast_to_float();

+    case AP_PARAM_INT16:

+        return ((AP_Int16 *)this)->cast_to_float();

+    case AP_PARAM_INT32:

+        return ((AP_Int32 *)this)->cast_to_float();

+    case AP_PARAM_FLOAT:

+        return ((AP_Float *)this)->cast_to_float();

+    default:

+        return NAN;

+    }

+}

+

+

+// print the value of all variables

+void AP_Param::show_all(void)

+{

+    ParamToken token;

+    AP_Param *ap;

+    enum ap_var_type type;

+

+    for (ap=AP_Param::first(&token, &type);

+         ap;

+         ap=AP_Param::next_scalar(&token, &type)) {

+        char s[AP_MAX_NAME_SIZE+1];

+        ap->copy_name(s, sizeof(s), true);

+        s[AP_MAX_NAME_SIZE] = 0;

+

+        switch (type) {

+        case AP_PARAM_INT8:

+            Serial.printf_P("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());

+            break;

+        case AP_PARAM_INT16:

+            Serial.printf_P("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());

+            break;

+        case AP_PARAM_INT32:

+            Serial.printf_P("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());

+            break;

+        case AP_PARAM_FLOAT:

+            Serial.printf_P("%s: %f\n", s, ((AP_Float *)ap)->get());

+            break;

+        default:

+            break;

+        }

+    }

+}

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Param.h
@@ -1,1 +1,474 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+/// @file	AP_Param.h

+/// @brief	A system for managing and storing variables that are of

+///			general interest to the system.

+

+#ifndef AP_PARAM_H

+#define AP_PARAM_H

+#include <stddef.h>

+#include <string.h>

+#include <stdint.h>

+

+#include <avr/pgmspace.h>

+#include <avr/eeprom.h>

+

+#define AP_MAX_NAME_SIZE 15

+#define AP_NESTED_GROUPS_ENABLED

+

+// a variant of offsetof() to work around C++ restrictions.

+// this can only be used when the offset of a variable in a object

+// is constant and known at compile time

+#define AP_VAROFFSET(type, element) (((uintptr_t)(&((const type *)1)->element))-1)

+

+// find the type of a variable given the class and element

+#define AP_CLASSTYPE(class, element) (((const class *)1)->element.vtype)

+

+// declare a group var_info line

+#define AP_GROUPINFO(name, idx, class, element) { AP_CLASSTYPE(class, element), idx, name, AP_VAROFFSET(class, element) }

+

+// declare a nested group entry in a group var_info

+#ifdef AP_NESTED_GROUPS_ENABLED

+#define AP_NESTEDGROUPINFO(class, idx) { AP_PARAM_GROUP, idx, "", 0, class::var_info }

+#endif

+

+#define AP_GROUPEND	{ AP_PARAM_NONE, 0xFF, "" }

+

+enum ap_var_type {

+    AP_PARAM_NONE    = 0,

+    AP_PARAM_INT8,

+    AP_PARAM_INT16,

+    AP_PARAM_INT32,

+    AP_PARAM_FLOAT,

+    AP_PARAM_VECTOR3F,

+    AP_PARAM_VECTOR6F,

+    AP_PARAM_MATRIX3F,

+    AP_PARAM_GROUP

+};

+

+/// Base class for variables.

+///

+/// Provides naming and lookup services for variables.

+///

+class AP_Param

+{

+public:

+    // the Info and GroupInfo structures are passed by the main

+    // program in setup() to give information on how variables are

+    // named and their location in memory

+    struct GroupInfo {

+        uint8_t type; // AP_PARAM_*

+        uint8_t idx;  // identifier within the group

+        const char name[AP_MAX_NAME_SIZE];

+        uintptr_t offset; // offset within the object

+        const struct GroupInfo *group_info;

+    };

+    struct Info {

+        uint8_t type; // AP_PARAM_*

+        const char name[AP_MAX_NAME_SIZE];

+        uint8_t key; // k_param_*

+        void *ptr;    // pointer to the variable in memory

+        const struct GroupInfo *group_info;

+    };

+

+    // a token used for first()/next() state

+    typedef struct {

+        uint8_t key;

+        uint8_t group_element;

+        uint8_t idx; // offset into array types

+    } ParamToken;

+

+    // called once at startup to setup the _var_info[] table. This

+    // will also check the EEPROM header and re-initialise it if the

+    // wrong version is found

+    static bool setup(const struct Info *info, uint8_t num_vars, uint16_t eeprom_size);

+

+    // return true if AP_Param has been initialised via setup()

+    static bool initialised(void);

+

+    /// Copy the variable's name, prefixed by any containing group name, to a buffer.

+    ///

+    /// If the variable has no name, the buffer will contain an empty string.

+    ///

+    /// Note that if the combination of names is larger than the buffer, the

+    /// result in the buffer will be truncated.

+    ///

+    /// @param	buffer			The destination buffer

+    /// @param	bufferSize		Total size of the destination buffer.

+    ///

+    void copy_name(char *buffer, size_t bufferSize, bool force_scalar=false);

+

+    /// Find a variable by name.

+    ///

+    /// If the variable has no name, it cannot be found by this interface.

+    ///

+    /// @param  name            The full name of the variable to be found.

+    /// @return                 A pointer to the variable, or NULL if

+    ///                         it does not exist.

+    ///

+    static AP_Param *find(const char *name, enum ap_var_type *ptype);

+

+    /// Save the current value of the variable to EEPROM.

+    ///

+    /// @return                True if the variable was saved successfully.

+    ///

+    bool save(void);

+

+    /// Load the variable from EEPROM.

+    ///

+    /// @return                True if the variable was loaded successfully.

+    ///

+    bool load(void);

+

+    /// Load all variables from EEPROM

+    ///

+    /// This function performs a best-efforts attempt to load all

+    /// of the variables from EEPROM.  If some fail to load, their

+    /// values will remain as they are.

+    ///

+    /// @return                False if any variable failed to load

+    ///

+    static bool load_all(void);

+

+    /// Erase all variables in EEPROM.

+    ///

+    static void erase_all(void);

+

+    /// print the value of all variables

+    static void show_all(void);

+

+    /// Returns the first variable

+    ///

+    /// @return             The first variable in _var_info, or NULL if

+    ///                     there are none.

+    ///

+    static AP_Param *first(ParamToken *token, enum ap_var_type *ptype);

+

+    /// Returns the next variable in _var_info, recursing into groups

+    /// as needed

+    static AP_Param *next(ParamToken *token, enum ap_var_type *ptype);

+

+    /// Returns the next scalar variable in _var_info, recursing into groups

+    /// as needed

+    static AP_Param *next_scalar(ParamToken *token, enum ap_var_type *ptype);

+

+    /// cast a variable to a float given its type

+    float cast_to_float(enum ap_var_type type);

+

+private:

+    /// EEPROM header

+    ///

+    /// This structure is placed at the head of the EEPROM to indicate

+    /// that the ROM is formatted for AP_Param.

+    ///

+    struct EEPROM_header {

+        uint8_t     magic[2];

+        uint8_t     revision;

+        uint8_t     spare;

+    };

+

+    // This header is prepended to a variable stored in EEPROM.

+    struct Param_header {

+        uint8_t    key;

+        uint8_t    group_element;

+        uint8_t    type;

+    };

+

+    // number of bits in each level of nesting of groups

+    static const uint8_t _group_level_shift = 4;

+    static const uint8_t _group_bits  = 8;

+

+    static const uint8_t  _sentinal_key   = 0xFF;

+    static const uint8_t  _sentinal_type  = 0xFF;

+    static const uint8_t  _sentinal_group = 0xFF;

+

+    static bool check_group_info(const struct GroupInfo *group_info, uint16_t *total_size, uint8_t max_bits);

+    static bool duplicate_key(uint8_t vindex, uint8_t key);

+    static bool check_var_info(void);

+    const struct Info *find_var_info_group(const struct GroupInfo *group_info,

+                                           uint8_t vindex,

+                                           uint8_t group_base,

+                                           uint8_t group_shift,

+                                           uint8_t *group_element,

+                                           const struct GroupInfo **group_ret,

+                                           uint8_t *idx);

+    const struct Info *find_var_info(uint8_t *group_element,

+                                     const struct GroupInfo **group_ret,

+                                     uint8_t *idx);

+    static const struct Info *find_by_header_group(struct Param_header phdr, void **ptr,

+                                                   uint8_t vindex,

+                                                   const struct GroupInfo *group_info,

+                                                   uint8_t group_base,

+                                                   uint8_t group_shift);

+    static const struct Info *find_by_header(struct Param_header phdr, void **ptr);

+    void add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx);

+    static AP_Param *find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype);

+    static void write_sentinal(uint16_t ofs);

+    bool scan(const struct Param_header *phdr, uint16_t *pofs);

+    static const uint8_t type_size(enum ap_var_type type);

+    static void eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size);

+    static AP_Param *next_group(uint8_t vindex, const struct GroupInfo *group_info,

+                                bool *found_current,

+                                uint8_t group_base,

+                                uint8_t group_shift,

+                                ParamToken *token,

+                                enum ap_var_type *ptype);

+

+    static uint16_t _eeprom_size;

+    static uint8_t _num_vars;

+    static const struct Info *_var_info;

+

+    // values filled into the EEPROM header

+    static const uint8_t   k_EEPROM_magic0      = 0x50;

+    static const uint8_t   k_EEPROM_magic1      = 0x41;   ///< "AP"

+    static const uint8_t   k_EEPROM_revision    = 5;        ///< current format revision

+};

+

+/// Template class for scalar variables.

+///

+/// Objects of this type have a value, and can be treated in many ways as though they

+/// were the value.

+///

+/// @tparam T			The scalar type of the variable

+/// @tparam PT			The AP_PARAM_* type

+///

+template<typename T, ap_var_type PT>

+class AP_ParamT : public AP_Param

+{

+public:

+    /// Constructor for scalar variable.

+    ///

+    /// Initialises a stand-alone variable with optional initial value.

+    ///

+    /// @param  default_value   Value the variable should have at startup.

+    ///

+    AP_ParamT<T,PT> (const T initial_value = 0) :

+        AP_Param(),

+        _value(initial_value)

+    {

+    }

+

+    static const ap_var_type vtype = PT;

+

+    /// Value getter

+    ///

+    T get(void) const {

+        return _value;

+    }

+

+    /// Value setter

+    ///

+    void set(T v) {

+        _value = v;

+    }

+

+    /// Combined set and save

+    ///

+    bool set_and_save(T v) {

+        set(v);

+        return save();

+    }

+

+    /// Combined set and save, but only does the save if the value if

+    /// different from the current ram value, thus saving us a

+    /// scan(). This should only be used where we have not set() the

+    /// value separately, as otherwise the value in EEPROM won't be

+    /// updated correctly.

+    bool set_and_save_ifchanged(T v) {

+        if (v == _value) {

+            return true;

+        }

+        set(v);

+        return save();

+    }

+

+    /// Conversion to T returns a reference to the value.

+    ///

+    /// This allows the class to be used in many situations where the value would be legal.

+    ///

+    operator T &() {

+        return _value;

+    }

+

+    /// Copy assignment from self does nothing.

+    ///

+    AP_ParamT<T,PT>& operator=(AP_ParamT<T,PT>& v) {

+        return v;

+    }

+

+    /// Copy assignment from T is equivalent to ::set.

+    ///

+    AP_ParamT<T,PT>& operator=(T v) {

+        _value = v;

+        return *this;

+    }

+

+    /// AP_ParamT types can implement AP_Param::cast_to_float

+    ///

+    float cast_to_float(void) {

+        return (float)_value;

+    }

+

+protected:

+    T _value;

+};

+

+

+/// Template class for non-scalar variables.

+///

+/// Objects of this type have a value, and can be treated in many ways as though they

+/// were the value.

+///

+/// @tparam T			The scalar type of the variable

+/// @tparam PT			AP_PARAM_* type

+///

+template<typename T, ap_var_type PT>

+class AP_ParamV : public AP_Param

+{

+public:

+    static const ap_var_type vtype = PT;

+

+    /// Value getter

+    ///

+    T get(void) const {

+        return _value;

+    }

+

+    /// Value setter

+    ///

+    void set(T v) {

+        _value = v;

+    }

+

+    /// Combined set and save

+    ///

+    bool set_and_save(T v) {

+        set(v);

+        return save();

+    }

+

+    /// Conversion to T returns a reference to the value.

+    ///

+    /// This allows the class to be used in many situations where the value would be legal.

+    ///

+    operator T &() {

+        return _value;

+    }

+

+    /// Copy assignment from self does nothing.

+    ///

+    AP_ParamV<T,PT>& operator=(AP_ParamV<T,PT>& v) {

+        return v;

+    }

+

+    /// Copy assignment from T is equivalent to ::set.

+    ///

+    AP_ParamV<T,PT>& operator=(T v) {

+        _value = v;

+        return *this;

+    }

+

+protected:

+    T _value;

+};

+

+

+/// Template class for array variables.

+///

+/// Objects created using this template behave like arrays of the type T,

+/// but are stored like single variables.

+///

+/// @tparam T           The scalar type of the variable

+/// @tparam N           number of elements

+/// @tparam PT          the AP_PARAM_* type

+///

+template<typename T, uint8_t N, ap_var_type PT>

+class AP_ParamA : public AP_Param

+{

+public:

+    static const ap_var_type vtype = PT;

+

+    /// Array operator accesses members.

+    ///

+    /// @note It would be nice to range-check i here, but then what would we return?

+    ///

+    T &operator [](uint8_t i) {

+        return _value[i];

+    }

+

+    /// Value getter

+    ///

+    /// @note   Returns zero for index values out of range.

+    ///

+    T get(uint8_t i) const {

+        if (i < N) {

+            return _value[i];

+        } else {

+            return (T)0;

+        }

+    }

+

+    /// Value setter

+    ///

+    /// @note   Attempts to set an index out of range are discarded.

+    ///

+    void set(uint8_t i, T v) {

+        if (i < N) {

+            _value[i] = v;

+        }

+    }

+

+    /// Copy assignment from self does nothing.

+    ///

+    AP_ParamA<T,N,PT>& operator=(AP_ParamA<T,N,PT>& v) {

+        return v;

+    }

+

+protected:

+    T _value[N];

+};

+

+

+

+/// Convenience macro for defining instances of the AP_ParamT template.

+///

+// declare a scalar type

+// _t is the base type

+// _suffix is the suffix on the AP_* type name

+// _pt is the enum ap_var_type type

+#define AP_PARAMDEF(_t, _suffix, _pt)   typedef AP_ParamT<_t, _pt> AP_##_suffix;

+AP_PARAMDEF(float, Float, AP_PARAM_FLOAT);    // defines AP_Float

+AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8);     // defines AP_Int8

+AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16);  // defines AP_Int16

+AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32);  // defines AP_Int32

+

+// declare an array type

+// _t is the base type

+// _suffix is the suffix on the AP_* type name

+// _size is the size of the array

+// _pt is the enum ap_var_type type

+#define AP_PARAMDEFA(_t, _suffix, _size, _pt)   typedef AP_ParamA<_t, _size, _pt> AP_##_suffix;

+AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F);

+

+// declare a non-scalar type

+// this is used in AP_Math.h

+// _t is the base type

+// _suffix is the suffix on the AP_* type name

+// _pt is the enum ap_var_type type

+#define AP_PARAMDEFV(_t, _suffix, _pt)   typedef AP_ParamV<_t, _pt> AP_##_suffix;

+

+/// Rely on built in casting for other variable types

+/// to minimize template creation and save memory

+#define AP_Uint8    AP_Int8

+#define AP_Uint16   AP_Int16

+#define AP_Uint32   AP_Int32

+#define AP_Bool     AP_Int8

+

+#endif // AP_PARAM_H

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Test.h
@@ -1,1 +1,128 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+/// @file   AP_Test.h

+/// @brief  A simple unit test framework.

+///

+/// AP_Test provides the usual test start, condition validation and reporting

+/// functions in a compact form.

+///

+/// Each test must be contained within a block; either a standalone function or

+/// a block within a function.  The TEST macro is used to start a test; it creates

+/// the local object which will track the results of the test and saves the name

+/// for later reporting.  Only one test may be performed within each block.

+///

+/// Within the test, use the REQUIRE macro to describe a condition that must be

+/// met for the test to pass.  If the condition within the macro is not met,

+/// the condition will be output as a diagnostic and the test will be considered

+/// to have failed.

+///

+/// The test ends at the end of the block, and the result of the test will be

+/// output as a diagnostic.

+///

+/// Optionally at the end of the test suite, the Test::report method may be used

+/// to summarize the results of all of the tests that were performed.

+///

+

+/// Unit test state and methods.

+///

+class Test

+{

+public:

+    /// Constructor - creates a new test.

+    ///

+    /// Normally called by the TEST macro.

+    ///

+    /// @param  name    The name of the test being started.

+    ///

+    Test(const char *name);

+

+    /// Destructor - ends the test.

+    ///

+    ~Test();

+

+    /// Perform a success check.

+    ///

+    /// @param  expr    If false, the test has failed.

+    /// @param  source  The expression source; emitted in the diagnostic

+    ///                 indicating test failure.

+    ///

+    void        require(bool expr, const char *source);

+

+    /// Report the overall number of tests/pass/fails.

+    ///

+    static void report();

+

+private:

+    const char  *_name;         ///< name of the current test

+    bool        _fail;          ///< set if any ::require calls indicate the test failed

+    static int  _passed;        ///< global pass count

+    static int  _failed;        ///< global fail count

+};

+

+/// Constructor

+///

+Test::Test(const char *name) :

+    _name(name),

+    _fail(false)

+{

+}

+

+/// Destructor

+///

+Test::~Test()

+{

+    Serial.printf("%s: %s\n", _fail ? "FAILED" : "passed", _name);

+    if (_fail) {

+        _failed++;

+    } else {

+        _passed++;

+    }

+}

+

+/// Success check

+///

+void

+Test::require(bool expr, const char *source)

+{

+    if (!expr) {

+        _fail = true;

+        Serial.printf("%s: fail: %s\n", _name, source);

+    }

+}

+

+/// Summary report

+///

+void

+Test::report()

+{

+    Serial.printf("\n%d passed  %d failed\n", _passed, _failed);

+}

+

+int Test::_passed = 0;

+int Test::_failed = 0;

+

+/// Start a new test.

+///

+/// This should be invoked at the beginning of a block, before any REQUIRE

+/// statements.  A new test called name is started, and subsequent REQUIRE

+/// statements will be applied to the test.  The test will continue until

+/// the end of the block (or until the _test object that is created otherwise

+/// goes out of scope).

+///

+#define TEST(name)      Test _test(#name)

+

+/// Attach an expression to the test's success criteria.

+///

+/// The expression expr must evaluate true for the test to pass.  If

+/// it does not, the text of the expression is output as a diagnostic

+/// and the test is marked as a failure.

+///

+#define REQUIRE(expr)   _test.require(expr, #expr)

+

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Var_menufuncs.cpp
@@ -1,1 +1,16 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+/// @file   AP_Var_menufuncs.cpp

+/// @brief  Useful functions compatible with the menu system for

+///         managing AP_Var variables.

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+

 

--- /dev/null
+++ b/libraries/AP_Common/AP_Vector.h
@@ -1,1 +1,383 @@
+/*

+ * Vector.h

+ * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#ifndef Vector_H

+#define Vector_H

+

+#include "../FastSerial/BetterStream.h"

+#include <stdlib.h>

+#include <inttypes.h>

+#if defined(ARDUINO) && ARDUINO >= 100

+	#include "Arduino.h"

+#else

+	#include <WProgram.h>

+#endif

+

+#ifdef ASSERT

+const static char vectorSource[] ="Vector.hpp";

+#endif

+

+// vector

+template <class dataType,class sumType=dataType>

+class Vector

+{

+private:

+    size_t size;

+    size_t extraAllocationSize; // extra space to add after each allocation

+    size_t sizeAllocated; // total allocated size

+    dataType* data;

+public:

+    // default constructor

+    Vector(const size_t & _size=0, const size_t & _extraAllocationSize=0) : size(0), extraAllocationSize(_extraAllocationSize), sizeAllocated(0), data(NULL) {

+        setSize(_size);

+    }

+    // 3 vector constructor

+    Vector(const dataType & a, const dataType & b, const dataType & c) : size(3), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {

+        setSize(size);

+        (*this)[0]=a;

+        (*this)[1]=b;

+        (*this)[2]=c;

+    }

+

+    // construct from array

+    Vector(const dataType* array, const size_t & _size, const size_t & _extraAllocationSize=0) : size(0), extraAllocationSize(_extraAllocationSize), sizeAllocated(0), data(NULL) {

+        setSize(_size);

+        for (size_t i=0; i<getSize(); i++)

+            (*this)[i]=array[i];

+    }

+    // copy constructor

+    Vector(const Vector &v) : size(0), extraAllocationSize(0), sizeAllocated(0), data(NULL) {

+        setSize(v.getSize());

+        for (size_t i=0; i<getSize(); i++)

+            (*this)[i]=v[i];

+    }

+    // convert to float vector

+    Vector<float> toFloat() const {

+        Vector<float> v(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            v[i]=(*this)[i];

+        return v;

+    }

+    // destructor

+    virtual ~Vector() {

+        empty();

+    }

+    void empty() {

+        if (data) delete [] data;

+        data = NULL;

+        sizeAllocated=0;

+        size=0;

+    }

+    // set the size

+    void setSize(const size_t & n) {

+        if (n==0) {

+            if (data) delete [] data;

+            data = NULL;

+            sizeAllocated=0;

+        }

+        if (n>sizeAllocated) {

+            dataType * newData = new dataType[n+extraAllocationSize];

+            memcpy(newData,data,sizeof(dataType)/sizeof(char)*getSize());

+            memset(newData+size,0,sizeof(dataType)/sizeof(char)*(n-getSize()));

+            delete[] data;

+            data = newData;

+            sizeAllocated=n+extraAllocationSize;

+        }

+        size=n;

+    }

+    // return size

+    const size_t & getSize() const {

+        return size;

+    }

+    // insert

+    void insert(const size_t index, const dataType value) {

+        //Serial.println("insert called");

+#ifdef ASSERT

+        assert(index<size+1,vectorSource,__LINE__);

+#endif

+        //Serial.print("Old Size: ");

+        //Serial.println(getSize());

+        setSize(getSize()+1);

+        //Serial.print("New Size: ");

+        //Serial.println(getSize());

+        //Serial.print("Size of dataType");

+        //Serial.println(sizeof(dataType));

+        if (index != getSize()-1) {

+            memmove(data+index+1,data+index,sizeof(dataType)/sizeof(char)*(getSize()-1-index));

+            //Serial.println("memmove called and completed");

+        }

+        (*this)[index]=value;

+        //Serial.println("insert done");

+    }

+    // remove

+    void remove(const size_t & index) {

+#ifdef ASSERT

+        assert(index<size,vectorSource,__LINE__);

+#endif

+        memmove(data+index,data+index+1,getSize()-index-1);

+        setSize(getSize()-1);

+    }

+    // push_back

+    void push_back(const dataType & value) {

+        //Serial.println("push_back called");

+        insert(getSize(),value);

+        //Serial.println("push_back done");

+    }

+    // pop_front

+    dataType & pop_front() {

+        dataType tmp = (*this)[0];

+        remove(0);

+        return tmp;

+    }

+    // push_back a vector

+    void push_back(const Vector & vector) {

+        for (size_t i=0; i<vector.getSize(); i++)

+            push_back(vector[i]);

+    }

+    // const array access operator

+    const dataType & operator[](const size_t & index) const {

+#ifdef ASSERT

+        assert(index<getSize(),vectorSource,__LINE__);

+#endif

+        return data[index];

+    }

+    // array access operator

+    dataType & operator[](const size_t & index) {

+#ifdef ASSERT

+        assert(index<getSize(),vectorSource,__LINE__);

+#endif

+        return data[index];

+    }

+    // assignment operator

+    Vector & operator=(const Vector & v) {

+        setSize(v.getSize());

+        for (size_t i=0; i<getSize(); i++)

+            (*this)[i]=v[i];

+        return *this;

+    }

+    // equal

+    const bool operator==(const Vector& v) const {

+#ifdef ASSERT

+        assert(getSize()==v.getSize(),vectorSource,__LINE__);

+#endif

+        for (size_t i=0; i<getSize(); i++) {

+            if ((*this)[i]!=v[i]) return false;

+        }

+        return true;

+    }

+    // not equal

+    const bool operator!=(const Vector& v) const {

+        return !((*this)==v);

+    }

+    // addition

+    const Vector operator+(const Vector& v) const {

+#ifdef ASSERT

+        assert(v.getSize() == getSize(),vectorSource,__LINE__);

+#endif

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result(i)=(*this)[i]+v[i];

+        return result;

+    }

+    // addition

+    const Vector operator+(const dataType& s) const {

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result[i]=(*this)[i]+s;

+        return result;

+    }

+    // subtraction

+    const Vector operator-(const Vector& v) const {

+#ifdef ASSERT

+        assert(v.getSize() == getSize(),vectorSource,__LINE__);

+#endif

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result[i]=(*this)[i]-v[i];

+        return result;

+    }

+    // negation

+    const Vector operator-() const {

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result[i]=-(*this)[i];

+        return result;

+    }

+    // +=

+    Vector& operator+=(const Vector& v) {

+#ifdef ASSERT

+        assert(v.getSize() == getSize(),vectorSource,__LINE__);

+#endif

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            (*this)(i)+=v(i);

+        return *this;

+    }

+    // -=

+    Vector& operator-=( const Vector& v) {

+#ifdef ASSERT

+        assert(v.getSize() == getSize(),vectorSource,__LINE__);

+#endif

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            (*this)(i)-=v(i);

+        return *this;

+    }

+    // elementwise  mult.

+    const Vector operator*(const Vector & v) const {

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result(i)=(*this)(i)*v(i);

+        return result;

+    }

+

+    // mult. by a scalar

+    const Vector operator*(const dataType & s) const {

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result(i)=(*this)(i)*s;

+        return result;

+    }

+    // div. by a scalar

+    const Vector operator/(const dataType & s) const {

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result(i)=(*this)(i)/s;

+        return result;

+    }

+    // elementwise div.

+    const Vector operator/(const Vector & v) const {

+        Vector result(getSize());

+        for (size_t i=0; i<getSize(); i++)

+            result(i)=(*this)(i)/v(i);

+        return result;

+    }

+

+    // div. by a scalar

+    Vector & operator/=(const dataType & s) {

+        for (size_t i=0; i<getSize(); i++)

+            (*this)(i)/=s;

+        return *this;

+    }

+    // mult. by a scalar

+    Vector & operator*=(const dataType & s) {

+        for (size_t i=0; i<getSize(); i++)

+            (*this)(i)*=s;

+        return *this;

+    }

+    // cross/vector product

+    const Vector cross(const Vector& v) const {

+        Vector result(3), u=*this;

+#ifdef ASSERT

+        assert(u.getSize()==3 && v.getSize()==3,vectorSource,__LINE__);

+#endif

+        result(0) = u(1)*v(2)-u(2)*v(1);

+        result(1) = -u(0)*v(2)+u(2)*v(0);

+        result(2) = u(0)*v(1)-u(1)*v(0);

+        return result;

+    }

+    // dot/scalar product

+    const dataType dot(const Vector& v) const {

+#ifdef ASSERT

+        assert(getSize()==v.getSize(),vectorSource,__LINE__);

+#endif

+        dataType result;

+        for (size_t i=0; i<getSize(); i++) result += (*this)(i)*v(i);

+        return result;

+    }

+    // norm

+    const dataType norm() const {

+        return sqrt(dot(*this));

+    }

+    // unit vector

+    const Vector unit() const {

+        return (*this)*(1/norm());

+    }

+    // sum

+    const sumType sum(const size_t & start=0,const int & end=-1) const {

+        size_t end2;

+        if (end==-1) end2=getSize()-1;

+        else end2=end;

+        sumType _sum = 0;

+        for (size_t i=start; i<=end2; i++) _sum += (*this)(i);

+        return _sum;

+    }

+    void sumFletcher(uint8_t & CK_A, uint8_t & CK_B, const size_t & start=0,const int & end=-1) const {

+        size_t end2;

+        if (end==-1) end2=getSize()-1;

+        else end2=end;

+

+        for (size_t i = start; i<=end2; i++) {

+            CK_A += (*this)(i);

+            CK_B += CK_A;

+        }

+    }

+    // range

+    const Vector range(const size_t & start, const size_t & stop) const {

+        Vector v(stop-start+1);

+        for (size_t i=start; i<=stop; i++) v(i-start) = (*this)(i);

+        return v;

+    }

+    // to Array

+    const dataType* toArray() const {

+        dataType array[getSize()];

+        for (size_t i=0; i<getSize(); i++) array[i] = (*this)(i);

+        return array;

+    }

+    // printing

+    void print(Stream & serial=Serial, const char * msg="", size_t format=0) const {

+        serial.print(msg);

+        for (size_t i=0; i<getSize(); i++) {

+            serial.print((*this)(i),format);

+            serial.print(" ");

+        }

+        serial.println();

+    }

+    // self test

+    static bool selfTest(Stream & serial=Serial) {

+        serial.println("Vector self test.");

+        Vector u(3),v(3),w(3);

+        u(0) = 1;

+        u(1) = 2;

+        u(2) = 3;

+        v(0) = -4;

+        v(1) = -5;

+        v(2) = -6;

+        u.print(serial,"u: ");

+        v.print(serial,"v: ");

+        (u+v).print(serial,"u + v: ");

+        (u-v).print(serial,"u - v: ");

+        Serial.print("u dot v: ");

+        Serial.println(u.dot(v));

+        Serial.print("size of u: ");

+        Serial.println(u.getSize());

+        Serial.print("size of v: ");

+        Serial.println(v.getSize());

+        w=u.cross(v);

+        w.print(serial,"u cross v: ");

+        Serial.print("size of u cross v: ");

+        Serial.println(w.getSize());

+        return true;

+    }

+

+};

+

+#endif

+

+// vim:ts=4:sw=4:expandtab

 

--- /dev/null
+++ b/libraries/AP_Common/Arduino.mk
@@ -1,1 +1,655 @@
+#

+# Copyright (c) 2010 Michael Smith. All rights reserved.

+# 

+# Redistribution and use in source and binary forms, with or without

+# modification, are permitted provided that the following conditions

+# are met:

+# 1. Redistributions of source code must retain the above copyright

+#	  notice, this list of conditions and the following disclaimer.

+# 2. Redistributions in binary form must reproduce the above copyright

+#	  notice, this list of conditions and the following disclaimer in the

+#	  documentation and/or other materials provided with the distribution.

+# 

+# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND

+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+# ARE DISCLAIMED.	IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE

+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL

+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS

+# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)

+# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY

+# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF

+# SUCH DAMAGE.

+#

+

+#

+# Build an Arduino sketch.

+#

+

+################################################################################

+# Paths

+#

+

+#

+# Save the system type for later use.

+#

+SYSTYPE			:=	$(shell uname)

+

+# force LANG to C so awk works sanely on MacOS

+export LANG=C

+

+#

+# Locate the sketch sources based on the initial Makefile's path

+#

+SRCROOT			:=	$(realpath $(dir $(firstword $(MAKEFILE_LIST))))

+ifneq ($(findstring CYGWIN, $(SYSTYPE)),) 

+  # Workaround a $(realpath ) bug on cygwin

+  ifeq ($(SRCROOT),)

+    SRCROOT	:=	$(shell cygpath -m ${CURDIR})

+    $(warning your realpath function is not working)

+    $(warning > setting SRCROOT to $(SRCROOT))

+  endif

+  # Correct the directory backslashes on cygwin

+  ARDUINO		:=	$(subst \,/,$(ARDUINO))

+endif

+

+#

+# We need to know the location of the sketchbook.  If it hasn't been overridden,

+# try the parent of the current directory.  If there is no libraries directory

+# there, assume that we are in a library's examples directory and try backing up

+# further.

+#

+ifeq ($(SKETCHBOOK),)

+  SKETCHBOOK		:=	$(shell cd $(SRCROOT)/.. && pwd)

+  ifeq ($(wildcard $(SKETCHBOOK)/libraries),)

+    SKETCHBOOK		:=	$(shell cd $(SRCROOT)/../.. && pwd)

+  endif

+  ifeq ($(wildcard $(SKETCHBOOK)/libraries),)

+    SKETCHBOOK		:=	$(shell cd $(SRCROOT)/../../.. && pwd)

+  endif

+  ifeq ($(wildcard $(SKETCHBOOK)/libraries),)

+    SKETCHBOOK		:=	$(shell cd $(SRCROOT)/../../../.. && pwd)

+  endif

+  ifeq ($(wildcard $(SKETCHBOOK)/libraries),)

+    $(error ERROR: cannot determine sketchbook location - please specify on the commandline with SKETCHBOOK=<path>)

+  endif

+else

+  ifeq ($(wildcard $(SKETCHBOOK)/libraries),)

+    $(warning WARNING: sketchbook directory $(SKETCHBOOK) contains no libraries)

+  endif

+endif

+ifneq ($(findstring CYGWIN, $(SYSTYPE)),) 

+	# Convert cygwin path into a windows normal path

+    SKETCHBOOK	:=	$(shell cygpath -d ${SKETCHBOOK})

+    SKETCHBOOK	:=	$(subst \,/,$(SKETCHBOOK))

+endif

+

+#

+# Work out the sketch name from the name of the source directory.

+#

+SKETCH			:=	$(lastword $(subst /, ,$(SRCROOT)))

+# Workaround a $(lastword ) bug on cygwin

+ifeq ($(SKETCH),)

+  WORDLIST		:=	$(subst /, ,$(SRCROOT))

+  SKETCH		:=	$(word $(words $(WORDLIST)),$(WORDLIST))

+endif

+

+#

+# Work out where we are going to be building things

+#

+TMPDIR			?=	/tmp

+BUILDROOT		:=	$(abspath $(TMPDIR)/$(SKETCH).build)

+ifneq ($(findstring CYGWIN, $(SYSTYPE)),)

+  # Workaround a $(abspath ) bug on cygwin

+  ifeq ($(BUILDROOT),)

+    BUILDROOT	:=	C:$(TMPDIR)/$(SKETCH).build

+    $(warning your abspath function is not working)

+    $(warning > setting BUILDROOT to $(BUILDROOT))

+  endif

+endif

+

+# Jump over the next makefile sections when runing a "make configure"

+ifneq ($(MAKECMDGOALS),configure)

+

+################################################################################

+# Config options

+#

+# The Makefile calling us must specify BOARD

+#

+include $(SKETCHBOOK)/config.mk

+ifeq ($(PORT),)

+$(error ERROR: could not locate $(SKETCHBOOK)/config.mk, please run 'make configure' first)

+endif

+

+HARDWARE		?=	arduino

+ifeq ($(BOARD),)

+$(error ERROR: must set BOARD before including this file)

+endif

+

+#

+# Find Arduino, if not explicitly specified

+#

+ifeq ($(ARDUINO),)

+

+  #

+  # List locations that might be valid ARDUINO settings

+  #

+  ifeq ($(SYSTYPE),Darwin)

+    # use Spotlight to find Arduino.app

+    ARDUINO_QUERY	=	'kMDItemKind == Application && kMDItemFSName == Arduino.app'

+    ARDUINOS		:=	$(addsuffix /Contents/Resources/Java,$(shell mdfind -literal $(ARDUINO_QUERY)))

+    ifeq ($(ARDUINOS),)

+      $(error ERROR: Spotlight cannot find Arduino on your system.)

+    endif

+  endif

+

+  ifeq ($(SYSTYPE),Linux)

+    ARDUINO_SEARCHPATH	=	/usr/share/arduino* /usr/local/share/arduino*

+    ARDUINOS		:=	$(wildcard $(ARDUINO_SEARCHPATH))

+  endif

+

+  ifneq ($(findstring CYGWIN, $(SYSTYPE)),)

+	# Most of the following commands are simply to deal with whitespaces in the path

+	# Read the "Program Files" system directory from the windows registry

+	PROGRAM_FILES		:=	$(shell cat /proc/registry/HKEY_LOCAL_MACHINE/SOFTWARE/Microsoft/Windows/CurrentVersion/ProgramFilesDir)

+	# Convert the path delimiters to /

+	PROGRAM_FILES		:=	$(shell cygpath -m ${PROGRAM_FILES})

+	# Escape the space with a backslash

+	PROGRAM_FILES		:=	$(shell echo $(PROGRAM_FILES) | sed s/\ /\\\\\ / )

+	# Use DOS paths because they do not contain spaces

+	PROGRAM_FILES		:=	$(shell cygpath -d ${PROGRAM_FILES})

+	# Convert the path delimiters to /

+	PROGRAM_FILES	:=	$(subst \,/,$(PROGRAM_FILES))

+	# Search for an Arduino instalation in a couple of paths

+	ARDUINO_SEARCHPATH	:=	c:/arduino* $(PROGRAM_FILES)/arduino*

+    ARDUINOS		:=	$(wildcard $(ARDUINO_SEARCHPATH))

+  endif

+

+  #

+  # Pick the first option if more than one candidate is found.

+  #

+  ARDUINO		:=	$(firstword $(ARDUINOS))

+  ifeq ($(ARDUINO),)

+    $(error ERROR: Cannot find Arduino on this system, please specify on the commandline with ARDUINO=<path> or on the config.mk file)

+  endif

+

+  ifneq ($(words $(ARDUINOS)),1)

+    $(warning WARNING: More than one copy of Arduino was found, using $(ARDUINO))

+  endif

+

+endif

+

+################################################################################

+# Tools

+#

+

+#

+# Decide where we are going to look for tools

+#

+ifeq ($(SYSTYPE),Darwin)

+  # use the tools that come with Arduino

+  TOOLPATH		:=	$(ARDUINOS)/hardware/tools/avr/bin

+  # use BWK awk

+  AWK			=	awk

+endif

+ifeq ($(SYSTYPE),Linux)

+  # expect that tools are on the path

+  TOOLPATH		:=	$(subst :, ,$(PATH))

+endif

+ifeq ($(findstring CYGWIN, $(SYSTYPE)),CYGWIN) 

+  TOOLPATH		:=	$(ARDUINO)/hardware/tools/avr/bin

+endif

+

+ifeq ($(findstring CYGWIN, $(SYSTYPE)),) 

+FIND_TOOL		=	$(firstword $(wildcard $(addsuffix /$(1),$(TOOLPATH))))

+else

+FIND_TOOL		=	$(firstword $(wildcard $(addsuffix /$(1).exe,$(TOOLPATH))))

+endif

+CXX			:=	$(call FIND_TOOL,avr-g++)

+CC			:=	$(call FIND_TOOL,avr-gcc)

+AS			:=	$(call FIND_TOOL,avr-gcc)

+AR			:=	$(call FIND_TOOL,avr-ar)

+LD			:=	$(call FIND_TOOL,avr-gcc)

+GDB			:=	$(call FIND_TOOL,avr-gdb)

+AVRDUDE		:=	$(call FIND_TOOL,avrdude)

+AVARICE		:=	$(call FIND_TOOL,avarice)

+OBJCOPY			:=	$(call FIND_TOOL,avr-objcopy)

+ifeq ($(CXX),)

+$(error ERROR: cannot find the compiler tools anywhere on the path $(TOOLPATH))

+endif

+

+# Find awk

+AWK			?=	gawk

+ifeq ($(shell which $(AWK)),)

+$(error ERROR: cannot find $(AWK) - you may need to install GNU awk)

+endif

+

+#

+# Tool options

+#

+DEFINES			=	-DF_CPU=$(F_CPU) -DARDUINO=$(ARDUINO_VERS) $(EXTRAFLAGS)

+OPTFLAGS		=	-Os -Wformat -Wall -Wshadow -Wpointer-arith -Wcast-align -Wwrite-strings -Wformat=2 -Wno-reorder

+DEPFLAGS		=	-MD -MT $@

+

+# XXX warning options TBD

+CXXOPTS			= 	-mcall-prologues -ffunction-sections -fdata-sections -fno-exceptions

+COPTS			=	-mcall-prologues -ffunction-sections -fdata-sections

+ASOPTS			=	-assembler-with-cpp 

+LISTOPTS		=	-adhlns=$(@:.o=.lst)

+

+CXXFLAGS		=	-g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS)

+CFLAGS			=	-g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(COPTS)

+ASFLAGS			=	-g -mmcu=$(MCU) $(DEFINES)     $(LISTOPTS) $(DEPFLAGS) $(ASOPTS)

+LDFLAGS			=	-g -mmcu=$(MCU) $(OPTFLAGS) -Wl,--relax,--gc-sections -Wl,-Map -Wl,$(SKETCHMAP)

+

+ifeq ($(BOARD),mega)

+  LDFLAGS		=	-g -mmcu=$(MCU) $(OPTFLAGS) -Wl,--gc-sections -Wl,-Map -Wl,$(SKETCHMAP)

+endif

+

+

+

+LIBS			=	-lm

+

+SRCSUFFIXES		=	*.cpp *.c *.S

+

+ifeq ($(VERBOSE),)

+v			=	@

+else

+v			=

+endif

+

+

+################################################################################

+# Sketch

+#

+

+# Sketch source files

+SKETCHPDESRCS		:=	$(wildcard $(SRCROOT)/*.pde $(SRCROOT)/*.ino)

+SKETCHSRCS		:=	$(wildcard $(addprefix $(SRCROOT)/,$(SRCSUFFIXES)))

+SKETCHPDE		:=	$(wildcard $(SRCROOT)/$(SKETCH).pde $(SRCROOT)/$(SKETCH).ino)

+SKETCHCPP		:=	$(BUILDROOT)/$(SKETCH).cpp

+ifneq ($(words $(SKETCHPDE)),1)

+$(error ERROR: sketch $(SKETCH) must contain exactly one of $(SKETCH).pde or $(SKETCH).ino)

+endif

+

+# Sketch object files

+SKETCHOBJS		:=	$(subst $(SRCROOT),$(BUILDROOT),$(SKETCHSRCS)) $(SKETCHCPP)

+SKETCHOBJS		:=	$(addsuffix .o,$(basename $(SKETCHOBJS)))

+

+# List of input files to the sketch.cpp file in the order they should

+# be appended to create it

+SKETCHCPP_SRC		:=	$(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS)))

+

+################################################################################

+# Libraries

+#

+# Pick libraries to add to the include path and to link with based on

+# #include directives in the sketchfiles.

+#

+# For example:

+#

+#   #include <Foo.h>

+#

+# implies that there might be a Foo library.

+#

+# Note that the # and $ require special treatment to avoid upsetting

+# make.

+#

+SEXPR			=	's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p'

+ifeq ($(SYSTYPE),Darwin)

+  LIBTOKENS        :=    $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR)))

+else

+  LIBTOKENS        :=    $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR)))

+endif

+

+#

+# Find sketchbook libraries referenced by the sketch.

+#

+# Include paths for sketch libraries 

+#

+SKETCHLIBS		:=	$(wildcard $(addprefix $(SKETCHBOOK)/libraries/,$(LIBTOKENS)))

+SKETCHLIBNAMES		:=	$(notdir $(SKETCHLIBS))

+SKETCHLIBSRCDIRS	:=	$(SKETCHLIBS) $(addsuffix /utility,$(SKETCHLIBS))

+SKETCHLIBSRCS		:=	$(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(SKETCHLIBSRCDIRS))))

+SKETCHLIBOBJS		:=	$(addsuffix .o,$(basename $(subst $(SKETCHBOOK),$(BUILDROOT),$(SKETCHLIBSRCS))))

+SKETCHLIBINCLUDES	:=	$(addprefix -I,$(SKETCHLIBS))

+

+#

+# Find Arduino libraries referenced by the sketch.  Exclude any that

+# are overloaded by versions in the sketchbook.

+#

+ARDUINOLIBS		:=	$(wildcard $(addprefix $(ARDUINO)/libraries/,$(filter-out $(SKETCHLIBNAMES),$(LIBTOKENS))))

+ARDUINOLIBNAMES		:=	$(notdir $(ARDUINOLIBS))

+ARDUINOLIBSRCDIRS	:=	$(ARDUINOLIBS) $(addsuffix /utility,$(ARDUINOLIBS))

+ARDUINOLIBSRCS		:=	$(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(ARDUINOLIBSRCDIRS))))

+ARDUINOLIBOBJS		:=	$(addsuffix .o,$(basename $(subst $(ARDUINO),$(BUILDROOT),$(ARDUINOLIBSRCS))))

+ARDUINOLIBINCLUDES	:=	$(addprefix -I,$(ARDUINOLIBS))

+

+# Library object files

+LIBOBJS			:=	$(SKETCHLIBOBJS) $(ARDUINOLIBOBJS)

+

+################################################################################

+# *duino core

+#

+

+# Pull the Arduino version

+ARDUINO_VERS	:=	$(shell $(SKETCHBOOK)/Tools/scripts/arduino_version.sh $(ARDUINO))

+

+# Find the hardware directory to use

+HARDWARE_DIR		:=	$(firstword $(wildcard $(SKETCHBOOK)/hardware/$(HARDWARE) \

+							$(ARDUINO)/hardware/$(HARDWARE)))

+ifeq ($(HARDWARE_DIR),)

+$(error ERROR: hardware directory for $(HARDWARE) not found)

+endif

+

+# Find the boards.txt that we are using

+BOARDFILE		:=	$(wildcard $(HARDWARE_DIR)/boards.txt)

+ifeq ($(BOARDFILE),)

+$(error ERROR: could not locate boards.txt for hardware $(HARDWARE))

+endif

+

+# Extract needed build parameters from the boardfile

+MCU			:=	$(shell grep $(BOARD).build.mcu $(BOARDFILE) | cut -d = -f 2)

+F_CPU			:=	$(shell grep $(BOARD).build.f_cpu $(BOARDFILE) | cut -d = -f 2)

+HARDWARE_CORE		:=	$(shell grep $(BOARD).build.core $(BOARDFILE) | cut -d = -f 2)

+UPLOAD_SPEED		:=	$(shell grep $(BOARD).upload.speed $(BOARDFILE) | cut -d = -f 2)

+

+ifeq ($(UPLOAD_PROTOCOL),)

+  UPLOAD_PROTOCOL	:=	$(shell grep $(BOARD).upload.protocol $(BOARDFILE) | cut -d = -f 2)

+endif

+

+# Adding override for mega since boards.txt uses stk500 instead of

+# arduino on 22 release

+ifeq ($(BOARD),mega)

+  UPLOAD_PROTOCOL	:=	arduino

+endif

+

+ifeq ($(MCU),)

+$(error ERROR: Could not locate board $(BOARD) in $(BOARDFILE))

+endif

+

+# Hardware source files

+CORESRC_DIR		=	$(HARDWARE_DIR)/cores/$(HARDWARE_CORE)

+CORESRC_PATTERNS	=	$(foreach suffix,/*.cpp /*.c /*.S,$(addsuffix $(suffix),$(CORESRC_DIR)))

+CORESRCS		:=	$(wildcard $(CORESRC_PATTERNS))

+

+# Include spec for core includes

+COREINCLUDES		=	-I$(CORESRC_DIR) -I$(HARDWARE_DIR)/variants/mega

+

+# Hardware object files

+CORELIBOBJS		:=	$(subst $(CORESRC_DIR),$(BUILDROOT)/$(HARDWARE),$(CORESRCS))

+CORELIBOBJS		:=	$(addsuffix .o,$(basename $(CORELIBOBJS)))

+

+################################################################################

+# Built products

+#

+

+# The ELF file

+SKETCHELF		=	$(BUILDROOT)/$(SKETCH).elf

+

+# HEX file

+SKETCHHEX		=	$(BUILDROOT)/$(SKETCH).hex

+

+# EEP file

+SKETCHEEP		=	$(BUILDROOT)/$(SKETCH).eep

+

+# Map file

+SKETCHMAP		=	$(BUILDROOT)/$(SKETCH).map

+

+# The core library

+CORELIB			=	$(BUILDROOT)/$(HARDWARE)/core.a

+

+# All of the objects that may be built

+ALLOBJS			=	$(SKETCHOBJS) $(LIBOBJS) $(CORELIBOBJS)

+

+# All of the dependency files that may be generated

+ALLDEPS			=	$(ALLOBJS:%.o=%.d)

+endif

+

+################################################################################

+# Targets

+#

+

+all:	$(SKETCHELF) $(SKETCHEEP) $(SKETCHHEX)

+

+.PHONY: upload

+upload: $(SKETCHHEX)

+	$(AVRDUDE) -c $(UPLOAD_PROTOCOL) -p $(MCU) -P $(PORT) -b$(UPLOAD_SPEED) -U flash:w:$(SKETCHHEX):i

+

+configure:

+	$(warning WARNING - A $(SKETCHBOOK)/config.mk file has been written)

+	$(warning Please edit the file to match your system configuration, if you use a different board or port)

+	@echo \# Select \'mega\' for the original APM, or \'mega2560\' for the V2 APM. > $(SKETCHBOOK)/config.mk

+	@echo BOARD=mega2560     >> $(SKETCHBOOK)/config.mk

+	@echo \# The communication port used to communicate with the APM. >> $(SKETCHBOOK)/config.mk

+ifneq ($(findstring CYGWIN, $(SYSTYPE)),)

+	@echo PORT=com3 >> $(SKETCHBOOK)/config.mk

+else

+	@echo PORT=/dev/ttyUSB0 >> $(SKETCHBOOK)/config.mk

+endif

+

+debug:

+	$(AVARICE) --mkII --capture --jtag usb :4242 & \

+	gnome-terminal -x $(GDB) $(SKETCHELF) & \

+	echo -e '\n\nat the gdb prompt type "target remote localhost:4242"'

+

+# this allows you to flash your image via JTAG for when you

+# have completely broken your USB

+jtag-program:

+	$(AVARICE) --mkII --jtag usb --erase --program --file $(SKETCHELF)

+

+clean:

+ifneq ($(findstring CYGWIN, $(SYSTYPE)),)

+	@del /S $(BUILDROOT)

+else

+	@rm -fr $(BUILDROOT)

+endif

+

+################################################################################

+# Rules

+#

+

+# fetch dependency info from a previous build if any of it exists

+-include $(ALLDEPS)

+

+# common header for rules, prints what is being built

+define RULEHDR

+	@echo %% $(subst $(BUILDROOT)/,,$@)

+	@mkdir -p $(dir $@)

+endef

+

+# Link the final object

+$(SKETCHELF):	$(SKETCHOBJS) $(LIBOBJS) $(CORELIB)

+	$(RULEHDR)

+	$(v)$(LD) $(LDFLAGS) -o $@ $^ $(LIBS)

+

+# Create the hex file

+$(SKETCHHEX):	$(SKETCHELF)

+	$(RULEHDR)

+	$(v)$(OBJCOPY) -O ihex -R .eeprom $< $@

+

+# Create the eep file

+$(SKETCHEEP):	$(SKETCHELF)

+	$(RULEHDR)

+	$(v)$(OBJCOPY) -O ihex -j.eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0 $< $@

+

+#

+# Build sketch objects

+#

+SKETCH_INCLUDES	=	$(SKETCHLIBINCLUDES) $(ARDUINOLIBINCLUDES) $(COREINCLUDES)

+

+$(BUILDROOT)/%.o: $(BUILDROOT)/%.cpp

+	$(RULEHDR)

+	$(v)$(CXX) $(CXXFLAGS) -c -o $@ $< -I$(SRCROOT) $(SKETCH_INCLUDES)

+

+$(BUILDROOT)/%.o: $(SRCROOT)/%.cpp

+	$(RULEHDR)

+	$(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(SKETCH_INCLUDES)

+

+$(BUILDROOT)/%.o: $(SRCROOT)/%.c

+	$(RULEHDR)

+	$(v)$(CC) $(CFLAGS) -c -o $@ $< $(SKETCH_INCLUDES)

+

+$(BUILDROOT)/%.o: $(SRCROOT)/%.S

+	$(RULEHDR)

+	$(v)$(AS) $(ASFLAGS) -c -o $@ $< $(SKETCH_INCLUDES)

+

+#

+# Build library objects from sources in the sketchbook

+#

+SLIB_INCLUDES	=	-I$(dir $<)/utility $(SKETCHLIBINCLUDES) $(ARDUINOLIBINCLUDES) $(COREINCLUDES)

+

+$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.cpp

+	$(RULEHDR)

+	$(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(SLIB_INCLUDES)

+

+$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.c

+	$(RULEHDR)

+	$(v)$(CC) $(CFLAGS) -c -o $@ $< $(SLIB_INCLUDES)

+

+$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.S

+	$(RULEHDR)

+	$(v)$(AS) $(ASFLAGS) -c -o $@ $< $(SLIB_INCLUDES)

+

+#

+# Build library objects from Ardiuno library sources

+#

+ALIB_INCLUDES	=	-I$(dir $<)/utility $(ARDUINOLIBINCLUDES) $(COREINCLUDES)

+

+$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.cpp

+	$(RULEHDR)

+	$(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(ALIB_INCLUDES)

+

+$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.c

+	$(RULEHDR)

+	$(v)$(CC) $(CFLAGS) -c -o $@ $< $(ALIB_INCLUDES)

+

+$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.S

+	$(RULEHDR)

+	$(v)$(AS) $(ASFLAGS) -c -o $@ $< $(ALIB_INCLUDES)

+

+#

+# Build objects from the hardware core

+#

+$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.cpp

+	$(RULEHDR)

+	$(v)$(CXX) $(filter-out -W%,$(CXXFLAGS)) -c -o $@ $< $(COREINCLUDES)

+

+$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.c

+	@mkdir -p $(dir $@)

+	$(v)$(CC) $(filter-out -W%,$(CFLAGS)) -c -o $@ $< $(COREINCLUDES)

+

+$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.S

+	$(RULEHDR)

+	$(v)$(AS) $(ASFLAGS) -c -o $@ $< $(COREINCLUDES)

+

+#

+# Build the core library

+#

+$(CORELIB): $(CORELIBOBJS)

+	$(RULEHDR)

+	$(v)$(AR) -rcs $@ $^

+

+#

+# Build the sketch.cpp file

+#

+# This process strives to be as faithful to the Arduino implementation as

+# possible.  Conceptually, the process is as follows:

+#

+# * All of the .pde/.ino files are concatenated, starting with the file named 

+#   for the sketch and followed by the others in alphabetical order.

+# * An insertion point is created in the concatenated file at

+#   the first statement that isn't a preprocessor directive or comment.

+# * An include of "WProgram.h" is inserted at the insertion point.

+# * The file following the insertion point is scanned for function definitions

+#   and prototypes for these functions are inserted at the insertion point.

+#

+# In addition, we add #line directives wherever the originating file changes

+# to help backtrack from compiler messages and in the debugger.

+#

+$(SKETCHCPP):	$(SKETCHCPP_SRC)

+	$(RULEHDR)

+	$(v)$(AWK) -v mode=header '$(SKETCH_SPLITTER)'   $(SKETCHCPP_SRC) >  $@

+	$(v)echo "#line 1 \"autogenerated\""                              >> $@

+	$(v)echo "#if defined(ARDUINO) && ARDUINO >= 100"                 >> $@

+	$(v)echo "#include \"Arduino.h\""                                 >> $@

+	$(v)echo "#else"                                                  >> $@

+	$(v)echo "#include \"WProgram.h\""                                >> $@

+	$(v)echo "#endif"                                                 >> $@

+	$(v)$(AWK)                '$(SKETCH_PROTOTYPER)' $(SKETCHCPP_SRC) >> $@

+	$(v)$(AWK) -v mode=body   '$(SKETCH_SPLITTER)'   $(SKETCHCPP_SRC) >> $@

+

+# delete the sketch.cpp file if a processing error occurs

+.DELETE_ON_ERROR: $(SKETCHCPP)

+

+#

+# The sketch splitter is an awk script used to split off the

+# header and body of the concatenated .pde/.ino files.  It also

+# inserts #line directives to help in backtracking from compiler

+# and debugger messages to the original source file.

+#

+# Note that # and $ require special treatment here to avoid upsetting

+# make.

+#

+# This script requires BWK or GNU awk.

+#

+define SKETCH_SPLITTER

+  BEGIN { 							\

+    scanning = 1; 						\

+    printing = (mode ~ "header") ? 1 : 0;			\

+  }								\

+  { toggles = 1 }						\

+  (FNR == 1) && printing { 					\

+    printf "#line %d \"%s\"\n", FNR, FILENAME;			\

+  }								\

+  /^[[:space:]]*\/\*/,/\*\// { 					\

+    toggles = 0;						\

+  }								\

+  /^[[:space:]]*$$/ || /^[[:space:]]*\/\/.*/ || /^\#.*$$/ { 	\

+    toggles = 0;						\

+  }								\

+  scanning && toggles { 					\

+    scanning = 0; 						\

+    printing = !printing;					\

+    if (printing) { 						\

+      printf "#line %d \"%s\"\n", FNR, FILENAME;		\

+    }								\

+  }								\

+  printing

+endef

+

+#

+# The prototype scanner is an awk script used to generate function

+# prototypes from the concantenated .pde/.ino files.

+#

+# Function definitions are expected to follow the form

+#

+#   <newline><type>[<qualifier>...]<name>([<arguments>]){

+#

+# with whitespace permitted between the various elements.  The pattern

+# is assembled from separate subpatterns to make adjustments easier.

+#

+# Note that $ requires special treatment here to avoid upsetting make,

+# and backslashes are doubled in the partial patterns to satisfy

+# escaping rules.

+#

+# This script requires BWK or GNU awk.

+#

+define SKETCH_PROTOTYPER

+  BEGIN {								\

+    RS="{";								\

+    type       = "((\\n)|(^))[[:space:]]*[[:alnum:]_]+[[:space:]]+";	\

+    qualifiers = "([[:alnum:]_\\*&]+[[:space:]]*)*";			\

+    name       = "[[:alnum:]_]+[[:space:]]*";				\

+    args       = "\\([[:space:][:alnum:]_,&\\*\\[\\]]*\\)";		\

+    bodycuddle = "[[:space:]]*$$";					\

+    pattern    = type qualifiers name args bodycuddle;			\

+  }									\

+  match($$0, pattern) {							\

+    proto = substr($$0, RSTART, RLENGTH);				\

+    gsub("\n", " ", proto);						\

+    printf "%s;\n", proto;						\

+  }

+endef

 

--- /dev/null
+++ b/libraries/AP_Common/c++.cpp
@@ -1,1 +1,92 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+//

+// C++ runtime support not provided by Arduino

+//

+// Note: use new/delete with caution.  The heap is small and

+// easily fragmented.

+//

+

+#include <stdlib.h>

+#include "c++.h"

+#if defined(ARDUINO) && ARDUINO >= 100

+	#include "Arduino.h"

+#else

+	#include "WProgram.h"

+#endif

+

+void * operator new(size_t size)

+{

+#ifdef AP_DISPLAYMEM

+    displayMemory();

+#endif

+    return(calloc(size, 1));

+}

+

+void operator delete(void *p)

+{

+    if (p) free(p);

+}

+

+extern "C" void __cxa_pure_virtual()

+{

+    while (1)

+    {

+        Serial.println("Error: pure virtual call");

+        delay(1000);

+    }

+}

+

+void * operator new[](size_t size)

+{

+#ifdef AP_DISPLAYMEM

+    displayMemory();

+#endif

+    return(calloc(size, 1));

+}

+

+void operator delete[](void * ptr)

+{

+    if (ptr) free(ptr);

+}

+

+__extension__ typedef int __guard __attribute__((mode (__DI__)));

+

+int __cxa_guard_acquire(__guard *g)

+{

+    return !*(char *)(g);

+};

+

+void __cxa_guard_release (__guard *g)

+{

+    *(char *)g = 1;

+};

+

+void __cxa_guard_abort (__guard *) {};

+

+// free memory

+extern unsigned int __bss_end;

+extern void *__brkval;

+

+void displayMemory()

+{

+    static int minMemFree=0;

+    if (minMemFree<=0 || freeMemory()<minMemFree) {

+        minMemFree = freeMemory();

+        Serial.print("bytes free: ");

+        Serial.println(minMemFree);

+    }

+}

+

+int freeMemory()

+{

+    int free_memory;

+

+    if ((intptr_t)__brkval == 0)

+        free_memory = ((intptr_t)&free_memory) - ((intptr_t)&__bss_end);

+    else

+        free_memory = ((intptr_t)&free_memory) - ((intptr_t)__brkval);

+

+    return free_memory;

+}

 

--- /dev/null
+++ b/libraries/AP_Common/c++.h
@@ -1,1 +1,8 @@
+#ifndef CPP_H

+#define CPP_H

+

+void displayMemory();

+int freeMemory();

+

+#endif

 

--- /dev/null
+++ b/libraries/AP_Common/examples/menu/menu.pde
@@ -1,1 +1,44 @@
+

+#include <FastSerial.h>

+#include <AP_Common.h>

+

+FastSerialPort0(Serial);

+

+int8_t

+menu_test(uint8_t argc, const Menu::arg *argv)

+{

+    int	i;

+

+    Serial.printf("This is a test with %d arguments\n", argc);

+    for (i = 1; i < argc; i++) {

+        Serial.printf("%d: int %ld  float ", i, argv[i].i);

+        Serial.println(argv[i].f, 6);    // gross

+    }

+}

+

+int8_t

+menu_auto(uint8_t argc, const Menu::arg *argv)

+{

+    Serial.println("auto text");

+}

+

+const struct Menu::command top_menu_commands[] PROGMEM = {

+    {"*",               menu_auto},

+    {"test",			menu_test},

+};

+

+MENU(top, "menu", top_menu_commands);

+

+void

+setup(void)

+{

+    Serial.begin(38400);

+    top.run();

+}

+

+void

+loop(void)

+{

+}

+

 

--- /dev/null
+++ b/libraries/AP_Common/include/menu.h
@@ -1,1 +1,144 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+/// @file	menu.h

+/// @brief	Simple commandline menu subsystem.

+/// @discussion

+/// The Menu class implements a simple CLI that accepts commands typed by

+/// the user, and passes the arguments to those commands to a function

+/// defined as handing the command.

+///

+/// Commands are defined in an array of Menu::command structures passed

+/// to the constructor.  Each entry in the array defines one command.

+///

+/// Arguments passed to the handler function are pre-converted to both

+/// long and float for convenience.

+

+#ifndef __AP_COMMON_MENU_H

+#define __AP_COMMON_MENU_H

+

+#include <inttypes.h>

+

+#define MENU_COMMANDLINE_MAX	32	///< maximum input line length

+#define MENU_ARGS_MAX			4	///< maximum number of arguments

+#define MENU_COMMAND_MAX		14	///< maximum size of a command name

+

+/// Class defining and handling one menu tree

+class Menu {

+public:

+    /// argument passed to a menu function

+    ///

+    /// Space-delimited arguments are parsed from the commandline and

+    /// separated into these structures.

+    ///

+    /// If the argument cannot be parsed as a float or a long, the value

+    /// of f or i respectively is undefined.  You should range-check

+    /// the inputs to your function.

+    ///

+    struct arg {

+        const char	*str;			///< string form of the argument

+        long		i;				///< integer form of the argument (if a number)

+        float		f;				///< floating point form of the argument (if a number)

+    };

+

+    /// menu command function

+    ///

+    /// Functions called by menu array entries are expected to be of this

+    /// type.

+    ///

+    /// @param	argc		The number of valid arguments, including the

+    ///						name of the command in argv[0].  Will never be

+    ///						more than MENU_ARGS_MAX.

+    /// @param	argv		Pointer to an array of Menu::arg structures

+    ///						detailing any optional arguments given to the

+    ///						command.  argv[0] is always the name of the

+    ///						command, so that the same function can be used

+    ///						to handle more than one command.

+    ///

+    typedef int8_t			(*func)(uint8_t argc, const struct arg *argv);

+

+    /// menu pre-prompt function

+    ///

+    /// Called immediately before waiting for the user to type a command; can be

+    /// used to display help text or status, for example.

+    ///

+    /// If this function returns false, the menu exits.

+    ///

+    typedef bool			(*preprompt)(void);

+

+    /// menu command description

+    ///

+    struct command {

+        /// Name of the command, as typed or received.

+        /// Command names are limited in size to keep this structure compact.

+        ///

+        const char	command[MENU_COMMAND_MAX];

+

+        /// The function to call when the command is received.

+        ///

+        /// The argc argument will be at least 1, and no more than

+        /// MENU_ARGS_MAX.  The argv array will be populated with

+        /// arguments typed/received up to MENU_ARGS_MAX.  The command

+        /// name will always be in argv[0].

+        ///

+        /// Commands may return -2 to cause the menu itself to exit.

+        /// The "?", "help" and "exit" commands are always defined, but

+        /// can be overridden by explicit entries in the command array.

+        ///

+        int8_t			(*func)(uint8_t argc, const struct arg *argv);	///< callback function

+    };

+

+    /// constructor

+    ///

+    /// Note that you should normally not call the constructor directly.  Use

+    /// the MENU and MENU2 macros defined below.

+    ///

+    /// @param prompt		The prompt to be displayed with this menu.

+    /// @param commands		An array of ::command structures in program memory (PROGMEM).

+    /// @param entries		The number of entries in the menu.

+    ///

+    Menu(const char *prompt, const struct command *commands, uint8_t entries, preprompt ppfunc = 0);

+

+    /// menu runner

+    void				run(void);

+

+private:

+    /// Implements the default 'help' command.

+    ///

+    void				_help(void);					///< implements the 'help' command

+

+    /// calls the function for the n'th menu item

+    ///

+    /// @param n			Index for the menu item to call

+    /// @param argc			Number of arguments prepared for the menu item

+    ///

+    int8_t				_call(uint8_t n, uint8_t argc);

+

+    const char			*_prompt;						///< prompt to display

+    const command		*_commands;						///< array of commands

+    const uint8_t		_entries;						///< size of the menu

+    const preprompt		_ppfunc;						///< optional pre-prompt action

+

+    static char			_inbuf[MENU_COMMANDLINE_MAX];	///< input buffer

+    static arg			_argv[MENU_ARGS_MAX + 1];		///< arguments

+};

+

+/// Macros used to define a menu.

+///

+/// The commands argument should be an arary of Menu::command structures, one

+/// per command name.  The array does not need to be terminated with any special

+/// record.

+///

+/// Use name.run() to run the menu.

+///

+/// The MENU2 macro supports the optional pre-prompt printing function.

+///

+#define MENU(name, prompt, commands)							\

+	static const char __menu_name__ ##name[] PROGMEM = prompt;	\

+	static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0]))

+

+#define MENU2(name, prompt, commands, preprompt)				\

+	static const char __menu_name__ ##name[] PROGMEM = prompt;	\

+	static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0]), preprompt)

+

+#endif

 

--- /dev/null
+++ b/libraries/AP_Common/keywords.txt
@@ -1,1 +1,5 @@
+Menu             KEYWORD1

+run		 KEYWORD2

+Location	 KEYWORD2

+

 

--- /dev/null
+++ b/libraries/AP_Common/menu.cpp
@@ -1,1 +1,156 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+//

+// Simple commandline menu system.

+//

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+

+#include <ctype.h>

+#include <string.h>

+#include <avr/pgmspace.h>

+

+#include "include/menu.h"

+

+// statics

+char Menu::_inbuf[MENU_COMMANDLINE_MAX];

+Menu::arg Menu::_argv[MENU_ARGS_MAX + 1];

+

+// constructor

+Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entries, preprompt ppfunc) :

+    _prompt(prompt),

+    _commands(commands),

+    _entries(entries),

+    _ppfunc(ppfunc)

+{

+}

+

+// run the menu

+void

+Menu::run(void)

+{

+    int8_t		ret;

+    uint8_t		len, i;

+    uint8_t		argc;

+    int			c;

+    char		*s;

+

+    // loop performing commands

+    for (;;) {

+

+        // run the pre-prompt function, if one is defined

+        if ((NULL != _ppfunc) && !_ppfunc())

+            return;

+

+        // loop reading characters from the input

+        len = 0;

+        Serial.printf("%S] ", FPSTR(_prompt));

+        for (;;) {

+            c = Serial.read();

+            if (-1 == c)

+                continue;

+            // carriage return -> process command

+            if ('\r' == c) {

+                _inbuf[len] = '\0';

+                Serial.write('\r');

+                Serial.write('\n');

+                break;

+            }

+            // backspace

+            if ('\b' == c) {

+                if (len > 0) {

+                    len--;

+                    Serial.write('\b');

+                    Serial.write(' ');

+                    Serial.write('\b');

+                    continue;

+                }

+            }

+            // printable character

+            if (isprint(c) && (len < (MENU_COMMANDLINE_MAX - 1))) {

+                _inbuf[len++] = c;

+                Serial.write((char)c);

+                continue;

+            }

+        }

+

+        // split the input line into tokens

+        argc = 0;

+        _argv[argc++].str = strtok_r(_inbuf, " ", &s);

+        // XXX should an empty line by itself back out of the current menu?

+        while (argc <= MENU_ARGS_MAX) {

+            _argv[argc].str = strtok_r(NULL, " ", &s);

+            if ('\0' == _argv[argc].str)

+                break;

+            _argv[argc].i = atol(_argv[argc].str);

+            _argv[argc].f = atof(_argv[argc].str);	// calls strtod, > 700B !

+            argc++;

+        }

+

+		if (_argv[0].str == NULL) {

+			continue;

+		}

+

+        // populate arguments that have not been specified with "" and 0

+        // this is safer than NULL in the case where commands may look

+        // without testing argc

+        i = argc;

+        while (i <= MENU_ARGS_MAX) {

+            _argv[i].str = "";

+            _argv[i].i = 0;

+            _argv[i].f = 0;

+            i++;

+        }

+

+		bool cmd_found = false;

+        // look for a command matching the first word (note that it may be empty)

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

+            if (!strcasecmp_P(_argv[0].str, _commands[i].command)) {

+                ret = _call(i, argc);

+                cmd_found=true;

+                if (-2 == ret)

+                    return;

+                break;

+            }

+        }

+

+        // implicit commands

+        if (i == _entries) {

+            if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) {

+                _help();

+                cmd_found=true;

+            } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) {

+                return;

+            }

+        }

+

+        if (cmd_found==false)

+        {

+    		Serial.println("Invalid command, type 'help'");

+    	}

+

+    }

+}

+

+// display the list of commands in response to the 'help' command

+void

+Menu::_help(void)

+{

+    int		i;

+

+    Serial.println("Commands:");

+    for (i = 0; i < _entries; i++)

+        Serial.printf("  %S\n", FPSTR(_commands[i].command));

+}

+

+// run the n'th command in the menu

+int8_t

+Menu::_call(uint8_t n, uint8_t argc)

+{

+    func		fn;

+

+    fn = (func)pgm_read_pointer(&_commands[n].func);

+    return(fn(argc, &_argv[0]));

+}

 

--- /dev/null
+++ b/libraries/AP_Common/tools/eedump.c
@@ -1,1 +1,85 @@
+/*

+ * Simple tool to dump the AP_Var contents from an EEPROM dump

+ */

+#include <stdio.h>

+#include <stdlib.h>

+#include <stdint.h>

+

+uint8_t	eeprom[0x1000];

+

+#pragma pack(1)

+

+struct EEPROM_header {

+  uint16_t    magic;

+  uint8_t     revision;

+  uint8_t     spare;

+};

+

+static const uint16_t   k_EEPROM_magic      = 0x5041;

+static const uint16_t   k_EEPROM_revision   = 2;

+

+struct Var_header {

+  uint8_t    size:6;

+  uint8_t    spare:2;

+  uint8_t    key;

+};

+

+static const uint8_t k_key_sentinel = 0xff;

+

+void

+fail(const char *why)

+{

+  fprintf(stderr, "ERROR: %s\n", why);

+  exit(1);

+}

+

+int

+main(int argc, char *argv[])

+{

+  FILE			*fp;

+  struct EEPROM_header	*header;

+  struct Var_header	*var;

+  unsigned		index;

+  unsigned		i;

+

+  if (argc != 2) {

+    fail("missing EEPROM file name");

+  }

+  if (NULL == (fp = fopen(argv[1], "rb"))) {

+    fail("can't open EEPROM file");

+  }

+  if (1 != fread(eeprom, sizeof(eeprom), 1, fp)) {

+    fail("can't read EEPROM file");

+  }

+  fclose(fp);

+

+  header = (struct EEPROM_header *)&eeprom[0];

+  if (header->magic != k_EEPROM_magic) {

+    fail("bad magic in EEPROM file");

+  }

+  if (header->revision != 2) {

+    fail("unsupported EEPROM format revision");

+  }

+  printf("Header OK\n");

+

+  index = sizeof(*header);

+  for (;;) {

+    var = (struct Var_header *)&eeprom[index];

+    if (var->key == k_key_sentinel) {

+      printf("end sentinel at %u\n", index);

+      break;

+    }

+    printf("%04x: key %u size %d\n ", index, var->key, var->size + 1);

+    index += sizeof(*var);

+    for (i = 0; i <= var->size; i++) {

+      printf(" %02x", eeprom[index + i]);

+    }

+    printf("\n");

+    index += var->size + 1;

+    if (index >= sizeof(eeprom)) {

+      fflush(stdout);

+      fail("missing end sentinel");

+    }

+  }

+}

 

--- /dev/null
+++ b/libraries/AP_Common/tools/eedump.pl
@@ -1,1 +1,47 @@
-
+#!/usr/bin/perl

+

+

+$file = $ARGV[0];

+

+

+open(IN,$file) || die print "Failed to open file: $file : $!";

+

+read(IN,$buffer,1);

+read(IN,$buffer2,1);

+if (ord($buffer) != 0x41 && ord($buffer2) != 0x50) {

+	print "bad header ". $buffer ." ".$buffer2. "\n";

+	exit;

+}

+read(IN,$buffer,1);

+if (ord($buffer) != 2) {

+	print "bad version";

+	exit;

+}

+

+# spare

+read(IN,$buffer,1);

+

+$a = 0;

+

+while (read(IN,$buffer,1)) {	

+	$pos = (tell(IN) - 1);

+

+	$size = ((ord($buffer) & 63));

+

+	read(IN,$buffer,1);

+

+	if (ord($buffer) == 0xff) {

+		printf("end sentinel at %u\n", $pos);

+		last;

+	}

+

+	printf("%04x: key %u size %d\n ", $pos, ord($buffer), $size + 1);

+

+	for ($i = 0; $i <= ($size); $i++) {

+		read(IN,$buffer,1);

+		printf(" %02x", ord($buffer));

+	}

+	print "\n";

+}

+

+close IN;

--- /dev/null
+++ b/libraries/AP_Common/tools/eedump_apparam.c
@@ -1,1 +1,182 @@
+/*

+ * Simple tool to dump the AP_Param contents from an EEPROM dump

+ * Andrew Tridgell February 2012

+ */

+#include <stdio.h>

+#include <stdlib.h>

+#include <stdint.h>

+

+uint8_t	eeprom[0x1000];

+

+#pragma pack(1)

+

+struct EEPROM_header {

+  uint8_t     magic[2];

+  uint8_t     revision;

+  uint8_t     spare;

+};

+

+static const uint16_t   k_EEPROM_magic0     = 0x50;

+static const uint16_t   k_EEPROM_magic1     = 0x41;

+static const uint16_t   k_EEPROM_revision   = 5;

+

+enum ap_var_type {

+    AP_PARAM_NONE    = 0,

+    AP_PARAM_INT8,

+    AP_PARAM_INT16,

+    AP_PARAM_INT32,

+    AP_PARAM_FLOAT,

+    AP_PARAM_VECTOR3F,

+    AP_PARAM_VECTOR6F,

+    AP_PARAM_MATRIX3F,

+    AP_PARAM_GROUP

+};

+

+static const char *type_names[8] = {

+	"NONE", "INT8", "INT16", "INT32", "FLOAT", "VECTOR3F", "MATRIX6F", "GROUP"

+};

+

+struct Param_header {

+        uint8_t    key;

+        uint8_t    group_element;

+        uint8_t    type;

+};

+

+

+static const uint8_t  _sentinal_key   = 0xFF;

+static const uint8_t  _sentinal_type  = 0xFF;

+static const uint8_t  _sentinal_group = 0xFF;

+

+static uint8_t type_size(enum ap_var_type type)

+{

+    switch (type) {

+    case AP_PARAM_NONE:

+    case AP_PARAM_GROUP:

+        return 0;

+    case AP_PARAM_INT8:

+        return 1;

+    case AP_PARAM_INT16:

+        return 2;

+    case AP_PARAM_INT32:

+        return 4;

+    case AP_PARAM_FLOAT:

+        return 4;

+    case AP_PARAM_VECTOR3F:

+        return 3*4;

+    case AP_PARAM_VECTOR6F:

+        return 6*4;

+    case AP_PARAM_MATRIX3F:

+        return 3*3*4;

+    }

+    printf("unknown type %u\n", type);

+    return 0;

+}

+

+static void

+fail(const char *why)

+{

+  fprintf(stderr, "ERROR: %s\n", why);

+  exit(1);

+}

+

+int

+main(int argc, char *argv[])

+{

+	FILE			*fp;

+	struct EEPROM_header	*header;

+	struct Param_header	*var;

+	unsigned		index;

+	unsigned		i;

+

+	if (argc != 2) {

+		fail("missing EEPROM file name");

+	}

+	if (NULL == (fp = fopen(argv[1], "rb"))) {

+		fail("can't open EEPROM file");

+	}

+	if (1 != fread(eeprom, sizeof(eeprom), 1, fp)) {

+		fail("can't read EEPROM file");

+	}

+	fclose(fp);

+

+	header = (struct EEPROM_header *)&eeprom[0];

+	if (header->magic[0] != k_EEPROM_magic0 ||

+	    header->magic[1] != k_EEPROM_magic1) {

+		fail("bad magic in EEPROM file");

+	}

+	if (header->revision != k_EEPROM_revision) {

+		fail("unsupported EEPROM format revision");

+	}

+	printf("Header OK\n");

+

+	index = sizeof(*header);

+	for (;;) {

+		uint8_t size;

+		var = (struct Param_header *)&eeprom[index];

+		if (var->key == _sentinal_key ||

+		    var->group_element == _sentinal_group ||

+		    var->type == _sentinal_type) {

+			printf("end sentinel at %u\n", index);

+			break;

+		}

+		size = type_size(var->type);

+		printf("%04x: type %u (%s) key %u group_element %u size %d value ",

+		       index, var->type, type_names[var->type], var->key, var->group_element, size);

+		index += sizeof(*var);

+		switch (var->type) {

+		case AP_PARAM_INT8:

+			printf("%d\n", (int)*(int8_t *)&eeprom[index]);

+			break;

+		case AP_PARAM_INT16:

+			printf("%d\n", (int)*(int16_t *)&eeprom[index]);

+			break;

+		case AP_PARAM_INT32:

+			printf("%d\n", (int)*(int32_t *)&eeprom[index]);

+			break;

+		case AP_PARAM_FLOAT:

+			printf("%f\n", *(float *)&eeprom[index]);

+			break;

+		case AP_PARAM_VECTOR3F:

+			printf("%f %f %f\n",

+			       *(float *)&eeprom[index],

+			       *(float *)&eeprom[index+4],

+			       *(float *)&eeprom[index+8]);

+			break;

+		case AP_PARAM_VECTOR6F:

+			printf("%f %f %f %f %f %f\n",

+			       *(float *)&eeprom[index],

+			       *(float *)&eeprom[index+4],

+			       *(float *)&eeprom[index+8],

+			       *(float *)&eeprom[index+12],

+			       *(float *)&eeprom[index+16],

+			       *(float *)&eeprom[index+20]);

+			break;

+		case AP_PARAM_MATRIX3F:

+			printf("%f %f %f %f %f %f %f %f %f\n",

+			       *(float *)&eeprom[index],

+			       *(float *)&eeprom[index+4],

+			       *(float *)&eeprom[index+8],

+			       *(float *)&eeprom[index+12],

+			       *(float *)&eeprom[index+16],

+			       *(float *)&eeprom[index+20],

+			       *(float *)&eeprom[index+24],

+			       *(float *)&eeprom[index+28],

+			       *(float *)&eeprom[index+32]);

+			break;

+		default:

+			printf("NONE\n");

+			break;

+		}

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

+			printf(" %02x", eeprom[index + i]);

+		}

+		printf("\n");

+		index += size;

+		if (index >= sizeof(eeprom)) {

+			fflush(stdout);

+			fail("missing end sentinel");

+		}

+	}

+	return 0;

+}

 

--- /dev/null
+++ b/libraries/AP_Common/tools/eedump_apparam.pl
@@ -1,1 +1,79 @@
+#!/usr/bin/perl

+

+

+$file = $ARGV[0];

+

+

+open(IN,$file) || die print "Failed to open file: $file : $!";

+

+read(IN,$buffer,1);

+read(IN,$buffer2,1);

+if (ord($buffer2) != 0x41 && ord($buffer) != 0x50) {

+	print "bad header ". $buffer ." ".$buffer2. "\n";

+	exit;

+}

+read(IN,$buffer,1);

+if (ord($buffer) != 5) {

+	print "bad version";

+	exit;

+}

+

+# spare

+read(IN,$buffer,1);

+

+$a = 0;

+

+while (read(IN,$buffer,1)) {	

+	$pos = (tell(IN) - 1);

+

+	if (ord($buffer) == 0xff) {

+		printf("end sentinel at %u\n", $pos);

+		last;

+	}

+    

+    read(IN,$buffer2,1);

+    read(IN,$buffer3,1);

+    

+    if (ord($buffer3) == 0) { #none

+        $size = 0;

+        $type = "NONE";

+    } elsif (ord($buffer3) == 1) { #int8

+        $size = 1;

+        $type = "INT8";

+    } elsif (ord($buffer3) == 2) { #int16

+        $size = 2;

+        $type = "INT16";

+    } elsif (ord($buffer3) == 3) { #int32

+        $size = 4;

+        $type = "INT32";

+    } elsif (ord($buffer3) == 4) { #float

+        $size = 4;

+        $type = "FLOAT";

+    } elsif (ord($buffer3) == 5) { #vector 3

+        $size = 3*4;

+        $type = "VECTOR3F";

+    } elsif (ord($buffer3) == 6) { #vector6

+        $size = 6*4;

+        $type = "VECTOR6F";

+    } elsif (ord($buffer3) == 7) { #matrix

+        $size = 3*3*4;

+        $type = "MATRIX6F";

+    } elsif (ord($buffer3) == 8) { #group

+        $size = 0;

+        $type = "GROUP";

+    } else {

+        print "Unknown type\n";

+        $size = 0;

+    }

+

+	printf("%04x: type %u ($type) key %u group_element %u size %d\n ", $pos, ord($buffer3),ord($buffer),ord($buffer2), $size);

+

+	for ($i = 0; $i < ($size); $i++) {

+		read(IN,$buffer,1);

+		printf(" %02x", ord($buffer));

+	}

+	print "\n";

+}

+

+close IN;

 

--- /dev/null
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -1,1 +1,70 @@
+#include "AP_Math.h"

+

+// a varient of asin() that checks the input ranges and ensures a

+// valid angle as output. If nan is given as input then zero is

+// returned.

+float safe_asin(float v)

+{

+	if (isnan(v)) {

+		return 0.0;

+	}

+	if (v >= 1.0) {

+		return PI/2;

+	}

+	if (v <= -1.0) {

+		return -PI/2;

+	}

+	return asin(v);

+}

+

+// a varient of sqrt() that checks the input ranges and ensures a

+// valid value as output. If a negative number is given then 0 is

+// returned. The reasoning is that a negative number for sqrt() in our

+// code is usually caused by small numerical rounding errors, so the

+// real input should have been zero

+float safe_sqrt(float v)

+{

+	float ret = sqrt(v);

+	if (isnan(ret)) {

+		return 0;

+	}

+	return ret;

+}

+

+

+// find a rotation that is the combination of two other

+// rotations. This is used to allow us to add an overall board

+// rotation to an existing rotation of a sensor such as the compass

+// Note that this relies the set of rotations being complete. The

+// optional 'found' parameter is for the test suite to ensure that it is.

+enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found)

+{

+	Vector3f tv1, tv2;

+	enum Rotation r;

+	tv1(1,2,3);

+	tv1.rotate(r1);

+	tv1.rotate(r2);

+

+	for (r=ROTATION_NONE; r<ROTATION_MAX;

+	     r = (enum Rotation)((uint8_t)r+1)) {

+		Vector3f diff;

+		tv2(1,2,3);

+		tv2.rotate(r);

+		diff = tv1 - tv2;

+		if (diff.length() < 1.0e-6) {

+			// we found a match

+			if (found) {

+				*found = true;

+			}

+			return r;

+		}

+	}

+

+	// we found no matching rotation. Someone has edited the

+	// rotations list and broken its completeness property ...

+	if (found) {

+		*found = false;

+	}

+	return ROTATION_NONE;

+}

 

--- /dev/null
+++ b/libraries/AP_Math/AP_Math.h
@@ -1,1 +1,33 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+#ifndef AP_MATH_H

+#define AP_MATH_H

+

+// Assorted useful math operations for ArduPilot(Mega)

+

+#include <AP_Common.h>

+#include <stdint.h>

+#include "rotations.h"

+#include "vector2.h"

+#include "vector3.h"

+#include "matrix3.h"

+#include "quaternion.h"

+#include "polygon.h"

+

+// define AP_Param types AP_Vector3f and Ap_Matrix3f

+AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);

+AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);

+

+// a varient of asin() that always gives a valid answer.

+float safe_asin(float v);

+

+// a varient of sqrt() that always gives a valid answer.

+float safe_sqrt(float v);

+

+// find a rotation that is the combination of two other

+// rotations. This is used to allow us to add an overall board

+// rotation to an existing rotation of a sensor such as the compass

+enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);

+

+#endif

 

--- /dev/null
+++ b/libraries/AP_Math/examples/eulers/Makefile
@@ -1,1 +1,5 @@
+include ../../../AP_Common/Arduino.mk

+

+sitl:

+	make -f ../../../../libraries/Desktop/Desktop.mk

 

--- /dev/null
+++ b/libraries/AP_Math/examples/eulers/eulers.pde
@@ -1,1 +1,282 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// Unit tests for the AP_Math euler code

+//

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+#include <AP_Math.h>

+

+FastSerialPort(Serial, 0);

+

+#ifdef DESKTOP_BUILD

+// all of this is needed to build with SITL

+#include <DataFlash.h>

+#include <APM_RC.h>

+#include <GCS_MAVLink.h>

+#include <Arduino_Mega_ISR_Registry.h>

+#include <AP_PeriodicProcess.h>

+#include <AP_ADC.h>

+#include <SPI.h>

+#include <I2C.h>

+#include <AP_Baro.h>

+#include <AP_Compass.h>

+#include <AP_GPS.h>

+#include <Filter.h>

+Arduino_Mega_ISR_Registry isr_registry;

+AP_Baro_BMP085_HIL      barometer;

+AP_Compass_HIL     compass;

+#endif

+

+#include <AP_Declination.h>

+

+

+static float rad_diff(float rad1, float rad2)

+{

+    float diff = rad1 - rad2;

+    if (diff > PI) {

+        diff -= 2*PI;

+    }

+    if (diff < -PI) {

+        diff += 2*PI;

+    }

+    return fabs(diff);

+}

+

+static void check_result(float roll, float pitch, float yaw,

+                         float roll2, float pitch2, float yaw2)

+{

+    if (isnan(roll2) ||

+        isnan(pitch2) ||

+        isnan(yaw2)) {

+        Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n",

+                      roll, pitch, yaw);

+    }

+

+    if (rad_diff(roll2,roll) > ToRad(179)) {

+        // reverse all 3

+        roll2 += fmod(roll2+PI, 2*PI);

+        pitch2 += fmod(pitch2+PI, 2*PI);

+        yaw2 += fmod(yaw2+PI, 2*PI);

+    }

+

+    if (rad_diff(roll2,roll) > 0.01 ||

+        rad_diff(pitch2, pitch) > 0.01 ||

+        rad_diff(yaw2, yaw) > 0.01) {

+        if (pitch >= PI/2 ||

+            pitch <= -PI/2 ||

+            ToDeg(rad_diff(pitch, PI/2)) < 1 ||

+            ToDeg(rad_diff(pitch, -PI/2)) < 1) {

+            // we expect breakdown at these poles

+            Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",

+                          ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));

+        } else {

+            Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",

+                          ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));

+        }

+    }

+}

+

+static void test_euler(float roll, float pitch, float yaw)

+{

+    Matrix3f m;

+    float roll2, pitch2, yaw2;

+

+    m.from_euler(roll, pitch, yaw);

+    m.to_euler(&roll2, &pitch2, &yaw2);

+    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

+}

+

+#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))

+

+static const float angles[] = { 0, PI/8, PI/4, PI/2, PI,

+                                -PI/8, -PI/4, -PI/2, -PI};

+

+void test_matrix_eulers(void)

+{

+    uint8_t i, j, k;

+    uint8_t N = ARRAY_LENGTH(angles);

+

+    Serial.println("rotation matrix unit tests\n");

+

+    for (i=0; i<N; i++)

+        for (j=0; j<N; j++)

+            for (k=0; k<N; k++)

+                test_euler(angles[i], angles[j], angles[k]);

+

+    Serial.println("tests done\n");

+}

+

+static void test_quaternion(float roll, float pitch, float yaw)

+{

+    Quaternion q;

+    float roll2, pitch2, yaw2;

+

+    q.from_euler(roll, pitch, yaw);

+    q.to_euler(&roll2, &pitch2, &yaw2);

+    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

+}

+

+void test_quaternion_eulers(void)

+{

+    uint8_t i, j, k;

+    uint8_t N = ARRAY_LENGTH(angles);

+

+    Serial.println("quaternion unit tests\n");

+

+    test_quaternion(PI/4, 0, 0);

+    test_quaternion(0, PI/4, 0);

+    test_quaternion(0, 0, PI/4);

+    test_quaternion(-PI/4, 0, 0);

+    test_quaternion(0, -PI/4, 0);

+    test_quaternion(0, 0, -PI/4);

+    test_quaternion(-PI/4, 1, 1);

+    test_quaternion(1, -PI/4, 1);

+    test_quaternion(1, 1, -PI/4);

+

+    test_quaternion(ToRad(89), 0, 0.1);

+    test_quaternion(0, ToRad(89), 0.1);

+    test_quaternion(0.1, 0, ToRad(89));

+

+    test_quaternion(ToRad(91), 0, 0.1);

+    test_quaternion(0, ToRad(91), 0.1);

+    test_quaternion(0.1, 0, ToRad(91));

+

+    for (i=0; i<N; i++)

+        for (j=0; j<N; j++)

+            for (k=0; k<N; k++)

+                test_quaternion(angles[i], angles[j], angles[k]);

+

+    Serial.println("tests done\n");

+}

+

+

+static void test_conversion(float roll, float pitch, float yaw)

+{

+    Quaternion q;

+    Matrix3f m, m2;

+

+    float roll2, pitch2, yaw2;

+    float roll3, pitch3, yaw3;

+

+    q.from_euler(roll, pitch, yaw);

+    q.to_euler(&roll2, &pitch2, &yaw2);

+    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

+

+    q.rotation_matrix(m);

+    m.to_euler(&roll2, &pitch2, &yaw2);

+

+    m2.from_euler(roll, pitch, yaw);

+    m2.to_euler(&roll3, &pitch3, &yaw3);

+    if (m.is_nan()) {

+        Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",

+                      roll, pitch, yaw);

+    }

+

+    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

+    check_result(roll, pitch, yaw, roll3, pitch3, yaw3);

+}

+

+void test_conversions(void)

+{

+    uint8_t i, j, k;

+    uint8_t N = ARRAY_LENGTH(angles);

+

+    Serial.println("matrix/quaternion tests\n");

+

+    test_conversion(1, 1.1, 1.2);

+    test_conversion(1, -1.1, 1.2);

+    test_conversion(1, -1.1, -1.2);

+    test_conversion(-1, 1.1, -1.2);

+    test_conversion(-1, 1.1, 1.2);

+

+    for (i=0; i<N; i++)

+        for (j=0; j<N; j++)

+            for (k=0; k<N; k++)

+                test_conversion(angles[i], angles[j], angles[k]);

+

+    Serial.println("tests done\n");

+}

+

+void test_frame_transforms(void)

+{

+    Vector3f v, v2;

+    Quaternion q;

+    Matrix3f m;

+

+    Serial.println("frame transform tests\n");

+

+    q.from_euler(ToRad(90), 0, 0);

+    v2 = v = Vector3f(0, 0, 1);

+    q.earth_to_body(v2);

+    printf("%f %f %f\n", v2.x, v2.y, v2.z);

+}

+

+// generate a random float between -1 and 1

+static float rand_num(void)

+{

+	float ret = ((unsigned)random()) % 2000000;

+	return (ret - 1.0e6) / 1.0e6;

+}

+

+void test_matrix_rotate(void)

+{

+    Matrix3f m1, m2, diff;

+    Vector3f r;

+

+    m1.identity();

+    m2.identity();

+    r.x = rand_num();

+    r.y = rand_num();

+    r.z = rand_num();

+

+    for (uint16_t i = 0; i<1000; i++) {

+        // old method

+        Matrix3f temp_matrix;

+        temp_matrix.a.x = 0;

+        temp_matrix.a.y = -r.z;

+        temp_matrix.a.z =  r.y;

+        temp_matrix.b.x =  r.z;

+        temp_matrix.b.y = 0;

+        temp_matrix.b.z = -r.x;

+        temp_matrix.c.x = -r.y;

+        temp_matrix.c.y =  r.x;

+        temp_matrix.c.z = 0;

+        temp_matrix = m1 * temp_matrix;

+        m1 += temp_matrix;

+

+        // new method

+        m2.rotate(r);

+

+        // check they behave in the same way

+        diff = m1 - m2;

+        float err = diff.a.length() + diff.b.length() + diff.c.length();

+

+        if (err > 0) {

+            Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err);

+        }

+    }

+}

+

+/*

+  euler angle tests

+ */

+void setup(void)

+{

+    Serial.begin(115200);

+    Serial.println("euler unit tests\n");

+

+    test_conversion(0, PI, 0);

+

+    test_frame_transforms();

+    test_conversions();

+    test_quaternion_eulers();

+    test_matrix_eulers();

+    test_matrix_rotate();

+}

+

+void

+loop(void)

+{

+}

 

--- /dev/null
+++ b/libraries/AP_Math/examples/polygon/Makefile
@@ -1,1 +1,2 @@
+include ../../../AP_Common/Arduino.mk

 

--- /dev/null
+++ b/libraries/AP_Math/examples/polygon/polygon.pde
@@ -1,1 +1,116 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// Unit tests for the AP_Math polygon code

+//

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+#include <AP_Math.h>

+

+FastSerialPort(Serial, 0);

+

+/*

+  this is the boundary of the 2010 outback challenge

+  Note that the last point must be the same as the first for the

+  Polygon_outside() algorithm

+ */

+static const Vector2l OBC_boundary[] = {

+    Vector2l(-265695640, 1518373730),

+    Vector2l(-265699560, 1518394050),

+    Vector2l(-265768230, 1518411420),

+    Vector2l(-265773080, 1518403440),

+    Vector2l(-265815110, 1518419500),

+    Vector2l(-265784860, 1518474690),

+    Vector2l(-265994890, 1518528860),

+    Vector2l(-266092110, 1518747420),

+    Vector2l(-266454780, 1518820530),

+    Vector2l(-266435720, 1518303500),

+    Vector2l(-265875990, 1518344050),

+    Vector2l(-265695640, 1518373730)

+};

+

+static const struct {

+    Vector2l point;

+    bool outside;

+} test_points[] = {

+    { Vector2l(-266398870, 1518220000), true },

+    { Vector2l(-266418700, 1518709260), false },

+    { Vector2l(-350000000, 1490000000), true },

+    { Vector2l(0, 0),                   true },

+    { Vector2l(-265768150, 1518408250), false },

+    { Vector2l(-265774060, 1518405860), true },

+    { Vector2l(-266435630, 1518303440), true },

+    { Vector2l(-266435650, 1518313540), false },

+    { Vector2l(-266435690, 1518303530), false },

+    { Vector2l(-266435690, 1518303490), true },

+    { Vector2l(-265875990, 1518344049), true },

+    { Vector2l(-265875990, 1518344051), false },

+    { Vector2l(-266454781, 1518820530), true },

+    { Vector2l(-266454779, 1518820530), true },

+    { Vector2l(-266092109, 1518747420), true },

+    { Vector2l(-266092111, 1518747420), false },

+    { Vector2l(-266092110, 1518747421), true },

+    { Vector2l(-266092110, 1518747419), false },

+    { Vector2l(-266092111, 1518747421), true },

+    { Vector2l(-266092109, 1518747421), true },

+    { Vector2l(-266092111, 1518747419), false },

+};

+

+#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))

+

+/*

+  polygon tests

+ */

+void setup(void)

+{

+    unsigned i, count;

+    bool all_passed = true;

+    uint32_t start_time;

+

+    Serial.begin(115200);

+    Serial.println("polygon unit tests\n");

+

+    if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {

+        Serial.println("OBC boundary is not complete!");

+        all_passed = false;

+    }

+

+    if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) {

+        Serial.println("Polygon_complete test failed");

+        all_passed = false;

+    }

+

+    for (i=0; i<ARRAY_LENGTH(test_points); i++) {

+        bool result;

+        result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));

+        Serial.printf_P(PSTR("%10f,%10f  %s  %s\n"),

+                        1.0e-7*test_points[i].point.x,

+                        1.0e-7*test_points[i].point.y,

+                        result?"OUTSIDE":"INSIDE ",

+                        result == test_points[i].outside?"PASS":"FAIL");

+        if (result != test_points[i].outside) {

+            all_passed = false;

+        }

+    }

+    Serial.println(all_passed?"TEST PASSED":"TEST FAILED");

+

+    Serial.println("Speed test:");

+    start_time = micros();

+    for (count=0; count<1000; count++) {

+        for (i=0; i<ARRAY_LENGTH(test_points); i++) {

+            bool result;

+            result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));

+            if (result != test_points[i].outside) {

+                all_passed = false;

+            }

+        }

+    }    

+    Serial.printf("%u usec/call\n", (unsigned)((micros() - start_time)/(count*ARRAY_LENGTH(test_points))));

+    Serial.println(all_passed?"ALL TESTS PASSED":"TEST FAILED");                  

+}

+

+void

+loop(void)

+{

+}

 

--- /dev/null
+++ b/libraries/AP_Math/examples/rotations/Makefile
@@ -1,1 +1,5 @@
+include ../../../AP_Common/Arduino.mk

+

+sitl:

+	make -f ../../../../libraries/Desktop/Desktop.mk

 

--- /dev/null
+++ b/libraries/AP_Math/examples/rotations/rotations.pde
@@ -1,1 +1,206 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+//

+// Unit tests for the AP_Math rotations code

+//

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+#include <AP_Math.h>

+

+FastSerialPort(Serial, 0);

+

+#ifdef DESKTOP_BUILD

+// all of this is needed to build with SITL

+#include <DataFlash.h>

+#include <APM_RC.h>

+#include <GCS_MAVLink.h>

+#include <Arduino_Mega_ISR_Registry.h>

+#include <AP_PeriodicProcess.h>

+#include <AP_ADC.h>

+#include <AP_Baro.h>

+#include <AP_Compass.h>

+#include <AP_GPS.h>

+Arduino_Mega_ISR_Registry isr_registry;

+AP_Baro_BMP085_HIL      barometer;

+AP_Compass_HIL     compass;

+#endif

+

+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library

+

+

+// standard rotation matrices (these are the originals from the old code)

+#define MATRIX_ROTATION_NONE               Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)

+#define MATRIX_ROTATION_YAW_45             Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_90             Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_135            Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_180            Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_225            Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_270            Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_315            Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)

+#define MATRIX_ROTATION_ROLL_180           Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_45    Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_90    Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_135   Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)

+#define MATRIX_ROTATION_PITCH_180          Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_225   Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_270   Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_315   Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)

+

+static void print_matrix(Matrix3f &m)

+{

+    Serial.printf("[%.2f %.2f %.2f] [%.2f %.2f %.2f] [%.2f %.2f %.2f]\n",

+                  m.a.x, m.a.y, m.a.z,

+                  m.b.x, m.b.y, m.b.z,

+                  m.c.x, m.c.y, m.c.z);

+}

+

+// test one matrix

+static void test_matrix(enum Rotation rotation, Matrix3f m)

+{

+	Matrix3f m2, diff;

+	const float accuracy = 1.0e-6;

+	m2.rotation(rotation);

+	diff = (m - m2);

+	if (diff.a.length() > accuracy ||

+	    diff.b.length() > accuracy ||

+	    diff.c.length() > accuracy) {

+		Serial.printf("rotation matrix %u incorrect\n", (unsigned)rotation);

+        print_matrix(m);

+        print_matrix(m2);

+	}

+}

+

+// test generation of rotation matrices

+static void test_matrices(void)

+{

+    Serial.println("testing rotation matrices\n");

+    test_matrix(ROTATION_NONE, MATRIX_ROTATION_NONE);

+    test_matrix(ROTATION_YAW_45, MATRIX_ROTATION_YAW_45);

+    test_matrix(ROTATION_YAW_90, MATRIX_ROTATION_YAW_90);

+    test_matrix(ROTATION_YAW_135, MATRIX_ROTATION_YAW_135);

+    test_matrix(ROTATION_YAW_180, MATRIX_ROTATION_YAW_180);

+    test_matrix(ROTATION_YAW_225, MATRIX_ROTATION_YAW_225);

+    test_matrix(ROTATION_YAW_270, MATRIX_ROTATION_YAW_270);

+    test_matrix(ROTATION_YAW_315, MATRIX_ROTATION_YAW_315);

+    test_matrix(ROTATION_ROLL_180, MATRIX_ROTATION_ROLL_180);

+    test_matrix(ROTATION_ROLL_180_YAW_45, MATRIX_ROTATION_ROLL_180_YAW_45);

+    test_matrix(ROTATION_ROLL_180_YAW_90, MATRIX_ROTATION_ROLL_180_YAW_90);

+    test_matrix(ROTATION_ROLL_180_YAW_135, MATRIX_ROTATION_ROLL_180_YAW_135);

+    test_matrix(ROTATION_PITCH_180, MATRIX_ROTATION_PITCH_180);

+    test_matrix(ROTATION_ROLL_180_YAW_225, MATRIX_ROTATION_ROLL_180_YAW_225);

+    test_matrix(ROTATION_ROLL_180_YAW_270, MATRIX_ROTATION_ROLL_180_YAW_270);

+    test_matrix(ROTATION_ROLL_180_YAW_315, MATRIX_ROTATION_ROLL_180_YAW_315);

+}

+

+// test rotation of vectors

+static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true)

+{

+	Vector3f v2, diff;

+	Matrix3f m;

+	v2 = v1;

+	m.rotation(rotation);

+	v1.rotate(rotation);

+	v2 = m * v2;

+	diff = v1 - v2;

+	if (diff.length() > 1.0e-6) {

+		Serial.printf("rotation vector %u incorrect\n", (unsigned)rotation);

+		Serial.printf("%u  %f %f %f\n",

+			      (unsigned)rotation,

+			      v2.x, v2.y, v2.z);

+	}

+    if (show) {

+        Serial.printf("%u  %f %f %f\n",

+                      (unsigned)rotation,

+                      v1.x, v1.y, v1.z);

+    }

+}

+

+// generate a random float between -1 and 1

+static float rand_num(void)

+{

+	float ret = ((unsigned)random()) % 2000000;

+	return (ret - 1.0e6) / 1.0e6;

+}

+

+// test rotation of vectors

+static void test_vector(enum Rotation rotation)

+{

+    uint8_t i;

+

+	Vector3f v1;

+	v1.x = 1;

+	v1.y = 2;

+	v1.z = 3;

+    test_vector(rotation, v1);

+

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

+        v1.x = rand_num();

+        v1.y = rand_num();

+        v1.z = rand_num();

+        test_vector(rotation, v1, false);

+    }

+}

+

+// test rotation of vectors

+static void test_vectors(void)

+{

+    Serial.println("testing rotation of vectors\n");

+    test_vector(ROTATION_NONE);

+    test_vector(ROTATION_YAW_45);

+    test_vector(ROTATION_YAW_90);

+    test_vector(ROTATION_YAW_135);

+    test_vector(ROTATION_YAW_180);

+    test_vector(ROTATION_YAW_225);

+    test_vector(ROTATION_YAW_270);

+    test_vector(ROTATION_YAW_315);

+    test_vector(ROTATION_ROLL_180);

+    test_vector(ROTATION_ROLL_180_YAW_45);

+    test_vector(ROTATION_ROLL_180_YAW_90);

+    test_vector(ROTATION_ROLL_180_YAW_135);

+    test_vector(ROTATION_PITCH_180);

+    test_vector(ROTATION_ROLL_180_YAW_225);

+    test_vector(ROTATION_ROLL_180_YAW_270);

+    test_vector(ROTATION_ROLL_180_YAW_315);

+}

+

+

+// test combinations of rotations

+static void test_combinations(void)

+{

+    enum Rotation r1, r2, r3;

+    bool found;

+

+	for (r1=ROTATION_NONE; r1<ROTATION_MAX;

+	     r1 = (enum Rotation)((uint8_t)r1+1)) {

+        for (r2=ROTATION_NONE; r2<ROTATION_MAX;

+             r2 = (enum Rotation)((uint8_t)r2+1)) {

+            r3 = rotation_combination(r1, r2, &found);

+            if (found) {

+                Serial.printf("rotation: %u + %u -> %u\n",

+                              (unsigned)r1, (unsigned)r2, (unsigned)r3);

+            } else {

+                Serial.printf("ERROR rotation: no combination for %u + %u\n",

+                              (unsigned)r1, (unsigned)r2);

+            }

+        }

+    }

+}

+

+/*

+  rotation tests

+ */

+void setup(void)

+{

+    Serial.begin(115200);

+    Serial.println("rotation unit tests\n");

+    test_matrices();

+    test_vectors();

+    test_combinations();

+    Serial.println("rotation unit tests done\n");

+}

+

+void

+loop(void)

+{

+}

 

--- /dev/null
+++ b/libraries/AP_Math/keywords.txt
@@ -1,1 +1,30 @@
+Vector2          KEYWORD1

+Vector2i         KEYWORD1

+Vector2ui        KEYWORD1

+Vector2l         KEYWORD1

+Vector2ul        KEYWORD1

+Vector2f         KEYWORD1

+Vector3          KEYWORD1

+Vector3i         KEYWORD1

+Vector3ui        KEYWORD1

+Vector3l         KEYWORD1

+Vector3ul        KEYWORD1

+Vector3f         KEYWORD1

+Matrix3          KEYWORD1

+Matrix3i         KEYWORD1

+Matrix3ui        KEYWORD1

+Matrix3l         KEYWORD1

+Matrix3ul        KEYWORD1

+Matrix3f         KEYWORD1

+length_squared   KEYWORD2

+length           KEYWORD2

+normalize        KEYWORD2

+normalized       KEYWORD2

+reflect          KEYWORD2

+project          KEYWORD2

+projected        KEYWORD2

+angle            KEYWORD2

+angle_normalized KEYWORD2

+rotate           KEYWORD2

+rotated          KEYWORD2

 

--- /dev/null
+++ b/libraries/AP_Math/matrix3.cpp
@@ -1,1 +1,200 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+/*

+ * matrix3.cpp

+ * Copyright (C) Andrew Tridgell 2012

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#include "AP_Math.h"

+

+#define HALF_SQRT_2 0.70710678118654757

+

+#define MATRIX_ROTATION_NONE               Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)

+#define MATRIX_ROTATION_YAW_45             Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_90             Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_135            Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_180            Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_225            Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_270            Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)

+#define MATRIX_ROTATION_YAW_315            Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1)

+#define MATRIX_ROTATION_ROLL_180           Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_45    Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_90    Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_135   Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1)

+#define MATRIX_ROTATION_PITCH_180          Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_225   Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_270   Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)

+#define MATRIX_ROTATION_ROLL_180_YAW_315   Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1)

+

+// fill in a matrix with a standard rotation

+template <typename T>

+void Matrix3<T>::rotation(enum Rotation r)

+{

+    switch (r) {

+    case ROTATION_NONE:

+    case ROTATION_MAX:

+	    *this = MATRIX_ROTATION_NONE;

+	    break;

+    case ROTATION_YAW_45:

+	    *this = MATRIX_ROTATION_YAW_45;

+	    break;

+    case ROTATION_YAW_90:

+	    *this = MATRIX_ROTATION_YAW_90;

+	    break;

+    case ROTATION_YAW_135:

+	    *this = MATRIX_ROTATION_YAW_135;

+	    break;

+    case ROTATION_YAW_180:

+	    *this = MATRIX_ROTATION_YAW_180;

+	    break;

+    case ROTATION_YAW_225:

+	    *this = MATRIX_ROTATION_YAW_225;

+	    break;

+    case ROTATION_YAW_270:

+	    *this = MATRIX_ROTATION_YAW_270;

+	    break;

+    case ROTATION_YAW_315:

+	    *this = MATRIX_ROTATION_YAW_315;

+	    break;

+    case ROTATION_ROLL_180:

+	    *this = MATRIX_ROTATION_ROLL_180;

+	    break;

+    case ROTATION_ROLL_180_YAW_45:

+	    *this = MATRIX_ROTATION_ROLL_180_YAW_45;

+	    break;

+    case ROTATION_ROLL_180_YAW_90:

+	    *this = MATRIX_ROTATION_ROLL_180_YAW_90;

+	    break;

+    case ROTATION_ROLL_180_YAW_135:

+	    *this = MATRIX_ROTATION_ROLL_180_YAW_135;

+	    break;

+    case ROTATION_PITCH_180:

+	    *this = MATRIX_ROTATION_PITCH_180;

+	    break;

+    case ROTATION_ROLL_180_YAW_225:

+	    *this = MATRIX_ROTATION_ROLL_180_YAW_225;

+	    break;

+    case ROTATION_ROLL_180_YAW_270:

+	    *this = MATRIX_ROTATION_ROLL_180_YAW_270;

+	    break;

+    case ROTATION_ROLL_180_YAW_315:

+	    *this = MATRIX_ROTATION_ROLL_180_YAW_315;

+	    break;

+    }

+}

+

+// create a rotation matrix given some euler angles

+// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf

+template <typename T>

+void Matrix3<T>::from_euler(float roll, float pitch, float yaw)

+{

+	float cp = cos(pitch);

+	float sp = sin(pitch);

+	float sr = sin(roll);

+	float cr = cos(roll);

+	float sy = sin(yaw);

+	float cy = cos(yaw);

+

+	a.x = cp * cy;

+	a.y = (sr * sp * cy) - (cr * sy);

+	a.z = (cr * sp * cy) + (sr * sy);

+	b.x = cp * sy;

+	b.y = (sr * sp * sy) + (cr * cy);

+	b.z = (cr * sp * sy) - (sr * cy);

+	c.x = -sp;

+	c.y = sr * cp;

+	c.z = cr * cp;

+}

+

+// calculate euler angles from a rotation matrix

+// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf

+template <typename T>

+void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)

+{

+	if (pitch != NULL) {

+		*pitch = -safe_asin(c.x);

+	}

+	if (roll != NULL) {

+		*roll = atan2(c.y, c.z);

+	}

+	if (yaw != NULL) {

+		*yaw = atan2(b.x, a.x);

+	}

+}

+

+// apply an additional rotation from a body frame gyro vector

+// to a rotation matrix.

+template <typename T>

+void Matrix3<T>::rotate(const Vector3<T> &g)

+{

+	Matrix3f temp_matrix;

+	temp_matrix.a.x = a.y * g.z - a.z * g.y;

+	temp_matrix.a.y = a.z * g.x - a.x * g.z;

+	temp_matrix.a.z = a.x * g.y - a.y * g.x;

+	temp_matrix.b.x = b.y * g.z - b.z * g.y;

+	temp_matrix.b.y = b.z * g.x - b.x * g.z;

+	temp_matrix.b.z = b.x * g.y - b.y * g.x;

+	temp_matrix.c.x = c.y * g.z - c.z * g.y;

+	temp_matrix.c.y = c.z * g.x - c.x * g.z;

+	temp_matrix.c.z = c.x * g.y - c.y * g.x;

+

+	(*this) += temp_matrix;

+}

+

+

+// multiplication by a vector

+template <typename T>

+Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const

+{

+    return Vector3<T>(a.x * v.x + a.y * v.y + a.z * v.z,

+                      b.x * v.x + b.y * v.y + b.z * v.z,

+                      c.x * v.x + c.y * v.y + c.z * v.z);

+}

+

+// multiplication of transpose by a vector

+template <typename T>

+Vector3<T> Matrix3<T>::mul_transpose(const Vector3<T> &v) const

+{

+    return Vector3<T>(a.x * v.x + b.x * v.y + c.x * v.z,

+                      a.y * v.x + b.y * v.y + c.y * v.z,

+                      a.z * v.x + b.z * v.y + c.z * v.z);

+}

+

+// multiplication by another Matrix3<T>

+template <typename T>

+Matrix3<T> Matrix3<T>::operator *(const Matrix3<T> &m) const

+{

+    Matrix3<T> temp (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,

+                                a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,

+                                a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),

+                     Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,

+                                b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,

+                                b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),

+                     Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,

+                                c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,

+                                c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));

+    return temp;

+}

+

+

+// only define for float

+template void Matrix3<float>::rotation(enum Rotation);

+template void Matrix3<float>::rotate(const Vector3<float> &g);

+template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);

+template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);

+template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;

+template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;

+template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;

 

--- /dev/null
+++ b/libraries/AP_Math/matrix3.h
@@ -1,1 +1,154 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+// Copyright 2010 Michael Smith, all rights reserved.

+

+//	This library is free software; you can redistribute it and / or

+//	modify it under the terms of the GNU Lesser General Public

+//	License as published by the Free Software Foundation; either

+//	version 2.1 of the License, or (at your option) any later version.

+

+// Inspired by:

+/****************************************

+ * 3D Vector Classes

+ * By Bill Perone (billperone@yahoo.com)

+ */

+

+//

+// 3x3 matrix implementation.

+//

+// Note that the matrix is organised in row-normal form (the same as

+// applies to array indexing).

+//

+// In addition to the template, this header defines the following types:

+//

+// Matrix3i		3x3 matrix of signed integers

+// Matrix3ui	3x3 matrix of unsigned integers

+// Matrix3l		3x3 matrix of signed longs

+// Matrix3ul	3x3 matrix of unsigned longs

+// Matrix3f		3x3 matrix of signed floats

+//

+

+#ifndef MATRIX3_H

+#define MATRIX3_H

+

+#include "vector3.h"

+

+// 3x3 matrix with elements of type T

+template <typename T>

+class Matrix3 {

+public:

+

+	// Vectors comprising the rows of the matrix

+	Vector3<T>	a, b, c;

+

+	// trivial ctor

+	// note that the Vector3 ctor will zero the vector elements

+	Matrix3<T>() {}

+

+	// setting ctor

+	Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}

+

+	// setting ctor

+	Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {}

+

+	// function call operator

+	void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)

+	{	a = a0; b = b0; c = c0;  }

+

+	// test for equality

+	bool operator == (const Matrix3<T> &m)

+	{	return (a==m.a && b==m.b && c==m.c);	}

+

+	// test for inequality

+	bool operator != (const Matrix3<T> &m)

+	{	return (a!=m.a || b!=m.b || c!=m.c);	}

+

+	// negation

+	Matrix3<T> operator - (void) const

+	{	return Matrix3<T>(-a,-b,-c);	}

+

+	// addition

+	Matrix3<T> operator + (const Matrix3<T> &m) const

+	{   return Matrix3<T>(a+m.a, b+m.b, c+m.c);	 }

+	Matrix3<T> &operator += (const Matrix3<T> &m)

+	{	return *this = *this + m;	}

+

+	// subtraction

+	Matrix3<T> operator - (const Matrix3<T> &m) const

+	{   return Matrix3<T>(a-m.a, b-m.b, c-m.c);	 }

+	Matrix3<T> &operator -= (const Matrix3<T> &m)

+	{	return *this = *this - m;	}

+

+	// uniform scaling

+	Matrix3<T> operator * (const T num) const

+	{	return Matrix3<T>(a*num, b*num, c*num);	}

+	Matrix3<T> &operator *= (const T num)

+	{	return *this = *this * num;	}

+	 Matrix3<T> operator / (const T num) const

+	{	return Matrix3<T>(a/num, b/num, c/num);	}

+	Matrix3<T> &operator /= (const T num)

+	{	return *this = *this / num;	}

+

+	// multiplication by a vector

+	Vector3<T> operator *(const Vector3<T> &v) const;

+

+	// multiplication of transpose by a vector

+	Vector3<T> mul_transpose(const Vector3<T> &v) const;

+

+	// multiplication by another Matrix3<T>

+	Matrix3<T> operator *(const Matrix3<T> &m) const;

+

+	Matrix3<T> &operator *=(const Matrix3<T> &m)

+	{	return *this = *this * m;	}

+

+	// transpose the matrix

+	Matrix3<T> transposed(void) const

+	{

+		return Matrix3<T>(Vector3<T>(a.x, b.x, c.x),

+						  Vector3<T>(a.y, b.y, c.y),

+						  Vector3<T>(a.z, b.z, c.z));

+	}

+	Matrix3<T> transpose(void)

+	{	return *this = transposed();	}

+

+	// zero the matrix

+	void zero(void) {

+		a.x = a.y = a.z = 0;

+		b.x = b.y = b.z = 0;

+		c.x = c.y = c.z = 0;

+	}

+

+	// setup the identity matrix

+	void identity(void) {

+		a.x = b.y = c.z = 1;

+		a.y = a.z = 0;

+		b.x = b.z = 0;

+		c.x = c.y = 0;

+	}

+

+	// check if any elements are NAN

+	bool is_nan(void)

+		{   return a.is_nan() || b.is_nan() || c.is_nan(); }

+

+	// fill in the matrix with a standard rotation

+	void rotation(enum Rotation rotation);

+

+    // create a rotation matrix from Euler angles

+	void from_euler(float roll, float pitch, float yaw);

+

+    // create eulers from a rotation matrix

+	void to_euler(float *roll, float *pitch, float *yaw);

+

+    // apply an additional rotation from a body frame gyro vector

+    // to a rotation matrix.

+	void rotate(const Vector3<T> &g);

+};

+

+typedef Matrix3<int16_t>		Matrix3i;

+typedef Matrix3<uint16_t>		Matrix3ui;

+typedef Matrix3<int32_t>		Matrix3l;

+typedef Matrix3<uint32_t>		Matrix3ul;

+typedef Matrix3<float>			Matrix3f;

+

+#endif // MATRIX3_H

 

--- /dev/null
+++ b/libraries/AP_Math/polygon.cpp
@@ -1,1 +1,92 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+/*

+ * polygon.cpp

+ * Copyright (C) Andrew Tridgell 2011

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#include "AP_Math.h"

+

+/*

+  The point in polygon algorithm is based on:

+  http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html

+*/

+

+

+/*

+  Polygon_outside(): test for a point in a polygon

+      Input:   P = a point,

+               V[] = vertex points of a polygon V[n+1] with V[n]=V[0]

+      Return:  true if P is outside the polygon

+

+  This does not take account of the curvature of the earth, but we

+  expect that to be very small over the distances involved in the

+  fence boundary

+*/

+bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n)

+{

+    unsigned i, j;

+    bool outside = true;

+    for (i = 0, j = n-1; i < n; j = i++) {

+        if ((V[i].y > P.y) == (V[j].y > P.y)) {

+            continue;

+        }

+        int32_t dx1, dx2, dy1, dy2;

+        dx1 = P.x - V[i].x;

+        dx2 = V[j].x - V[i].x;

+        dy1 = P.y - V[i].y;

+        dy2 = V[j].y - V[i].y;

+        int8_t dx1s, dx2s, dy1s, dy2s, m1, m2;

+#define sign(x) ((x)<0?-1:1)

+        dx1s = sign(dx1);

+        dx2s = sign(dx2);

+        dy1s = sign(dy1);

+        dy2s = sign(dy2);

+        m1 = dx1s * dy2s;

+        m2 = dx2s * dy1s;

+        // we avoid the 64 bit multiplies if we can based on sign checks.

+        if (dy2 < 0) {

+            if (m1 > m2) {

+                outside = !outside;

+            } else if (m1 < m2) {

+                continue;

+            } else if ( dx1 * (int64_t)dy2 > dx2 * (int64_t)dy1 ) {

+                outside = !outside;

+            }

+        } else {

+            if (m1 < m2) {

+                outside = !outside;

+            } else if (m1 > m2) {

+                continue;

+            } else if ( dx1 * (int64_t)dy2 < dx2 * (int64_t)dy1 ) {

+                outside = !outside;

+            }

+        }            

+    }

+    return outside;

+}

+

+/*

+  check if a polygon is complete. 

+

+  We consider a polygon to be complete if we have at least 4 points,

+  and the first point is the same as the last point. That is the

+  minimum requirement for the Polygon_outside function to work

+ */

+bool Polygon_complete(const Vector2l *V, unsigned n)

+{

+    return (n >= 4 && V[n-1].x == V[0].x && V[n-1].y == V[0].y);

+}

 

--- /dev/null
+++ b/libraries/AP_Math/polygon.h
@@ -1,1 +1,23 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+/*

+ * polygon.h

+ * Copyright (C) Andrew Tridgell 2011

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n);

+bool Polygon_complete(const Vector2l *V, unsigned n);

+

 

--- /dev/null
+++ b/libraries/AP_Math/quaternion.cpp
@@ -1,1 +1,90 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+/*

+ * quaternion.cpp

+ * Copyright (C) Andrew Tridgell 2012

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#include "AP_Math.h"

+

+// return the rotation matrix equivalent for this quaternion

+void Quaternion::rotation_matrix(Matrix3f &m)

+{

+	float q3q3 = q3 * q3;

+	float q3q4 = q3 * q4;

+	float q2q2 = q2 * q2;

+	float q2q3 = q2 * q3;

+	float q2q4 = q2 * q4;

+	float q1q2 = q1 * q2;

+	float q1q3 = q1 * q3;

+	float q1q4 = q1 * q4;

+    float q4q4 = q4 * q4;

+

+    m.a.x = 1-2*(q3q3 + q4q4);

+    m.a.y =   2*(q2q3 - q1q4);

+    m.a.z =   2*(q2q4 + q1q3);

+    m.b.x =   2*(q2q3 + q1q4);

+    m.b.y = 1-2*(q2q2 + q4q4);

+    m.b.z =   2*(q3q4 - q1q2);

+    m.c.x =   2*(q2q4 - q1q3);

+    m.c.y =   2*(q3q4 + q1q2);

+    m.c.z = 1-2*(q2q2 + q3q3);

+}

+

+// convert a vector from earth to body frame

+void Quaternion::earth_to_body(Vector3f &v)

+{

+    Matrix3f m;

+    // we reverse z before and afterwards because of the differing

+    // quaternion conventions from APM conventions.

+    v.z = -v.z;

+    rotation_matrix(m);

+    v = m * v;

+    v.z = -v.z;

+}

+

+// create a quaternion from Euler angles

+void Quaternion::from_euler(float roll, float pitch, float yaw)

+{

+    float cr2 = cos(roll*0.5);

+    float cp2 = cos(pitch*0.5);

+    float cy2 = cos(yaw*0.5);

+    float sr2 = sin(roll*0.5);

+    float sp2 = sin(pitch*0.5);

+    float sy2 = sin(yaw*0.5);

+

+    q1 = cr2*cp2*cy2 + sr2*sp2*sy2;

+    q2 = sr2*cp2*cy2 - cr2*sp2*sy2;

+    q3 = cr2*sp2*cy2 + sr2*cp2*sy2;

+    q4 = cr2*cp2*sy2 - sr2*sp2*cy2;

+}

+

+// create eulers from a quaternion

+void Quaternion::to_euler(float *roll, float *pitch, float *yaw)

+{

+    if (roll) {

+        *roll = (atan2(2.0*(q1*q2 + q3*q4),

+                       1 - 2.0*(q2*q2 + q3*q3)));

+    }

+    if (pitch) {

+        // we let safe_asin() handle the singularities near 90/-90 in pitch

+        *pitch = safe_asin(2.0*(q1*q3 - q4*q2));

+    }

+    if (yaw) {

+        *yaw = atan2(2.0*(q1*q4 + q2*q3),

+                     1 - 2.0*(q3*q3 + q4*q4));

+    }

+}

 

--- /dev/null
+++ b/libraries/AP_Math/quaternion.h
@@ -1,1 +1,49 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+// Copyright 2012 Andrew Tridgell, all rights reserved.

+

+//	This library is free software; you can redistribute it and / or

+//	modify it under the terms of the GNU Lesser General Public

+//	License as published by the Free Software Foundation; either

+//	version 2.1 of the License, or (at your option) any later version.

+

+#ifndef QUATERNION_H

+#define QUATERNION_H

+

+#include <math.h>

+

+class Quaternion

+{

+public:

+	float q1, q2, q3, q4;

+

+	// constructor creates a quaternion equivalent

+	// to roll=0, pitch=0, yaw=0

+	Quaternion() { q1 = 1; q2 = q3 = q4 = 0; }

+

+	// setting constructor

+	Quaternion(const float _q1, const float _q2, const float _q3, const float _q4):

+	q1(_q1), q2(_q2), q3(_q3), q4(_q4) {}

+

+	// function call operator

+	void operator ()(const float _q1, const float _q2, const float _q3, const float _q4)

+	{ q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4; }

+

+	// check if any elements are NAN

+	bool is_nan(void)

+	{   return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); }

+

+	// return the rotation matrix equivalent for this quaternion

+	void rotation_matrix(Matrix3f &m);

+

+	// convert a vector from earth to body frame

+	void earth_to_body(Vector3f &v);

+

+    // create a quaternion from Euler angles

+	void from_euler(float roll, float pitch, float yaw);

+

+    // create eulers from a quaternion

+	void to_euler(float *roll, float *pitch, float *yaw);

+};

+#endif // QUATERNION_H

 

--- /dev/null
+++ b/libraries/AP_Math/rotations.h
@@ -1,1 +1,47 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+/*

+ * rotations.h

+ * Copyright (C) Andrew Tridgell 2012

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+

+// these rotations form a full set - every rotation in the following

+// list when combined with another in the list forms an entry which is

+// also in the list. This is an important property. Please run the

+// rotations test suite if you add to the list.

+

+// these rotation values are stored to EEPROM, so be careful not to

+// change the numbering of any existing entry when adding a new entry.

+enum Rotation {

+	ROTATION_NONE = 0,

+	ROTATION_YAW_45,

+	ROTATION_YAW_90,

+	ROTATION_YAW_135,

+	ROTATION_YAW_180,

+	ROTATION_YAW_225,

+	ROTATION_YAW_270,

+	ROTATION_YAW_315,

+	ROTATION_ROLL_180,

+	ROTATION_ROLL_180_YAW_45,

+	ROTATION_ROLL_180_YAW_90,

+	ROTATION_ROLL_180_YAW_135,

+	ROTATION_PITCH_180,

+	ROTATION_ROLL_180_YAW_225,

+	ROTATION_ROLL_180_YAW_270,

+	ROTATION_ROLL_180_YAW_315,

+    ROTATION_MAX

+};

 

--- /dev/null
+++ b/libraries/AP_Math/vector2.h
@@ -1,1 +1,158 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+// Copyright 2010 Michael Smith, all rights reserved.

+

+//	This library is free software; you can redistribute it and / or

+//	modify it under the terms of the GNU Lesser General Public

+//	License as published by the Free Software Foundation; either

+//	version 2.1 of the License, or (at your option) any later version.

+

+// Derived closely from:

+/****************************************

+ * 2D Vector Classes

+ * By Bill Perone (billperone@yahoo.com)

+ * Original: 9-16-2002

+ * Revised: 19-11-2003

+ *          18-12-2003

+ *          06-06-2004

+ *


+ * credit is given to Bill Perone in the application it is used in

+ ****************************************/

+

+#ifndef VECTOR2_H

+#define VECTOR2_H

+

+#include <math.h>

+

+template <typename T>

+struct Vector2

+{

+	T x, y;

+

+	// trivial ctor

+	Vector2<T>() { x = y = 0; }

+

+	// setting ctor

+	Vector2<T>(const T x0, const T y0): x(x0), y(y0) {}

+

+	// function call operator

+	void operator ()(const T x0, const T y0)

+	{	x= x0; y= y0;	}

+

+	// test for equality

+	bool operator==(const Vector2<T> &v)

+	{	return (x==v.x && y==v.y);	}

+

+	// test for inequality

+	bool operator!=(const Vector2<T> &v)

+	{	return (x!=v.x || y!=v.y);	}

+

+	// negation

+	Vector2<T> operator -(void) const

+	{	return Vector2<T>(-x, -y);	}

+

+	// addition

+	Vector2<T> operator +(const Vector2<T> &v) const

+	{	return Vector2<T>(x+v.x, y+v.y);	}

+

+	// subtraction

+	Vector2<T> operator -(const Vector2<T> &v) const

+	{   return Vector2<T>(x-v.x, y-v.y);	}

+

+	// uniform scaling

+	Vector2<T> operator *(const T num) const

+	{

+		Vector2<T> temp(*this);

+		return temp*=num;

+	}

+

+	// uniform scaling

+	Vector2<T> operator /(const T num) const

+	{

+		Vector2<T> temp(*this);

+		return temp/=num;

+	}

+

+	// addition

+	Vector2<T> &operator +=(const Vector2<T> &v)

+	{

+		x+=v.x;	y+=v.y;

+		return *this;

+	}

+

+	// subtraction

+	Vector2<T> &operator -=(const Vector2<T> &v)

+	{

+		x-=v.x;	y-=v.y;

+		return *this;

+	}

+

+	// uniform scaling

+	Vector2<T> &operator *=(const T num)

+	{

+		x*=num;	y*=num;

+		return *this;

+	}

+

+	// uniform scaling

+	Vector2<T> &operator /=(const T num)

+	{

+		x/=num;	y/=num;

+		return *this;

+	}

+

+	// dot product

+	T operator *(const Vector2<T> &v) const

+	{	return x*v.x + y*v.y;	}

+

+	// gets the length of this vector squared

+	T length_squared() const

+	{	return (T)(*this * *this);   }

+

+	// gets the length of this vector

+	T length() const

+	{	return (T)sqrt(*this * *this);   }

+

+	// normalizes this vector

+	void normalize()

+	{	*this/=length();	}

+

+	// returns the normalized vector

+	Vector2<T> normalized() const

+	{   return  *this/length();  }

+

+	// reflects this vector about n

+	void reflect(const Vector2<T> &n)

+	{

+		Vector2<T> orig(*this);

+		project(n);

+		*this= *this*2 - orig;

+	}

+

+	// projects this vector onto v

+	void project(const Vector2<T> &v)

+	{	*this= v * (*this * v)/(v*v);	}

+

+	// returns this vector projected onto v

+	Vector2<T> projected(const Vector2<T> &v)

+	{   return v * (*this * v)/(v*v);	}

+

+	// computes the angle between 2 arbitrary vectors

+	T angle(const Vector2<T> &v1, const Vector2<T> &v2)

+	{   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }

+

+	// computes the angle between 2 normalized arbitrary vectors

+	T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)

+	{   return (T)acosf(v1*v2);  }

+

+};

+

+typedef Vector2<int16_t>	Vector2i;

+typedef Vector2<uint16_t>	Vector2ui;

+typedef Vector2<int32_t>	Vector2l;

+typedef Vector2<uint32_t>	Vector2ul;

+typedef Vector2<float>		Vector2f;

+

+#endif // VECTOR2_H

 

--- /dev/null
+++ b/libraries/AP_Math/vector3.cpp
@@ -1,1 +1,116 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

+/*

+ * vector3.cpp

+ * Copyright (C) Andrew Tridgell 2012

+ *

+ * This file 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 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This file 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, see <http://www.gnu.org/licenses/>.

+ */

+

+#include "AP_Math.h"

+

+#define HALF_SQRT_2 0.70710678118654757

+

+// rotate a vector by a standard rotation, attempting

+// to use the minimum number of floating point operations

+template <typename T>

+void Vector3<T>::rotate(enum Rotation rotation)

+{

+    T tmp;

+    switch (rotation) {

+    case ROTATION_NONE:

+    case ROTATION_MAX:

+        return;

+    case ROTATION_YAW_45: {

+        tmp = HALF_SQRT_2*(x - y);

+        y   = HALF_SQRT_2*(x + y);

+        x = tmp;

+        return;

+    }

+    case ROTATION_YAW_90: {

+        tmp = x; x = -y; y = tmp;

+        return;

+    }

+    case ROTATION_YAW_135: {

+        tmp = -HALF_SQRT_2*(x + y);

+        y   =  HALF_SQRT_2*(x - y);

+        x = tmp;

+        return;

+    }

+    case ROTATION_YAW_180:

+        x = -x; y = -y;

+        return;

+    case ROTATION_YAW_225: {

+        tmp = HALF_SQRT_2*(y - x);

+        y   = -HALF_SQRT_2*(x + y);

+        x = tmp;

+        return;

+    }

+    case ROTATION_YAW_270: {

+        tmp = x; x = y; y = -tmp;

+        return;

+    }

+    case ROTATION_YAW_315: {

+        tmp = HALF_SQRT_2*(x + y);

+        y   = HALF_SQRT_2*(y - x);

+        x = tmp;

+        return;

+    }

+    case ROTATION_ROLL_180: {

+        y = -y; z = -z;

+        return;

+    }

+    case ROTATION_ROLL_180_YAW_45: {

+        tmp = HALF_SQRT_2*(x + y);

+        y   = HALF_SQRT_2*(x - y);

+        x = tmp; z = -z;

+        return;

+    }

+    case ROTATION_ROLL_180_YAW_90: {

+        tmp = x; x = y; y = tmp; z = -z;

+        return;

+    }

+    case ROTATION_ROLL_180_YAW_135: {

+        tmp = HALF_SQRT_2*(y - x);

+	y   = HALF_SQRT_2*(y + x);

+        x = tmp; z = -z;

+        return;

+    }

+    case ROTATION_PITCH_180: {

+        x = -x; z = -z;

+        return;

+    }

+    case ROTATION_ROLL_180_YAW_225: {

+        tmp = -HALF_SQRT_2*(x + y);

+	y   =  HALF_SQRT_2*(y - x);

+        x = tmp; z = -z;

+        return;

+    }

+    case ROTATION_ROLL_180_YAW_270: {

+        tmp = x; x = -y; y = -tmp; z = -z;

+        return;

+    }

+    case ROTATION_ROLL_180_YAW_315: {

+        tmp =  HALF_SQRT_2*(x - y);

+	y   = -HALF_SQRT_2*(x + y);

+        x = tmp; z = -z;

+        return;

+    }

+    }

+}

+

+// only define for signed numbers

+template void Vector3<float>::rotate(enum Rotation);

+template void Vector3<int16_t>::rotate(enum Rotation);

+template void Vector3<int32_t>::rotate(enum Rotation);

 

--- /dev/null
+++ b/libraries/AP_Math/vector3.h
@@ -1,1 +1,200 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+// Copyright 2010 Michael Smith, all rights reserved.

+

+//	This library is free software; you can redistribute it and / or

+//	modify it under the terms of the GNU Lesser General Public

+//	License as published by the Free Software Foundation; either

+//	version 2.1 of the License, or (at your option) any later version.

+

+// Derived closely from:

+/****************************************

+ * 3D Vector Classes

+ * By Bill Perone (billperone@yahoo.com)

+ * Original: 9-16-2002

+ * Revised: 19-11-2003

+ *          11-12-2003

+ *          18-12-2003

+ *          06-06-2004

+ *


+ * credit is given to Bill Perone in the application it is used in

+ *

+ * Notes:

+ * if a*b = 0 then a & b are orthogonal

+ * a%b = -b%a

+ * a*(b%c) = (a%b)*c

+ * a%b = a(cast to matrix)*b

+ * (a%b).length() = area of parallelogram formed by a & b

+ * (a%b).length() = a.length()*b.length() * sin(angle between a & b)

+ * (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0

+ * a * (b%c) = volume of parallelpiped formed by a, b, c

+ * vector triple product: a%(b%c) = b*(a*c) - c*(a*b)

+ * scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)

+ * vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)

+ * if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0

+ * vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0

+ *           or if the matrix (A) * b = 0

+ *

+ ****************************************/

+

+#ifndef VECTOR3_H

+#define VECTOR3_H

+

+#include <math.h>

+#include <string.h>

+

+template <typename T>

+class Vector3

+{

+public:

+	T x, y, z;

+

+	// trivial ctor

+	Vector3<T>() { x = y = z = 0; }

+

+	// setting ctor

+	Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {}

+

+	// function call operator

+	void operator ()(const T x0, const T y0, const T z0)

+	{	x= x0; y= y0; z= z0;  }

+

+	// test for equality

+	bool operator==(const Vector3<T> &v)

+	{	return (x==v.x && y==v.y && z==v.z);	}

+

+	// test for inequality

+	bool operator!=(const Vector3<T> &v)

+	{	return (x!=v.x || y!=v.y || z!=v.z);	}

+

+	// negation

+	Vector3<T> operator -(void) const

+	{	return Vector3<T>(-x,-y,-z);	}

+

+	// addition

+	Vector3<T> operator +(const Vector3<T> &v) const

+	{   return Vector3<T>(x+v.x, y+v.y, z+v.z);	 }

+

+	// subtraction

+	Vector3<T> operator -(const Vector3<T> &v) const

+	{   return Vector3<T>(x-v.x, y-v.y, z-v.z);	 }

+

+	// uniform scaling

+	Vector3<T> operator *(const T num) const

+	{

+		Vector3<T> temp(*this);

+		return temp*=num;

+	}

+

+	// uniform scaling

+	Vector3<T> operator /(const T num) const

+	{

+		Vector3<T> temp(*this);

+		return temp/=num;

+	}

+

+	// addition

+	Vector3<T> &operator +=(const Vector3<T> &v)

+	{

+		x+=v.x;	y+=v.y;	z+=v.z;

+		return *this;

+	}

+

+	// subtraction

+	Vector3<T> &operator -=(const Vector3<T> &v)

+	{

+		x-=v.x;	y-=v.y;	z-=v.z;

+		return *this;

+	}

+

+	// uniform scaling

+	Vector3<T> &operator *=(const T num)

+	{

+		x*=num; y*=num; z*=num;

+		return *this;

+	}

+

+	// uniform scaling

+	Vector3<T> &operator /=(const T num)

+	{

+		x/=num; y/=num; z/=num;

+		return *this;

+	}

+

+	// dot product

+	T operator *(const Vector3<T> &v) const

+	{	return x*v.x + y*v.y + z*v.z;	}

+

+	// cross product

+	Vector3<T> operator %(const Vector3<T> &v) const

+	{

+		Vector3<T> temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);

+		return temp;

+	}

+

+	// gets the length of this vector squared

+	T length_squared() const

+	{	return (T)(*this * *this);   }

+

+	// gets the length of this vector

+	float length() const

+	{	return (T)sqrt(*this * *this);   }

+

+	// normalizes this vector

+	void normalize()

+	{	*this/=length();	}

+

+	// zero the vector

+	void zero()

+	{	x = y = z = 0.0; }

+

+	// returns the normalized version of this vector

+	Vector3<T> normalized() const

+	{   return  *this/length();  }

+

+	// reflects this vector about n

+	void reflect(const Vector3<T> &n)

+	{

+		Vector3<T> orig(*this);

+		project(n);

+		*this= *this*2 - orig;

+	}

+

+	// projects this vector onto v

+	void project(const Vector3<T> &v)

+	{	*this= v * (*this * v)/(v*v);	}

+

+	// returns this vector projected onto v

+	Vector3<T> projected(const Vector3<T> &v)

+	{   return v * (*this * v)/(v*v);	}

+

+	// computes the angle between 2 arbitrary vectors

+	T angle(const Vector3<T> &v1, const Vector3<T> &v2)

+	{   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }

+

+	// computes the angle between 2 arbitrary normalized vectors

+	T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)

+	{   return (T)acosf(v1*v2);  }

+

+	// check if any elements are NAN

+	bool is_nan(void)

+		{   return isnan(x) || isnan(y) || isnan(z); }

+

+	// check if any elements are infinity

+	bool is_inf(void)

+		{   return isinf(x) || isinf(y) || isinf(z); }

+

+	// rotate by a standard rotation

+	void rotate(enum Rotation rotation);

+

+};

+

+typedef Vector3<int16_t>		Vector3i;

+typedef Vector3<uint16_t>		Vector3ui;

+typedef Vector3<int32_t>		Vector3l;

+typedef Vector3<uint32_t>		Vector3ul;

+typedef Vector3<float>			Vector3f;

+

+#endif // VECTOR3_H

 

--- /dev/null
+++ b/libraries/FastSerial/BetterStream.cpp
@@ -1,1 +1,61 @@
-
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+//

+//      Copyright (c) 2010 Michael Smith. All rights reserved.

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+//

+// Enhancements to the Arduino Stream class.

+//

+

+#include "limits.h"

+#include "BetterStream.h"

+

+// Stream extensions////////////////////////////////////////////////////////////

+

+void

+BetterStream::print_P(const prog_char *s)

+{

+        char    c;

+

+        while ('\0' != (c = pgm_read_byte(s++)))

+                write(c);

+}

+

+void

+BetterStream::println_P(const char *s)

+{

+        print_P(s);

+        println();

+}

+

+void

+BetterStream::printf(const char *fmt, ...)

+{

+        va_list ap;

+

+        va_start(ap, fmt);

+        _vprintf(0, fmt, ap);

+        va_end(ap);

+}

+

+void

+BetterStream::printf_P(const char *fmt, ...)

+{

+        va_list ap;

+

+        va_start(ap, fmt);

+        _vprintf(1, fmt, ap);

+        va_end(ap);

+}

+

+int

+BetterStream::txspace(void)

+{

+        // by default claim that there is always space in transmit buffer

+        return(INT_MAX);

+}

--- /dev/null
+++ b/libraries/FastSerial/BetterStream.h
@@ -1,1 +1,39 @@
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+//

+//      Copyright (c) 2010 Michael Smith. All rights reserved.

+//

+// This is free software; you can redistribute it and/or modify it under

+// the terms of the GNU Lesser General Public License as published by the

+// Free Software Foundation; either version 2.1 of the License, or (at

+// your option) any later version.

+//

+

+#ifndef __BETTERSTREAM_H

+#define __BETTERSTREAM_H

+

+#include <Stream.h>

+#include <avr/pgmspace.h>

+

+class BetterStream : public Stream {

+public:

+        BetterStream(void) {

+        }

+

+        // Stream extensions

+        void            print_P(const char *);

+        void            println_P(const char *);

+        void            printf(const char *, ...)

+                __attribute__ ((format(__printf__, 2, 3)));

+        void            printf_P(const char *, ...)

+                __attribute__ ((format(__printf__, 2, 3)));

+

+        virtual int     txspace(void);

+

+private:

+        void            _vprintf(unsigned char, const char *, va_list)

+                __attribute__ ((format(__printf__, 3, 0)));

+};

+

+#endif // __BETTERSTREAM_H

+

 

--- /dev/null
+++ b/libraries/FastSerial/FastSerial.cpp
@@ -1,1 +1,282 @@
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+//

+// Interrupt-driven serial transmit/receive library.

+//

+//      Copyright (c) 2010 Michael Smith. All rights reserved.

+//

+// Receive and baudrate calculations derived from the Arduino 

+// HardwareSerial driver:

+//

+//      Copyright (c) 2006 Nicholas Zambetti.  All right reserved.

+//

+// Transmit algorithm inspired by work:

+//

+//      Code Jose Julio and Jordi Munoz. DIYDrones.com

+//

+//      This library is free software; you can redistribute it and/or

+//      modify it under the terms of the GNU Lesser General Public

+//      License as published by the Free Software Foundation; either

+//      version 2.1 of the License, or (at your option) any later version.

+//

+//      This library 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

+//      Lesser General Public License for more details.

+//

+//      You should have received a copy of the GNU Lesser General Public

+//      License along with this library; if not, write to the Free Software

+//      Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

+//

+

+

+//#include "../AP_Common/AP_Common.h"

+#include "FastSerial.h"

+#include "WProgram.h"

+

+#if   defined(UDR3)

+# define FS_MAX_PORTS   4

+#elif defined(UDR2)

+# define FS_MAX_PORTS   3

+#elif defined(UDR1)

+# define FS_MAX_PORTS   2

+#else

+# define FS_MAX_PORTS   1

+#endif

+

+FastSerial::Buffer	__FastSerial__rxBuffer[FS_MAX_PORTS];

+FastSerial::Buffer	__FastSerial__txBuffer[FS_MAX_PORTS];

+

+// Default buffer sizes

+#define RX_BUFFER_SIZE  128

+#define TX_BUFFER_SIZE  64

+#define BUFFER_MAX      512

+

+// Constructor /////////////////////////////////////////////////////////////////

+

+FastSerial::FastSerial(const uint8_t portNumber,

+                       volatile uint8_t *ubrrh,

+                       volatile uint8_t *ubrrl,

+                       volatile uint8_t *ucsra,

+                       volatile uint8_t *ucsrb,

+                       const uint8_t u2x,

+                       const uint8_t portEnableBits,

+                       const uint8_t portTxBits)

+{

+        _ubrrh = ubrrh;

+        _ubrrl = ubrrl;

+        _ucsra = ucsra;

+        _ucsrb = ucsrb;

+        _u2x   = u2x;

+        _portEnableBits = portEnableBits;

+        _portTxBits     = portTxBits;

+

+        // init buffers

+        _rxBuffer = &__FastSerial__rxBuffer[portNumber];

+        _txBuffer->head = _txBuffer->tail = 0;

+        _txBuffer = &__FastSerial__txBuffer[portNumber];

+        _rxBuffer->head = _rxBuffer->tail = 0;

+}

+

+// Public Methods //////////////////////////////////////////////////////////////

+

+void FastSerial::begin(long baud)

+{

+        unsigned int    rxb, txb;

+

+        // If we are re-configuring an already-open port, preserve the

+        // existing buffer sizes.

+        if (_open) {

+                rxb = _rxBuffer->mask + 1;

+                txb = _txBuffer->mask + 1;

+        } else {

+                rxb = RX_BUFFER_SIZE;

+                txb = TX_BUFFER_SIZE;

+        }

+

+        begin(baud, RX_BUFFER_SIZE, TX_BUFFER_SIZE);

+}

+

+void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace)

+{

+        uint16_t        ubrr;

+        bool            use_u2x = false;

+        int             ureg, u2;

+        long            breg, b2, dreg, d2;

+       

+        // if we are currently open, close and restart

+        if (_open)

+                end();

+

+        // allocate buffers

+        if (!_allocBuffer(_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) ||

+            !_allocBuffer(_txBuffer, txSpace ? : TX_BUFFER_SIZE)) {

+                end();

+                return;                 // couldn't allocate buffers - fatal

+        }

+        _open = true;

+

+        // U2X mode is needed for bitrates higher than (F_CPU / 16)

+        if (baud > F_CPU / 16) {

+                use_u2x = true;

+                ubrr = F_CPU / (8 * baud) - 1;

+        } else {

+                // Determine whether u2x mode would give a closer

+                // approximation of the desired speed.

+    

+                // ubrr for non-2x mode, corresponding baudrate and delta

+                ureg = F_CPU / 16 / baud - 1;

+                breg = F_CPU / 16 / (ureg + 1);

+                dreg = abs(baud - breg);

+                

+                // ubrr for 2x mode, corresponding bitrate and delta

+                u2   = F_CPU / 8 / baud - 1;

+                b2   = F_CPU / 8 / (u2 + 1); 

+                d2   = abs(baud - b2);

+

+                // Pick the setting that gives the smallest rate

+                // error, preferring non-u2x mode if the delta is

+                // identical.

+                if (dreg <= d2) {

+                        ubrr = ureg;

+                } else {

+                        ubrr = u2;

+                        use_u2x = true;

+                }                

+        }

+  

+        *_ucsra = use_u2x ? _BV(_u2x) : 0;

+        *_ubrrh = ubrr >> 8;

+        *_ubrrl = ubrr;

+        *_ucsrb |= _portEnableBits;

+}

+

+void FastSerial::end()

+{

+        *_ucsrb &= ~(_portEnableBits | _portTxBits);

+

+        _freeBuffer(_rxBuffer);

+        _freeBuffer(_txBuffer);

+        _open = false;

+}

+

+int

+FastSerial::available(void)

+{

+        if (!_open)

+                return(-1);

+        return((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask);

+}

+

+int FastSerial::txspace(void)

+{

+	if (!_open)

+		return (-1);

+	return ((_txBuffer->mask+1) - ((_txBuffer->head - _txBuffer->tail) & _txBuffer->mask));

+}

+

+int

+FastSerial::read(void)

+{

+        uint8_t         c;

+

+        // if the head and tail are equal, the buffer is empty

+        if (!_open || (_rxBuffer->head == _rxBuffer->tail))

+                return(-1);

+

+        // pull character from tail

+        c = _rxBuffer->bytes[_rxBuffer->tail];

+        _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask;

+

+        return(c);

+}

+

+int

+FastSerial::peek(void)

+{

+

+        // if the head and tail are equal, the buffer is empty

+        if (!_open || (_rxBuffer->head == _rxBuffer->tail))

+                return(-1);

+

+        // pull character from tail

+        return(_rxBuffer->bytes[_rxBuffer->tail]);

+}

+

+

+void

+FastSerial::flush(void)

+{

+        // don't reverse this or there may be problems if the RX interrupt

+        // occurs after reading the value of _rxBuffer->head but before writing

+        // the value to _rxBuffer->tail; the previous value of head

+        // may be written to tail, making it appear as if the buffer

+        // don't reverse this or there may be problems if the RX interrupt

+        // occurs after reading the value of head but before writing

+        // the value to tail; the previous value of rx_buffer_head

+        // may be written to tail, making it appear as if the buffer

+        // were full, not empty.

+        _rxBuffer->head = _rxBuffer->tail;

+

+        // don't reverse this or there may be problems if the TX interrupt

+        // occurs after reading the value of _txBuffer->tail but before writing

+        // the value to _txBuffer->head.

+        _txBuffer->tail = _rxBuffer->head;

+}

+

+size_t

+FastSerial::write(uint8_t c)

+{

+        int16_t         i;

+

+        if (!_open)                     // drop bytes if not open

+                return c;

+

+        // wait for room in the tx buffer

+        i = (_txBuffer->head + 1) & _txBuffer->mask;

+        while (i == _txBuffer->tail)

+                ;

+

+        // add byte to the buffer

+        _txBuffer->bytes[_txBuffer->head] = c;

+        _txBuffer->head = i;

+

+        // enable the data-ready interrupt, as it may be off if the buffer is empty

+        *_ucsrb |= _portTxBits;

+}

+

+// Buffer management ///////////////////////////////////////////////////////////

+

+bool

+FastSerial::_allocBuffer(Buffer *buffer, unsigned int size)

+{

+        uint8_t shift;

+

+        // init buffer state

+        buffer->head = buffer->tail = 0;

+

+        // Compute the power of 2 greater or equal to the requested buffer size

+        // and then a mask to simplify wrapping operations.  Using __builtin_clz

+        // would seem to make sense, but it uses a 256(!) byte table.

+        // Note that we ignore requests for more than BUFFER_MAX space.

+        for (shift = 1; (1U << shift) < min(BUFFER_MAX, size); shift++)

+                ;

+        buffer->mask = (1 << shift) - 1;

+

+        // allocate memory for the buffer - if this fails, we fail

+        buffer->bytes = (uint8_t *)malloc(buffer->mask + 1);

+

+        return(buffer->bytes != NULL);

+}

+

+void

+FastSerial::_freeBuffer(Buffer *buffer)

+{

+        buffer->head = buffer->tail = 0;

+        buffer->mask = 0;

+        if (NULL != buffer->bytes) {

+                free(buffer->bytes);

+                buffer->bytes = NULL;

+        }

+}

+

 

--- /dev/null
+++ b/libraries/FastSerial/FastSerial.h
@@ -1,1 +1,247 @@
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+//

+// Interrupt-driven serial transmit/receive library.

+//

+//      Copyright (c) 2010 Michael Smith. All rights reserved.

+//

+// Receive and baudrate calculations derived from the Arduino 

+// HardwareSerial driver:

+//

+//      Copyright (c) 2006 Nicholas Zambetti.  All right reserved.

+//

+// Transmit algorithm inspired by work:

+//

+//      Code Jose Julio and Jordi Munoz. DIYDrones.com

+//

+//      This library is free software; you can redistribute it and/or

+//      modify it under the terms of the GNU Lesser General Public

+//      License as published by the Free Software Foundation; either

+//      version 2.1 of the License, or (at your option) any later

+//      version.

+//

+//      This library 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 Lesser General Public License for more

+//      details.

+//

+//      You should have received a copy of the GNU Lesser General

+//      Public License along with this library; if not, write to the

+//      Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,

+//      Boston, MA 02110-1301 USA

+//

+

+//

+// Note that this library does not pre-declare drivers for serial

+// ports; the user must explicitly create drivers for the ports they

+// wish to use.  This is less friendly than the stock Arduino driver,

+// but it saves 24 bytes of RAM for every unused port and frees up

+// the vector for another driver (e.g. MSPIM on USARTs).

+//

+

+#ifndef FastSerial_h

+#define FastSerial_h

+

+// disable the stock Arduino serial driver

+#ifdef HardwareSerial_h

+# error Must include FastSerial.h before the Arduino serial driver is defined.

+#endif

+#define HardwareSerial_h

+

+#include <inttypes.h>

+#include <stdlib.h>

+#include <avr/io.h>

+#include <avr/interrupt.h>

+

+#include "BetterStream.h"

+

+//

+// Because Arduino libraries aren't really libraries, but we want to

+// only define interrupt handlers for serial ports that are actually

+// used, we have to force our users to define them using a macro.

+//

+// Due to the way interrupt vectors are specified, we have to have

+// a separate macro for every port.  Ugh.

+//

+// The macros are:

+//

+// FastSerialPort0(<port name>)         creates <port name> referencing serial port 0

+// FastSerialPort1(<port name>)         creates <port name> referencing serial port 1

+// FastSerialPort2(<port name>)         creates <port name> referencing serial port 2

+// FastSerialPort3(<port name>)         creates <port name> referencing serial port 3

+//

+// Note that macros are only defined for ports that exist on the target device.

+//

+

+//

+// Forward declarations for clients that want to assume that the

+// default Serial* objects exist.

+//

+// Note that the application is responsible for ensuring that these

+// actually get defined, otherwise Arduino will suck in the

+// HardwareSerial library and linking will fail.

+//

+extern class FastSerial Serial;

+extern class FastSerial Serial1;

+extern class FastSerial Serial2;

+extern class FastSerial Serial3;

+

+class FastSerial : public BetterStream {

+public:

+        FastSerial(const uint8_t portNumber,

+                   volatile uint8_t *ubrrh,

+                   volatile uint8_t *ubrrl,

+                   volatile uint8_t *ucsra,

+                   volatile uint8_t *ucsrb,

+                   const uint8_t u2x,

+                   const uint8_t portEnableBits,

+                   const uint8_t portTxBits);

+

+        // Serial API

+        virtual void    begin(long baud);

+        virtual void    begin(long baud, unsigned int rxSpace, unsigned int txSpace);

+        virtual void    end(void);

+        virtual int     available(void);

+        virtual int     txspace(void);

+        virtual int     read(void);

+        virtual int     peek(void);

+        virtual void    flush(void);

+        virtual size_t    write(uint8_t c);

+        using BetterStream::write;

+

+        // public so the interrupt handlers can see it

+        struct Buffer {

+                volatile uint16_t head, tail;

+                uint16_t        mask;

+                uint8_t         *bytes;

+        };

+

+private:

+        // register accessors

+        volatile uint8_t *_ubrrh;

+        volatile uint8_t *_ubrrl;

+        volatile uint8_t *_ucsra;

+        volatile uint8_t *_ucsrb;

+

+        // register magic numbers

+        uint8_t         _portEnableBits;        // rx, tx and rx interrupt enables

+        uint8_t         _portTxBits;            // tx data and completion interrupt enables

+        uint8_t         _u2x;

+

+        // ring buffers

+        Buffer          *_rxBuffer;

+        Buffer          *_txBuffer;

+        bool            _open;

+

+        static bool     _allocBuffer(Buffer *buffer, unsigned int size);

+        static void     _freeBuffer(Buffer *buffer);

+};

+

+// Used by the per-port interrupt vectors

+extern FastSerial::Buffer	__FastSerial__rxBuffer[];

+extern FastSerial::Buffer	__FastSerial__txBuffer[];

+

+// Generic Rx/Tx vectors for a serial port - needs to know magic numbers

+#define FastSerialHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR, _UCSRB, _TXBITS) \

+ISR(_RXVECTOR, ISR_BLOCK)                                               \

+{                                                                       \

+        uint8_t c;                                                      \

+        int16_t i;                                                      \

+                                                                        \

+        /* read the byte as quickly as possible */                      \

+        c = _UDR;                                                       \

+        /* work out where the head will go next */                      \

+        i = (__FastSerial__rxBuffer[_PORT].head + 1) & __FastSerial__rxBuffer[_PORT].mask; \

+        /* decide whether we have space for another byte */             \

+        if (i != __FastSerial__rxBuffer[_PORT].tail) {                  \

+                /* we do, move the head */                              \

+                __FastSerial__rxBuffer[_PORT].bytes[__FastSerial__rxBuffer[_PORT].head] = c; \

+                __FastSerial__rxBuffer[_PORT].head = i;                 \

+        }                                                               \

+}                                                                       \

+ISR(_TXVECTOR, ISR_BLOCK)                                               \

+{                                                                       \

+        /* if there is another character to send */                     \

+        if (__FastSerial__txBuffer[_PORT].tail != __FastSerial__txBuffer[_PORT].head) { \

+                _UDR = __FastSerial__txBuffer[_PORT].bytes[__FastSerial__txBuffer[_PORT].tail]; \

+                /* increment the tail */                                \

+                __FastSerial__txBuffer[_PORT].tail =                    \

+                        (__FastSerial__txBuffer[_PORT].tail + 1) & __FastSerial__txBuffer[_PORT].mask; \

+        } else {                                                        \

+                /* there are no more bytes to send, disable the interrupt */ \

+                if (__FastSerial__txBuffer[_PORT].head == __FastSerial__txBuffer[_PORT].tail) \

+                        _UCSRB &= ~_TXBITS;                             \

+        }                                                               \

+}                                                                       \

+struct hack

+

+//

+// Portability; convert various older sets of defines for U(S)ART0 up

+// to match the definitions for the 1280 and later devices.

+//

+#if !defined(USART0_RX_vect)

+# if defined(USART_RX_vect)

+#  define USART0_RX_vect        USART_RX_vect

+#  define USART0_UDRE_vect      USART_UDRE_vect

+# elif defined(UART0_RX_vect)

+#  define USART0_RX_vect        UART0_RX_vect

+#  define USART0_UDRE_vect      UART0_UDRE_vect

+# endif

+#endif

+

+#if !defined(USART1_RX_vect)

+# if defined(UART1_RX_vect)

+#  define USART1_RX_vect        UART1_RX_vect

+#  define USART1_UDRE_vect      UART1_UDRE_vect

+# endif

+#endif

+

+#if !defined(UDR0)

+# if defined(UDR)

+#  define UDR0                  UDR

+#  define UBRR0H                UBRRH

+#  define UBRR0L                UBRRL

+#  define UCSR0A                UCSRA

+#  define UCSR0B                UCSRB

+#  define U2X0                  U2X

+#  define RXEN0                 RXEN

+#  define TXEN0                 TXEN

+#  define RXCIE0                RXCIE

+#  define UDRIE0                UDRIE

+# endif

+#endif

+

+//

+// Macro defining a FastSerial port instance.

+//

+#define FastSerialPort(_name, _num)                                     \

+	FastSerial _name(_num,                                          \

+                         &UBRR##_num##H,                                \

+                         &UBRR##_num##L,                                \

+                         &UCSR##_num##A,                                \

+                         &UCSR##_num##B,                                \

+                         U2X##_num,                                     \

+                         (_BV(RXEN##_num) |  _BV(TXEN##_num) | _BV(RXCIE##_num)), \

+                         (_BV(UDRIE##_num)));                           \

+	FastSerialHandler(_num,                                         \

+                          USART##_num##_RX_vect,                        \

+                          USART##_num##_UDRE_vect,                      \

+                          UDR##_num,                                    \

+                          UCSR##_num##B,                                \

+                          _BV(UDRIE##_num))

+

+//

+// Compatibility macros for previous FastSerial versions.

+//

+// Note that these are not conditionally defined, as the errors

+// generated when using these macros for a board that does not support

+// the port are better than the errors generated for a macro that's not

+// defined at all.

+//

+#define FastSerialPort0(_portName)     FastSerialPort(_portName, 0)

+#define FastSerialPort1(_portName)     FastSerialPort(_portName, 1)

+#define FastSerialPort2(_portName)     FastSerialPort(_portName, 2)

+#define FastSerialPort3(_portName)     FastSerialPort(_portName, 3)

+

+#endif // FastSerial_h

 

--- /dev/null
+++ b/libraries/FastSerial/WProgram.h
@@ -1,1 +1,30 @@
+#ifndef WProgram_h
+#define WProgram_h
 
+#include "wiring.h"
+#include <stdarg.h>
+#include "HardwareSerial.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <stdint.h>
+
+typedef uint8_t byte;
+
+void delay(unsigned long msec);
+char *itoa(int __val, char *__s, int __radix);
+char *ltoa(long __val, char *__s, int __radix);
+char *ultoa(unsigned long __val, char *__s, int __radix);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+

--- /dev/null
+++ b/libraries/FastSerial/examples/FastSerial/FastSerial.pde
@@ -1,1 +1,71 @@
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+

+//

+// Example code for the FastSerial driver.

+//

+// This code is placed into the public domain.

+//

+

+//

+// Include the FastSerial library header.

+//

+// Note that this causes the standard Arduino Serial* driver to be

+// disabled.

+//

+#include <FastSerial.h>

+

+#undef PROGMEM 

+#define PROGMEM __attribute__(( section(".progmem.data") )) 

+

+#undef PSTR 

+#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 

+

+//

+// Create a FastSerial driver that looks just like the stock Arduino

+// driver.

+//

+FastSerialPort0(Serial);

+

+//

+// To create a driver for a different serial port, on a board that

+// supports more than one, use the appropriate macro:

+//

+//FastSerialPort2(Serial2);

+

+

+void setup(void)

+{

+        //

+        // Set the speed for our replacement serial port.

+        //

+	Serial.begin(38400);

+

+        //

+        // Test printing things

+        //

+        Serial.print("test");

+        Serial.println(" begin");

+        Serial.println(1000);

+        Serial.println(1000, 8);

+        Serial.println(1000, 10);

+        Serial.println(1000, 16);

+        Serial.println_P(PSTR("progmem"));

+        Serial.printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));

+        Serial.printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));

+        Serial.println("done");

+}

+

+void

+loop(void)

+{

+    int    c;

+

+    //

+    // Perform a simple loopback operation.

+    //

+    c = Serial.read();

+    if (-1 != c)

+        Serial.write(c);

+}

+

 

--- /dev/null
+++ b/libraries/FastSerial/examples/FastSerial/Makefile
@@ -1,1 +1,3 @@
+BOARD	=	mega

+include ../../../AP_Common/Arduino.mk

 

--- /dev/null
+++ b/libraries/FastSerial/ftoa_engine.S
@@ -1,1 +1,532 @@
-
+/* Copyright (c) 2005, Dmitry Xmelkov
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/* $Id: ftoa_engine.S,v 1.3 2009/04/01 23:11:00 arcanum Exp $ */
+
+#ifndef	__DOXYGEN__
+
+#include "macros.inc"
+#include "ftoa_engine.h"
+
+#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
+#  define AVR_ENH_LPM	1
+#else
+#  define AVR_ENH_LPM	0
+#endif
+
+/*
+   int __ftoa_engine (double val, char *buf,
+                      unsigned char prec, unsigned char maxdgs)
+ Input:
+    val    - value to convert
+    buf    - output buffer address
+    prec   - precision: number of decimal digits is 'prec + 1'
+    maxdgs - (0 if unused) precision restriction for "%f" specification
+
+ Output:
+    return     - decimal exponent of first digit
+    buf[0]     - flags (FTOA_***)
+    buf[1],... - decimal digits
+    Number of digits:
+	maxdgs == 0 ? prec+1 :
+	(buf[0] & FTOA_CARRY) == 0 || buf[1] != '1' ?
+	    aver(1, maxdgs+exp, prec+1) :
+	    aver(1, masdgs+exp-1, prec+1)
+
+ Notes:
+    * Output string is not 0-terminated. For possibility of user's buffer
+    usage in any case.
+    * If used, 'maxdgs' is a number of digits for value with zero exponent.
+*/
+
+    /* Input */
+#define maxdgs	r16
+#define	prec	r18
+#define	buf_lo	r20
+#define	buf_hi	r21
+#define	val_lo	r22
+#define	val_hi	r23
+#define	val_hlo	r24
+#define	val_hhi	r25
+
+    /* Float value parse	*/
+#define	flag	r19
+
+    /* Multiplication of mantisses	*/
+#define	exp_sv	r17
+#define	mlt_1	r19	/* lowest result byte	*/
+#define mlt_2	r14
+#define	mlt_3	r15
+#define	mlt_4	r20
+#define	mlt_5	r21
+#define	mlt_6	r28
+#define	mlt_7	r29
+
+    /* Conversion to string	*/
+#define	pwr_2	r1	/* lowest byte of 'powr10' element	*/
+#define	pwr_3	r17
+#define	pwr_4	r19
+#define	pwr_5	r22
+#define	pwr_6	r25
+#define	pwr_7	r0
+#define	digit	r23
+#define	exp10	r24
+
+    /* Fixed */
+#define	zero	r1
+
+/*    ASSEMBLY_CLIB_SECTION */
+	
+    .global	__ftoa_engine
+    .type	__ftoa_engine, "function"
+__ftoa_engine:
+
+/* --------------------------------------------------------------------
+   Float value parse.
+*/
+  ; limit 'prec'
+	cpi	prec, 8
+	brlo	1f
+	ldi	prec, 7
+1:
+  ; init.
+	clr	flag
+	X_movw	XL, buf_lo
+  ; val_hhi := exponent, sign test and remove
+#if  FTOA_MINUS != 1
+#  error  FTOA_MINUS must be 1:  add with carry used
+#endif
+	lsl	val_hhi
+	adc	flag, zero		; FTOA_MINUS
+	sbrc	val_hlo, 7
+	ori	val_hhi, 1
+  ; zero test
+	adiw	val_hlo, 0
+	cpc	val_lo, zero
+	cpc	val_hi, zero
+	brne	3f
+  ; return 0
+	ori	flag, FTOA_ZERO
+	subi	prec, -2
+2:	st	X+, flag
+	ldi	flag, '0'
+	dec	prec
+	brne	2b
+	ret				; r24,r25 == 0
+3:
+  ; infinity, NaN ?
+#if  FTOA_NAN != 2 * FTOA_INF
+#  error  Must: FTOA_NAN == 2*FTOA_INF: 'rjmp' is absent
+#endif
+	cpi	val_hhi, 0xff
+	brlo	6f
+	cpi	val_hlo, 0x80
+	cpc	val_hi, zero
+	cpc	val_lo, zero
+	breq	5f
+	subi	flag, -FTOA_INF		; FTOA_NAN
+5:	subi	flag, -FTOA_INF
+6:
+  ; write flags byte
+	st	X+, flag
+  ; hidden bit
+	cpi	val_hhi, 1
+	brlo	7f			; if subnormal value
+	ori	val_hlo, 0x80
+7:	adc	val_hhi, zero
+  ; pushes
+	push	r29
+	push	r28
+	push	r17
+	push	r16
+	push	r15
+	push	r14
+
+/* --------------------------------------------------------------------
+   Multiplication of mantisses (val and table).
+   At the begin:
+	val_hlo .. val_lo  - input value mantisse
+	val_hhi            - input value exponent
+	X                  - second byte address (string begin)
+   At the end:
+	mlt_7 .. mlt_2     - multiplication result
+	exp10              - decimal exponent
+*/
+
+  ; save
+	mov	exp_sv, val_hhi
+  ; Z := & base10[exp / 8]	(sizeof(base10[0]) == 5)
+	andi	val_hhi, ~7
+	lsr	val_hhi			; (exp/8) * 4
+	mov	ZL, val_hhi
+	lsr	val_hhi
+	lsr	val_hhi			; exp/8
+	add	ZL, val_hhi		; (exp/8) * 5
+	clr	ZH
+	subi	ZL, lo8(-(.L_base10))
+	sbci	ZH, hi8(-(.L_base10))
+  ; highest mantissa byte  (mult. shifting prepare)
+	clr	val_hhi
+  ; result initializ.
+	clr	mlt_1
+	clr	mlt_2
+	clr	mlt_3
+	X_movw	mlt_4, mlt_2
+	X_movw	mlt_6, mlt_2
+
+  ; multiply to 1-st table byte
+#if  AVR_ENH_LPM
+	lpm	r0, Z+
+#else
+	lpm
+	adiw	ZL, 1
+#endif
+	sec			; for loop end control
+	ror	r0
+  ; addition
+10:	brcc	11f
+	add	mlt_1, val_lo
+	adc	mlt_2, val_hi
+	adc	mlt_3, val_hlo
+	adc	mlt_4, val_hhi
+	adc	mlt_5, zero
+  ; arg shift
+11:	lsl	val_lo
+	rol	val_hi
+	rol	val_hlo
+	rol	val_hhi
+  ; next bit
+	lsr	r0
+	brne	10b
+
+  ; second table byte
+#if  AVR_ENH_LPM
+	lpm	r0, Z+		; C flag is stay 1
+#else
+	lpm
+	adiw	ZL, 1
+	sec
+#endif
+	ror	r0
+  ; addition
+12:	brcc	13f
+	add	mlt_2, val_hi		; val_hi is the least byte now
+	adc	mlt_3, val_hlo
+	adc	mlt_4, val_hhi
+	adc	mlt_5, val_lo
+	adc	mlt_6, zero
+  ; arg shift
+13:	lsl	val_hi
+	rol	val_hlo
+	rol	val_hhi
+	rol	val_lo
+  ; next bit
+	lsr	r0
+	brne	12b
+
+  ; 3-t table byte
+#if  AVR_ENH_LPM
+	lpm	r0, Z+		; C flag is stay 1
+#else
+	lpm
+	adiw	ZL, 1
+	sec
+#endif
+	ror	r0
+  ; addition
+14:	brcc	15f
+	add	mlt_3, val_hlo		; val_hlo is the least byte now
+	adc	mlt_4, val_hhi
+	adc	mlt_5, val_lo
+	adc	mlt_6, val_hi
+	adc	mlt_7, zero
+  ; arg shift
+15:	lsl	val_hlo
+	rol	val_hhi
+	rol	val_lo
+	rol	val_hi
+  ; next bit
+	lsr	r0
+	brne	14b
+
+  ; 4-t table byte
+#if  AVR_ENH_LPM
+	lpm	r0, Z+		; C flag is stay 1
+#else
+	lpm
+#endif
+	ror	r0
+  ; addition
+16:	brcc	17f
+	add	mlt_4, val_hhi		; val_hhi is the least byte now
+	adc	mlt_5, val_lo
+	adc	mlt_6, val_hi
+	adc	mlt_7, val_hlo
+  ; arg shift
+17:	lsl	val_hhi
+	rol	val_lo
+	rol	val_hi
+	rol	val_hlo
+  ; next bit
+	lsr	r0
+	brne	16b
+
+  ; decimal exponent
+#if  AVR_ENH_LPM
+	lpm	exp10, Z
+#else
+	adiw	ZL, 1
+	lpm
+	mov	exp10, r0
+#endif
+
+  ; result shift:  mlt_7..2 >>= (~exp & 7)
+	com	exp_sv
+	andi	exp_sv, 7
+	breq	19f
+18:	lsr	mlt_7
+	ror	mlt_6
+	ror	mlt_5
+	ror	mlt_4
+	ror	mlt_3
+	ror	mlt_2
+	dec	exp_sv
+	brne	18b
+19:
+
+/* --------------------------------------------------------------------
+   Conversion to string.
+
+   Registers usage:
+      mlt_7 .. mlt_2	- new mantissa (multiplication result)
+      pwr_7 .. pwr_2	- 'powr10' table element
+      Z			- 'powr10' table pointer
+      X			- output string pointer
+      maxdgs		- number of digits
+      prec		- number of digits stays to output
+      exp10		- decimal exponent
+      digit		- conversion process
+
+   At the end:
+      X			- end of buffer (nonfilled byte)
+      exp10		- corrected dec. exponent
+      mlt_7 .. mlt_2	- remainder
+      pwr_7 .. pwr_2	- last powr10[] element
+
+   Notes:
+     * It is possible to leave out powr10'x table with subnormal value.
+      Result: accuracy degrease on the rounding phase.  No matter: high
+      precision with subnormals is not needed. (Now 0x00000001 is converted
+      exactly on prec = 5, i.e. 6 digits.)
+*/
+
+  ; to find first digit
+	ldi	ZL, lo8(.L_powr10)
+	ldi	ZH, hi8(.L_powr10)
+	set
+  ; 'pwr10' element reading
+.L_digit:
+	X_lpm	pwr_2, Z+
+	X_lpm	pwr_3, Z+
+	X_lpm	pwr_4, Z+
+	X_lpm	pwr_5, Z+
+	X_lpm	pwr_6, Z+
+	X_lpm	pwr_7, Z+
+  ; 'digit' init.
+	ldi	digit, '0' - 1
+  ; subtraction loop
+20:	inc	digit
+	sub	mlt_2, pwr_2
+	sbc	mlt_3, pwr_3
+	sbc	mlt_4, pwr_4
+	sbc	mlt_5, pwr_5
+	sbc	mlt_6, pwr_6
+	sbc	mlt_7, pwr_7
+	brsh	20b
+  ; restore mult
+	add	mlt_2, pwr_2
+	adc	mlt_3, pwr_3
+	adc	mlt_4, pwr_4
+	adc	mlt_5, pwr_5
+	adc	mlt_6, pwr_6
+	adc	mlt_7, pwr_7
+  ; analisys
+	brtc	25f
+	cpi	digit, '0'
+	brne	21f		; this is the first digit finded
+	dec	exp10
+	rjmp	.L_digit
+  ; now is the first digit
+21:	clt
+  ; number of digits
+	subi	maxdgs, 1
+	brlo	23f			; maxdgs was 0
+	add	maxdgs, exp10
+	brpl	22f
+	clr	maxdgs
+22:	cp	maxdgs, prec
+	brsh	23f
+	mov	prec, maxdgs
+23:	inc	prec
+	mov	maxdgs, prec	
+  ; operate digit
+25:	cpi	digit, '0' + 10
+	brlo	27f
+  ; overflow, digit > '9'
+	ldi	digit, '9'
+26:	st	X+, digit
+	dec	prec
+	brne	26b
+	rjmp	.L_up
+  ; write digit
+27:	st	X+, digit
+	dec	prec
+	brne	.L_digit
+
+/* --------------------------------------------------------------------
+    Rounding.
+*/
+.L_round:
+  ; pwr10 /= 2
+	lsr	pwr_7
+	ror	pwr_6
+	ror	pwr_5
+	ror	pwr_4
+	ror	pwr_3
+	ror	pwr_2
+  ; mult -= pwr10  (half of last 'pwr10' value)
+	sub	mlt_2, pwr_2
+	sbc	mlt_3, pwr_3
+	sbc	mlt_4, pwr_4
+	sbc	mlt_5, pwr_5
+	sbc	mlt_6, pwr_6
+	sbc	mlt_7, pwr_7
+  ; rounding direction?
+	brlo	.L_rest
+  ; round to up
+.L_up:
+	inc	prec
+	ld	digit, -X
+	inc	digit
+	cpi	digit, '9' + 1
+	brlo	31f
+	ldi	digit, '0'
+31:	st	X, digit
+	cpse	prec, maxdgs
+	brsh	.L_up
+  ; it was a carry to master digit
+	ld	digit, -X		; flags
+	ori	digit, FTOA_CARRY	; 'C' is not changed
+	st	X+, digit
+	brlo	.L_rest			; above comparison
+  ; overflow
+	inc	exp10
+	ldi	digit, '1'
+32:	st	X+, digit
+	ldi	digit, '0'
+	dec	prec
+	brne	32b
+  ; restore
+.L_rest:
+	clr	zero
+	pop	r14
+	pop	r15
+	pop	r16
+	pop	r17
+	pop	r28
+	pop	r29
+  ; return
+	clr	r25
+	sbrc	exp10, 7		; high byte
+	com	r25
+	ret
+
+    .size  __ftoa_engine, . - __ftoa_engine
+
+/* --------------------------------------------------------------------
+    Tables.  '.L_powr10' is placed first -- for subnormals stability.
+*/
+    .section .progmem.data,"a",@progbits
+
+    .type .L_powr10, "object"
+.L_powr10:
+	.byte	0, 64, 122, 16, 243, 90	; 100000000000000
+	.byte	0, 160, 114, 78, 24, 9	; 10000000000000
+	.byte	0, 16, 165, 212, 232, 0	; 1000000000000
+	.byte	0, 232, 118, 72, 23, 0	; 100000000000
+	.byte	0, 228, 11, 84, 2, 0	; 10000000000
+	.byte	0, 202, 154, 59, 0, 0	; 1000000000
+	.byte	0, 225, 245, 5, 0, 0	; 100000000
+	.byte	128, 150, 152, 0, 0, 0	; 10000000
+	.byte	64, 66, 15, 0, 0, 0	; 1000000
+	.byte	160, 134, 1, 0, 0, 0	; 100000
+	.byte	16, 39, 0, 0, 0, 0	; 10000
+	.byte	232, 3, 0, 0, 0, 0	; 1000
+	.byte	100, 0, 0, 0, 0, 0	; 100
+	.byte	10, 0, 0, 0, 0, 0	; 10
+	.byte	1, 0, 0, 0, 0, 0	; 1
+    .size .L_powr10, . - .L_powr10
+
+    .type	.L_base10, "object"
+.L_base10:
+	.byte	44, 118, 216, 136, -36	; 2295887404
+	.byte	103, 79, 8, 35, -33	; 587747175
+	.byte	193, 223, 174, 89, -31	; 1504632769
+	.byte	177, 183, 150, 229, -29	; 3851859889
+	.byte	228, 83, 198, 58, -26	; 986076132
+	.byte	81, 153, 118, 150, -24	; 2524354897
+	.byte	230, 194, 132, 38, -21	; 646234854
+	.byte	137, 140, 155, 98, -19	; 1654361225
+	.byte	64, 124, 111, 252, -17	; 4235164736
+	.byte	188, 156, 159, 64, -14	; 1084202172
+	.byte	186, 165, 111, 165, -12	; 2775557562
+	.byte	144, 5, 90, 42, -9	; 710542736
+	.byte	92, 147, 107, 108, -7	; 1818989404
+	.byte	103, 109, 193, 27, -4	; 465661287
+	.byte	224, 228, 13, 71, -2	; 1192092896
+	.byte	245, 32, 230, 181, 0	; 3051757813
+	.byte	208, 237, 144, 46, 3	; 781250000
+	.byte	0, 148, 53, 119, 5	; 2000000000
+	.byte	0, 128, 132, 30, 8	; 512000000
+	.byte	0, 0, 32, 78, 10	; 1310720000
+	.byte	0, 0, 0, 200, 12	; 3355443200
+	.byte	51, 51, 51, 51, 15	; 858993459
+	.byte	152, 110, 18, 131, 17	; 2199023256
+	.byte	65, 239, 141, 33, 20	; 562949953
+	.byte	137, 59, 230, 85, 22	; 1441151881
+	.byte	207, 254, 230, 219, 24	; 3689348815
+	.byte	209, 132, 75, 56, 27	; 944473297
+	.byte	247, 124, 29, 144, 29	; 2417851639
+	.byte	164, 187, 228, 36, 32	; 618970020
+	.byte	50, 132, 114, 94, 34	; 1584563250
+	.byte	129, 0, 201, 241, 36	; 4056481921
+	.byte	236, 161, 229, 61, 39	; 1038459372
+    .size .L_base10, . - .L_base10
+
+	.end
+#endif	/* !__DOXYGEN__ */

--- /dev/null
+++ b/libraries/FastSerial/ftoa_engine.h
@@ -1,1 +1,49 @@
+/* Copyright (c) 2005, Dmitry Xmelkov
+   All rights reserved.
 
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/* $Id: ftoa_engine.h 1218 2007-02-18 13:18:41Z dmix $ */
+
+#ifndef	_FTOA_ENGINE_H
+#define	_FTOA_ENGINE_H
+
+#ifndef	__ASSEMBLER__
+
+int __ftoa_engine (double val, char *buf,
+                   unsigned char prec, unsigned char maxdgs);
+
+#endif
+
+/* '__ftoa_engine' return next flags (in buf[0]):	*/
+#define	FTOA_MINUS	1
+#define	FTOA_ZERO	2
+#define	FTOA_INF	4
+#define	FTOA_NAN	8
+#define	FTOA_CARRY	16	/* Carry was to master position.	*/
+
+#endif	/* !_FTOA_ENGINE_H */
+

--- /dev/null
+++ b/libraries/FastSerial/keywords.txt
@@ -1,1 +1,8 @@
+FastSerial	KEYWORD1
+begin		KEYWORD2
+end		KEYWORD2
+available	KEYWORD2
+read		KEYWORD2
+flush		KEYWORD2
+write		KEYWORD2
 

--- /dev/null
+++ b/libraries/FastSerial/macros.inc
@@ -1,1 +1,366 @@
-
+/* Copyright (c) 2002, 2005, 2006, 2007 Marek Michalkiewicz
+   Copyright (c) 2006 Dmitry Xmelkov
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/*
+   macros.inc - macros for use in assembler sources
+
+   Contributors:
+     Created by Marek Michalkiewicz <marekm@linux.org.pl>
+ */
+
+#include <avr/io.h>
+//#include "sectionname.h"
+
+/* if not defined, assume old version with underscores */
+#ifndef __USER_LABEL_PREFIX__
+#define __USER_LABEL_PREFIX__ _
+#endif
+
+#ifndef __REGISTER_PREFIX__
+#define __REGISTER_PREFIX__
+#endif
+
+/* the assembler line separator (just in case it ever changes) */
+#define _L $
+
+#define CONCAT1(a, b) CONCAT2(a, b)
+#define CONCAT2(a, b) a ## b
+
+#define _U(x) CONCAT1(__USER_LABEL_PREFIX__, x)
+
+#define _R(x) CONCAT1(__REGISTER_PREFIX__, x)
+
+/* these should help to fix the "can't have function named r1()" bug
+   which may require adding '%' in front of register names.  */
+
+#define r0 _R(r0)
+#define r1 _R(r1)
+#define r2 _R(r2)
+#define r3 _R(r3)
+#define r4 _R(r4)
+#define r5 _R(r5)
+#define r6 _R(r6)
+#define r7 _R(r7)
+#define r8 _R(r8)
+#define r9 _R(r9)
+#define r10 _R(r10)
+#define r11 _R(r11)
+#define r12 _R(r12)
+#define r13 _R(r13)
+#define r14 _R(r14)
+#define r15 _R(r15)
+#define r16 _R(r16)
+#define r17 _R(r17)
+#define r18 _R(r18)
+#define r19 _R(r19)
+#define r20 _R(r20)
+#define r21 _R(r21)
+#define r22 _R(r22)
+#define r23 _R(r23)
+#define r24 _R(r24)
+#define r25 _R(r25)
+#define r26 _R(r26)
+#define r27 _R(r27)
+#define r28 _R(r28)
+#define r29 _R(r29)
+#define r30 _R(r30)
+#define r31 _R(r31)
+
+#ifndef __tmp_reg__
+#define __tmp_reg__ r0
+#endif
+
+#ifndef __zero_reg__
+#define __zero_reg__ r1
+#endif
+
+#if __AVR_MEGA__
+  #define XJMP jmp
+  #define XCALL call
+#else
+  #define XJMP rjmp
+  #define XCALL rcall
+#endif
+
+/* used only by fplib/strtod.S - libgcc internal function calls */
+#define PROLOGUE_SAVES(offset) XJMP (__prologue_saves__ + 2 * (offset))
+#define EPILOGUE_RESTORES(offset) XJMP (__epilogue_restores__ + 2 * (offset))
+
+#if FLASHEND > 0x10000  /* ATmega103 */
+  #define BIG_CODE 1
+#else
+  #define BIG_CODE 0
+#endif
+
+#ifndef __AVR_HAVE_MOVW__
+#  if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
+#   define __AVR_HAVE_MOVW__ 1
+#  endif
+#endif
+
+#ifndef __AVR_HAVE_LPMX__
+# if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
+#  define __AVR_HAVE_LPMX__ 1
+# endif
+#endif
+
+#ifndef __AVR_HAVE_MUL__
+# if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
+#  define __AVR_HAVE_MUL__ 1
+# endif
+#endif
+
+/*
+   Smart version of movw:
+    - uses "movw" if possible (supported by MCU, and both registers even)
+    - handles overlapping register pairs correctly
+    - no instruction generated if source and destination are the same
+   (may expand to 0, 1 or 2 instructions).
+ */
+
+.macro  X_movw dst src
+	.L_movw_dst = -1
+	.L_movw_src = -1
+	.L_movw_n = 0
+	.irp  reg,	r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \
+			r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \
+			r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \
+			r30,r31
+		.ifc  \reg,\dst
+			.L_movw_dst = .L_movw_n
+		.endif
+		.ifc  \reg,\src
+			.L_movw_src = .L_movw_n
+		.endif
+		.L_movw_n = .L_movw_n + 1
+	.endr
+	.L_movw_n = 0
+	.irp  reg,	R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \
+			R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \
+			R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \
+			R30,R31
+		.ifc  \reg,\dst
+			.L_movw_dst = .L_movw_n
+		.endif
+		.ifc  \reg,\src
+			.L_movw_src = .L_movw_n
+		.endif
+		.L_movw_n = .L_movw_n + 1
+	.endr
+	.if   .L_movw_dst < 0
+		.L_movw_n = 0
+		.rept   32
+			.if \dst == .L_movw_n
+				.L_movw_dst = .L_movw_n
+			.endif
+			.L_movw_n = .L_movw_n + 1
+		.endr
+	.endif
+	.if   .L_movw_src < 0
+		.L_movw_n = 0
+		.rept   32
+			.if \src == .L_movw_n
+				.L_movw_src = .L_movw_n
+			.endif
+			.L_movw_n = .L_movw_n + 1
+		.endr
+	.endif
+	.if   (.L_movw_dst < 0) || (.L_movw_src < 0)
+		.err    ; Invalid 'X_movw' arg.
+	.endif
+                
+	.if ((.L_movw_src) - (.L_movw_dst))  /* different registers */
+		.if (((.L_movw_src) | (.L_movw_dst)) & 0x01)
+			.if (((.L_movw_src)-(.L_movw_dst)) & 0x80) /* src < dest */
+				mov     (.L_movw_dst)+1, (.L_movw_src)+1
+				mov     (.L_movw_dst), (.L_movw_src)
+			.else                                      /* src > dest */
+				mov     (.L_movw_dst), (.L_movw_src)
+				mov     (.L_movw_dst)+1, (.L_movw_src)+1
+			.endif
+		.else  /* both even -> overlap not possible */
+#if  defined(__AVR_HAVE_MOVW__) && __AVR_HAVE_MOVW__
+			movw    \dst, \src
+#else
+			mov     (.L_movw_dst), (.L_movw_src)
+			mov     (.L_movw_dst)+1, (.L_movw_src)+1
+#endif
+		.endif
+	.endif
+.endm
+
+/* Macro 'X_lpm' extends enhanced lpm instruction for classic chips.
+   Usage:
+	X_lpm	reg, dst
+   where
+	reg	is 0..31, r0..r31 or R0..R31
+	dst	is z, Z, z+ or Z+
+   It is possible to omit both arguments.
+
+   Possible results for classic chips:
+	lpm
+	lpm / mov Rd,r0
+	lpm / adiw ZL,1
+	lpm / mov Rd,r0 / adiw ZL,1
+	
+   For enhanced chips it is one instruction always.
+
+   ATTENTION:  unlike enhanced chips SREG (S,V,N,Z,C) flags are
+   changed in case of 'Z+' dst.  R0 is scratch.
+ */
+.macro	X_lpm	dst=r0, src=Z
+
+  /* dst evaluation	*/
+  .L_lpm_dst = -1
+
+  .L_lpm_n = 0
+  .irp  reg,  r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \
+	     r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \
+	     r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \
+	     r30,r31
+    .ifc  \reg,\dst
+      .L_lpm_dst = .L_lpm_n
+    .endif
+    .L_lpm_n = .L_lpm_n + 1
+  .endr
+
+  .L_lpm_n = 0
+  .irp  reg,  R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \
+	     R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \
+	     R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \
+	     R30,R31
+    .ifc  \reg,\dst
+      .L_lpm_dst = .L_lpm_n
+    .endif
+    .L_lpm_n = .L_lpm_n + 1
+  .endr
+
+  .if  .L_lpm_dst < 0
+    .L_lpm_n = 0
+    .rept 32
+      .if  \dst == .L_lpm_n
+	.L_lpm_dst = .L_lpm_n
+      .endif
+      .L_lpm_n = .L_lpm_n + 1
+    .endr
+  .endif
+
+  .if  (.L_lpm_dst < 0)
+    .err	; Invalid dst arg of 'X_lpm' macro.
+  .endif
+
+  /* src evaluation	*/    
+  .L_lpm_src = -1
+  .L_lpm_n = 0
+  .irp  reg,  z,Z,z+,Z+
+    .ifc  \reg,\src
+      .L_lpm_src = .L_lpm_n
+    .endif
+    .L_lpm_n = .L_lpm_n + 1
+  .endr
+
+  .if  (.L_lpm_src < 0)
+    .err	; Invalid src arg of 'X_lpm' macro.
+  .endif
+
+  /* instruction(s)	*/    
+  .if  .L_lpm_src < 2
+    .if  .L_lpm_dst == 0
+	lpm
+    .else
+#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
+	lpm	.L_lpm_dst, Z
+#else
+	lpm
+	mov	.L_lpm_dst, r0
+#endif
+    .endif
+  .else
+    .if  (.L_lpm_dst >= 30)
+      .err	; Registers 30 and 31 are inhibited as 'X_lpm *,Z+' dst.
+    .endif
+#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
+	lpm	.L_lpm_dst, Z+
+#else
+	lpm
+    .if  .L_lpm_dst
+	mov	.L_lpm_dst, r0
+    .endif
+	adiw	r30, 1
+#endif
+  .endif
+.endm
+
+/*
+   LPM_R0_ZPLUS_INIT is used before the loop to initialize RAMPZ
+   for future devices with RAMPZ:Z auto-increment - [e]lpm r0, Z+.
+
+   LPM_R0_ZPLUS_NEXT is used inside the loop to load a byte from
+   the program memory at [RAMPZ:]Z to R0, and increment [RAMPZ:]Z.
+
+   The argument in both macros is a register that contains the
+   high byte (bits 23-16) of the address, bits 15-0 should be in
+   the Z (r31:r30) register.  It can be any register except for:
+   r0, r1 (__zero_reg__ - assumed to always contain 0), r30, r31.
+ */
+
+	.macro	LPM_R0_ZPLUS_INIT hhi
+#if __AVR_ENHANCED__
+  #if BIG_CODE
+	out	AVR_RAMPZ_ADDR, \hhi
+  #endif
+#endif
+	.endm
+
+	.macro	LPM_R0_ZPLUS_NEXT hhi
+#if __AVR_ENHANCED__
+  #if BIG_CODE
+    /* ELPM with RAMPZ:Z post-increment, load RAMPZ only once */
+	elpm	r0, Z+
+  #else
+    /* LPM with Z post-increment, max 64K, no RAMPZ (ATmega83/161/163/32) */
+	lpm	r0, Z+
+  #endif
+#else
+  #if BIG_CODE
+    /* ELPM without post-increment, load RAMPZ each time (ATmega103) */
+	out	AVR_RAMPZ_ADDR, \hhi
+	elpm
+	adiw	r30,1
+	adc	\hhi, __zero_reg__
+  #else
+    /* LPM without post-increment, max 64K, no RAMPZ (AT90S*) */
+	lpm
+	adiw	r30,1
+  #endif
+#endif
+	.endm
+

--- /dev/null
+++ b/libraries/FastSerial/ntz.h
@@ -1,1 +1,55 @@
+/* Copyright (c) 2007, Dmitry Xmelkov
+   All rights reserved.
 
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/* $Id: ntz.h 1217 2007-02-18 13:18:05Z dmix $	*/
+
+#ifndef	_NTZ_H
+#define	_NTZ_H
+
+/* Number of Tail Zeros:  ntz(x)= (ffs(x) ? ffs(x)-1 : 16)
+   It works with all: cpp, gcc and gas expressions.	*/
+#define ntz(x)	\
+	( (1 & (((x) & 1) == 0))        \
+	+ (1 & (((x) & 3) == 0))        \
+    	+ (1 & (((x) & 7) == 0))        \
+        + (1 & (((x) & 017) == 0))      \
+        + (1 & (((x) & 037) == 0))      \
+	+ (1 & (((x) & 077) == 0))      \
+	+ (1 & (((x) & 0177) == 0))     \
+	+ (1 & (((x) & 0377) == 0))     \
+	+ (1 & (((x) & 0777) == 0))     \
+	+ (1 & (((x) & 01777) == 0))    \
+	+ (1 & (((x) & 03777) == 0))    \
+	+ (1 & (((x) & 07777) == 0))    \
+	+ (1 & (((x) & 017777) == 0))   \
+	+ (1 & (((x) & 037777) == 0))   \
+	+ (1 & (((x) & 077777) == 0))   \
+	+ (1 & (((x) & 0177777) == 0)) )
+
+#endif	/* !_NTZ_H */
+

--- /dev/null
+++ b/libraries/FastSerial/ultoa_invert.S
@@ -1,1 +1,217 @@
-
+/* Copyright (c) 2005,2007  Dmitry Xmelkov
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $	*/
+
+#ifndef	__DOXYGEN__
+
+#include "macros.inc"
+#include "ntz.h"
+#include "xtoa_fast.h"
+
+/* --------------------------------------------------------------------
+   char * __ultoa_invert (unsigned long val, char * str, int base)
+
+   This function is intended for usage as internal printf's one.
+   It differs from others of `xtoa_fast' family:
+       * srt[] will NOT 0 terminated.
+       * Sequence of digits is inverted.
+       * It returns pointer to first byte after a string.
+       * Only `XTOA_UPPER' flag is operated.
+   Notes:
+       * base: check only 8 and 16, all others are treated as 10.
+       (internal printf's function).
+*/
+
+    /* Input	*/
+#define v_lo	r22
+#define	v_hi	r23
+#define	v_hlo	r24
+#define	v_hhi	r25
+#define	str_lo	r20
+#define	str_hi	r21
+#define	base	r18
+#define	flags	r19
+
+    /* Used	*/
+#define	v_fifth	r26	/* val: bits 39..32			*/
+#define	t_lo	r18	/* temporary for shifted `val'		*/
+#define	t_hi	r19
+#define	t_hlo	r20
+#define	t_hhi	r21
+#define	symb	r20	/* write to string			*/
+#define	cnt	r27	/* shift loop counter, local arg	*/
+
+    /* Fixed	*/
+#define	rzero	r1
+
+/*	ASSEMBLY_CLIB_SECTION */
+	.global	__ultoa_invert
+	.type	__ultoa_invert, "function"
+
+__ultoa_invert:
+	X_movw	ZL, str_lo
+	clr	v_fifth			; needed for all (ultoa_lsr)
+	cpi	base, 8
+	breq	.L_oct
+	cpi	base, 16
+	breq	.L_hex
+
+  ; decimal format
+	clt				; flag of val == 0
+.L_dec_loop:
+	push	v_lo			; to calculate remander
+  ; val &= ~1
+	andi	v_lo, ~1
+  ; val += 2
+	subi	v_lo, lo8(-2)
+	sbci	v_hi, hi8(-2)
+	sbci	v_hlo, hlo8(-2)
+	sbci	v_hhi, hhi8(-2)
+	sbci	v_fifth, hhi8(-2)
+  ; val += val/2
+	ldi	cnt, 1
+	rcall	.L_div_add
+  ; val += val/16
+	ldi	cnt, 4
+	rcall	.L_div_add
+  ; val += val/256
+	add	v_lo, v_hi
+	adc	v_hi, v_hlo
+	adc	v_hlo, v_hhi
+	adc	v_hhi, v_fifth
+	adc	v_fifth, rzero
+  ; val += val/65536
+	add	v_lo, v_hlo
+	adc	v_hi, v_hhi
+	adc	v_hlo, v_fifth
+	adc	v_hhi, rzero
+	adc	v_fifth, rzero
+  ; val += val >> 32
+	add	v_lo, v_fifth
+	adc	v_hi, rzero
+	adc	v_hlo, rzero
+	adc	v_hhi, rzero
+	adc	v_fifth, rzero
+  ; division result:  val /= 16
+	rcall	.L_lsr_4		; v_fitth := 0
+	brne	1f
+	set				; T := Z flag
+1:
+  ; rem:  val_original - 10*val
+	pop	t_hi
+#if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
+	ldi	t_lo, 10
+	mul	t_lo, v_lo
+	clr	r1
+#else
+	mov	r0, v_lo
+	lsl	r0
+	sub	t_hi, r0
+	lsl	r0
+	lsl	r0
+#endif
+	sub	t_hi, r0
+  ; output digit
+	subi	t_hi, lo8(-'0')
+	st	Z+, t_hi
+  ; quotient == 0 ?
+	brtc	.L_dec_loop
+  ; end of string
+.L_eos:
+	X_movw	r24, ZL
+	ret
+
+  ; octal format
+.L_oct:
+	mov	symb, v_lo
+	andi	symb, 7
+	subi	symb, lo8(-'0')
+	st	Z+, symb
+	ldi	cnt, 3
+	rcall	.L_lsr
+	brne	.L_oct
+	rjmp	.L_eos
+
+  ; hex format
+.L_hex:
+	mov	symb, v_lo
+	andi	symb, 0x0f
+	subi	symb, lo8(-'0')
+	cpi	symb, '9' + 1
+	brlo	3f
+	subi	symb, lo8('9' + 1 - 'a')
+	sbrc	flags, ntz(XTOA_UPPER) - 8
+	subi	symb, lo8('a' - 'A')
+3:	st	Z+, symb
+	rcall	.L_lsr_4
+	brne	.L_hex
+	rjmp	.L_eos
+	
+.L_lsr_4:
+	ldi	cnt, 4
+.L_lsr:
+	lsr	v_fifth
+	ror	v_hhi
+	ror	v_hlo
+	ror	v_hi
+	ror	v_lo
+	dec	cnt
+	brne	.L_lsr
+  ; tst
+	sbiw	v_hlo, 0		; only Z flag is needed
+	cpc	v_lo, rzero
+	cpc	v_hi, rzero
+	ret
+
+.L_div_add:
+  ; copy to temporary
+	X_movw	t_lo, v_lo
+	X_movw	t_hlo, v_hlo
+	mov	r0, v_fifth
+  ; lsr temporary
+7:	lsr	r0
+	ror	t_hhi
+	ror	t_hlo
+	ror	t_hi
+	ror	t_lo
+	dec	cnt
+	brne	7b
+  ; add
+	add	v_lo, t_lo
+	adc	v_hi, t_hi
+	adc	v_hlo, t_hlo
+	adc	v_hhi, t_hhi
+	adc	v_fifth, r0		; here r0 == 0
+	ret
+
+	.size	__ultoa_invert, . - __ultoa_invert
+	.end
+
+#endif	/* !__DOXYGEN__ */
+

--- /dev/null
+++ b/libraries/FastSerial/vprintf.cpp
@@ -1,1 +1,522 @@
-
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
+/*
+   Adapted from the avr-libc vfprintf:
+
+   Copyright (c) 2002, Alexander Popov (sasho@vip.bg)
+   Copyright (c) 2002,2004,2005 Joerg Wunsch
+   Copyright (c) 2005, Helmut Wallner
+   Copyright (c) 2007, Dmitry Xmelkov
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in
+   the documentation and/or other materials provided with the
+   distribution.
+   * Neither the name of the copyright holders nor the names of
+   contributors may be used to endorse or promote products derived
+   from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */
+/* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */
+
+#include "BetterStream.h"
+
+#include <avr/pgmspace.h>
+#include <stdarg.h>
+#include <string.h>
+extern "C" {
+#include "ftoa_engine.h"
+#include "ntz.h"
+#include "xtoa_fast.h"
+}
+
+// workaround for GCC bug c++/34734
+#undef PROGMEM 
+#define PROGMEM __attribute__(( section(".progmem.data") )) 
+#undef PSTR 
+#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 
+
+#define GETBYTE(flag, mask, pnt)        ({                              \
+                        unsigned char __c;                              \
+                        asm (                                           \
+                             "sbrc      %2,%3   \n\t"                   \
+                             "lpm       %0,Z+   \n\t"                   \
+                             "sbrs      %2,%3   \n\t"                   \
+                             "ld        %0,Z+   "                       \
+                             : "=r" (__c),                              \
+                               "+z" (pnt)                               \
+                             : "r" (flag),                              \
+                               "I" (ntz(mask))                          \
+                        );                                              \
+                        __c;                                            \
+                })
+/*
+#define GETBYTE(flag, mask, pnt)        ({      \
+       unsigned char __c;                       \
+       __c = ((flag) & (mask))                  \
+             ? pgm_read_byte(pnt) : *pnt;       \
+       pnt++;                                   \
+       __c;                                     \
+})
+*/
+
+#define FL_ZFILL	0x01
+#define FL_PLUS		0x02
+#define FL_SPACE	0x04
+#define FL_LPAD		0x08
+#define FL_ALT		0x10
+#define FL_WIDTH	0x20
+#define FL_PREC		0x40
+#define FL_LONG		0x80
+
+#define FL_PGMSTRING	FL_LONG
+#define FL_NEGATIVE	FL_LONG
+
+#define FL_ALTUPP	FL_PLUS
+#define FL_ALTHEX	FL_SPACE
+
+#define	FL_FLTUPP	FL_ALT
+#define FL_FLTEXP	FL_PREC
+#define	FL_FLTFIX	FL_LONG
+
+void
+BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
+{
+        unsigned char c;        /* holds a char from the format string */
+        unsigned char flags;
+        unsigned char width;
+        unsigned char prec;
+        unsigned char buf[11];  /* size for -1 in octal, without '\0'   */
+
+        for (;;) {
+
+                /*
+                 * Process non-format characters
+                 */
+                for (;;) {
+                        c = GETBYTE (in_progmem, 1, fmt);
+                        if (!c) return;
+                        if (c == '%') {
+                                c = GETBYTE (in_progmem, 1, fmt);
+                                if (c != '%') break;
+                        }
+                        /* emit cr before lf to make most terminals happy */
+                        if (c == '\n')
+                                write('\r');
+                        write(c);
+                }
+
+                flags = 0;
+                width = 0;
+                prec = 0;
+                
+                /*
+                 * Process format adjustment characters, precision, width.
+                 */
+                do {
+                        if (flags < FL_WIDTH) {
+                                switch (c) {
+                                case '0':
+                                        flags |= FL_ZFILL;
+                                        continue;
+                                case '+':
+                                        flags |= FL_PLUS;
+                                        /* FALLTHROUGH */
+                                case ' ':
+                                        flags |= FL_SPACE;
+                                        continue;
+                                case '-':
+                                        flags |= FL_LPAD;
+                                        continue;
+                                case '#':
+                                        flags |= FL_ALT;
+                                        continue;
+                                }
+                        }
+
+                        if (flags < FL_LONG) {
+                                if (c >= '0' && c <= '9') {
+                                        c -= '0';
+                                        if (flags & FL_PREC) {
+                                                prec = 10*prec + c;
+                                                continue;
+                                        }
+                                        width = 10*width + c;
+                                        flags |= FL_WIDTH;
+                                        continue;
+                                }
+                                if (c == '.') {
+                                        if (flags & FL_PREC)
+                                                return;
+                                        flags |= FL_PREC;
+                                        continue;
+                                }
+                                if (c == 'l') {
+                                        flags |= FL_LONG;
+                                        continue;
+                                }
+                                if (c == 'h')
+                                        continue;
+                        }
+            
+                        break;
+                } while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0);
+
+                /*
+                 * Handle floating-point formats E, F, G, e, f, g.
+                 */
+                if (c >= 'E' && c <= 'G') {
+                        flags |= FL_FLTUPP;
+                        c += 'e' - 'E';
+                        goto flt_oper;
+
+                } else if (c >= 'e' && c <= 'g') {
+
+                        int exp;                /* exponent of master decimal digit     */
+                        int n;
+                        unsigned char vtype;    /* result of float value parse  */
+                        unsigned char sign;     /* sign character (or 0)        */
+                        unsigned char ndigs;
+
+                        flags &= ~FL_FLTUPP;
+
+                flt_oper:
+                        if (!(flags & FL_PREC))
+                                prec = 6;
+                        flags &= ~(FL_FLTEXP | FL_FLTFIX);
+                        if (c == 'e')
+                                flags |= FL_FLTEXP;
+                        else if (c == 'f')
+                                flags |= FL_FLTFIX;
+                        else if (prec > 0)
+                                prec -= 1;
+
+                        if (flags & FL_FLTFIX) {
+                                vtype = 7;              /* 'prec' arg for 'ftoa_engine' */
+                                ndigs = prec < 60 ? prec + 1 : 60;
+                        } else {
+                                if (prec > 7) prec = 7;
+                                vtype = prec;
+                                ndigs = 0;
+                        }
+                        exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);
+                        vtype = buf[0];
+    
+                        sign = 0;
+                        if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))
+                                sign = '-';
+                        else if (flags & FL_PLUS)
+                                sign = '+';
+                        else if (flags & FL_SPACE)
+                                sign = ' ';
+
+                        if (vtype & (FTOA_NAN | FTOA_INF)) {
+                                const char *p;
+                                ndigs = sign ? 4 : 3;
+                                if (width > ndigs) {
+                                        width -= ndigs;
+                                        if (!(flags & FL_LPAD)) {
+                                                do {
+                                                        write(' ');
+                                                } while (--width);
+                                        }
+                                } else {
+                                        width = 0;
+                                }
+                                if (sign)
+                                        write(sign);
+                                p = PSTR("inf");
+                                if (vtype & FTOA_NAN)
+                                        p = PSTR("nan");
+                                while ( (ndigs = pgm_read_byte(p)) != 0) {
+                                        if (flags & FL_FLTUPP)
+                                                ndigs += 'I' - 'i';
+                                        write(ndigs);
+                                        p++;
+                                }
+                                goto tail;
+                        }
+
+                        /* Output format adjustment, number of decimal digits in buf[] */
+                        if (flags & FL_FLTFIX) {
+                                ndigs += exp;
+                                if ((vtype & FTOA_CARRY) && buf[1] == '1')
+                                        ndigs -= 1;
+                                if ((signed char)ndigs < 1)
+                                        ndigs = 1;
+                                else if (ndigs > 8)
+                                        ndigs = 8;
+                        } else if (!(flags & FL_FLTEXP)) {              /* 'g(G)' format */
+                                if (exp <= prec && exp >= -4)
+                                        flags |= FL_FLTFIX;
+                                while (prec && buf[1+prec] == '0')
+                                        prec--;
+                                if (flags & FL_FLTFIX) {
+                                        ndigs = prec + 1;               /* number of digits in buf */
+                                        prec = prec > exp
+                                                ? prec - exp : 0;       /* fractional part length  */
+                                }
+                        }
+    
+                        /* Conversion result length, width := free space length */
+                        if (flags & FL_FLTFIX)
+                                n = (exp>0 ? exp+1 : 1);
+                        else
+                                n = 5;          /* 1e+00 */
+                        if (sign) n += 1;
+                        if (prec) n += prec + 1;
+                        width = width > n ? width - n : 0;
+    
+                        /* Output before first digit    */
+                        if (!(flags & (FL_LPAD | FL_ZFILL))) {
+                                while (width) {
+                                        write(' ');
+                                        width--;
+                                }
+                        }
+                        if (sign) write(sign);
+                        if (!(flags & FL_LPAD)) {
+                                while (width) {
+                                        write('0');
+                                        width--;
+                                }
+                        }
+    
+                        if (flags & FL_FLTFIX) {                /* 'f' format           */
+
+                                n = exp > 0 ? exp : 0;          /* exponent of left digit */
+                                do {
+                                        if (n == -1)
+                                                write('.');
+                                        flags = (n <= exp && n > exp - ndigs)
+                                                ? buf[exp - n + 1] : '0';
+                                        if (--n < -prec)
+                                                break;
+                                        write(flags);
+                                } while (1);
+                                if (n == exp
+                                    && (buf[1] > '5'
+                                        || (buf[1] == '5' && !(vtype & FTOA_CARRY))) )
+                                        {
+                                                flags = '1';
+                                        }
+                                write(flags);
+        
+                        } else {                                /* 'e(E)' format        */
+
+                                /* mantissa     */
+                                if (buf[1] != '1')
+                                        vtype &= ~FTOA_CARRY;
+                                write(buf[1]);
+                                if (prec) {
+                                        write('.');
+                                        sign = 2;
+                                        do {
+                                                write(buf[sign++]);
+                                        } while (--prec);
+                                }
+
+                                /* exponent     */
+                                write(flags & FL_FLTUPP ? 'E' : 'e');
+                                ndigs = '+';
+                                if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {
+                                        exp = -exp;
+                                        ndigs = '-';
+                                }
+                                write(ndigs);
+                                for (ndigs = '0'; exp >= 10; exp -= 10)
+                                        ndigs += 1;
+                                write(ndigs);
+                                write('0' + exp);
+                        }
+
+                        goto tail;
+                }
+
+                /*
+                 * Handle string formats c, s, S.
+                 */
+                {
+                        const char * pnt;
+                        size_t size;
+
+                        switch (c) {
+
+                        case 'c':
+                                buf[0] = va_arg (ap, int);
+                                pnt = (char *)buf;
+                                size = 1;
+                                goto no_pgmstring;
+
+                        case 's':
+                                pnt = va_arg (ap, char *);
+                                size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0);
+                        no_pgmstring:
+                                flags &= ~FL_PGMSTRING;
+                                goto str_lpad;
+
+                        case 'S':
+                        pgmstring:
+                                pnt = va_arg (ap, char *);
+                                size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0);
+                                flags |= FL_PGMSTRING;
+
+                        str_lpad:
+                                if (!(flags & FL_LPAD)) {
+                                        while (size < width) {
+                                                write(' ');
+                                                width--;
+                                        }
+                                }
+                                while (size) {
+                                        write(GETBYTE (flags, FL_PGMSTRING, pnt));
+                                        if (width) width -= 1;
+                                        size -= 1;
+                                }
+                                goto tail;
+                        }
+                }
+
+                /*
+                 * Handle integer formats variations for d/i, u, o, p, x, X.
+                 */
+                if (c == 'd' || c == 'i') {
+                        long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int);
+                        flags &= ~(FL_NEGATIVE | FL_ALT);
+                        if (x < 0) {
+                                x = -x;
+                                flags |= FL_NEGATIVE;
+                        }
+                        c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf;
+
+                } else {
+                        int base;
+
+                        if (c == 'u') {
+                                flags &= ~FL_ALT;
+                                base = 10;
+                                goto ultoa;
+                        }
+
+                        flags &= ~(FL_PLUS | FL_SPACE);
+
+                        switch (c) {
+                        case 'o':
+                                base = 8;
+                                goto ultoa;
+                        case 'p':
+                                flags |= FL_ALT;
+                                /* no break */
+                        case 'x':
+                                if (flags & FL_ALT)
+                                        flags |= FL_ALTHEX;
+                                base = 16;
+                                goto ultoa;
+                        case 'X':
+                                if (flags & FL_ALT)
+                                        flags |= (FL_ALTHEX | FL_ALTUPP);
+                                base = 16 | XTOA_UPPER;
+                        ultoa:
+                                c = __ultoa_invert ((flags & FL_LONG)
+                                                    ? va_arg(ap, unsigned long)
+                                                    : va_arg(ap, unsigned int),
+                                                    (char *)buf, base)  -  (char *)buf;
+                                flags &= ~FL_NEGATIVE;
+                                break;
+
+                        default:
+                                return;
+                        }
+                }
+
+                /*
+                 * Format integers.
+                 */
+                {
+                        unsigned char len;
+
+                        len = c;
+                        if (flags & FL_PREC) {
+                                flags &= ~FL_ZFILL;
+                                if (len < prec) {
+                                        len = prec;
+                                        if ((flags & FL_ALT) && !(flags & FL_ALTHEX))
+                                                flags &= ~FL_ALT;
+                                }
+                        }
+                        if (flags & FL_ALT) {
+                                if (buf[c-1] == '0') {
+                                        flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP);
+                                } else {
+                                        len += 1;
+                                        if (flags & FL_ALTHEX)
+                                                len += 1;
+                                }
+                        } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
+                                len += 1;
+                        }
+
+                        if (!(flags & FL_LPAD)) {
+                                if (flags & FL_ZFILL) {
+                                        prec = c;
+                                        if (len < width) {
+                                                prec += width - len;
+                                                len = width;
+                                        }
+                                }
+                                while (len < width) {
+                                        write(' ');
+                                        len++;
+                                }
+                        }
+        
+                        width =  (len < width) ? width - len : 0;
+
+                        if (flags & FL_ALT) {
+                                write('0');
+                                if (flags & FL_ALTHEX)
+                                        write(flags & FL_ALTUPP ? 'X' : 'x');
+                        } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
+                                unsigned char z = ' ';
+                                if (flags & FL_PLUS) z = '+';
+                                if (flags & FL_NEGATIVE) z = '-';
+                                write(z);
+                        }
+                
+                        while (prec > c) {
+                                write('0');
+                                prec--;
+                        }
+        
+                        do {
+                                write(buf[--c]);
+                        } while (c);
+                }
+        
+        tail:
+                /* Tail is possible.    */
+                while (width) {
+                        write(' ');
+                        width--;
+                }
+        } /* for (;;) */
+}
+

--- /dev/null
+++ b/libraries/FastSerial/wiring.h
@@ -1,1 +1,137 @@
+/*
+  wiring.h - Partial implementation of the Wiring API for the ATmega8.
+  Part of Arduino - http://www.arduino.cc/
 
+  Copyright (c) 2005-2006 David A. Mellis
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library 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
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General
+  Public License along with this library; if not, write to the
+  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
+  Boston, MA  02111-1307  USA
+
+  $Id$
+*/
+
+#ifndef Wiring_h
+#define Wiring_h
+
+#include <avr/io.h>
+#include <stdlib.h>
+#include "binary.h"
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+
+#define HIGH 0x1
+#define LOW  0x0
+
+#define INPUT 0x0
+#define OUTPUT 0x1
+
+#define true 0x1
+#define false 0x0
+
+#define PI 3.1415926535897932384626433832795
+#define HALF_PI 1.5707963267948966192313216916398
+#define TWO_PI 6.283185307179586476925286766559
+#define DEG_TO_RAD 0.017453292519943295769236907684886
+#define RAD_TO_DEG 57.295779513082320876798154814105
+
+#define SERIAL  0x0
+#define DISPLAY 0x1
+
+#define LSBFIRST 0
+#define MSBFIRST 1
+
+#define CHANGE 1
+#define FALLING 2
+#define RISING 3
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(DESKTOP_BUILD)
+#define INTERNAL1V1 2
+#define INTERNAL2V56 3
+#else
+#define INTERNAL 3
+#endif
+#define DEFAULT 1
+#define EXTERNAL 0
+
+// undefine stdlib's abs if encountered
+#ifdef abs
+#undef abs
+#endif
+
+#define min(a,b) ((a)<(b)?(a):(b))
+#define max(a,b) ((a)>(b)?(a):(b))
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+//#define round(x)     ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
+#define radians(deg) ((deg)*DEG_TO_RAD)
+#define degrees(rad) ((rad)*RAD_TO_DEG)
+#define sq(x) ((x)*(x))
+
+#define interrupts() sei()
+#define noInterrupts() cli()
+
+#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L )
+#define clockCyclesToMicroseconds(a) ( ((a) * 1000L) / (F_CPU / 1000L) )
+#define microsecondsToClockCycles(a) ( ((a) * (F_CPU / 1000L)) / 1000L )
+
+#define lowByte(w) ((uint8_t) ((w) & 0xff))
+#define highByte(w) ((uint8_t) ((w) >> 8))
+
+#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
+#define bitSet(value, bit) ((value) |= (1UL << (bit)))
+#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
+#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))
+#define bit_is_set(value, bit) (((value) & bit) != 0)
+
+typedef unsigned int word;
+
+#define bit(b) (1UL << (b))
+
+typedef uint8_t boolean;
+typedef uint8_t byte;
+
+void init(void);
+
+void pinMode(uint8_t, uint8_t);
+void digitalWrite(uint8_t, uint8_t);
+int digitalRead(uint8_t);
+void analogReference(uint8_t mode);
+void analogWrite(uint8_t, int);
+
+unsigned long millis(void);
+unsigned long micros(void);
+void delay(unsigned long);
+void delayMicroseconds(unsigned int us);
+unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);
+
+void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val);
+uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder);
+
+void attachInterrupt(uint8_t, void (*)(void), int mode);
+void detachInterrupt(uint8_t);
+
+int abs(int v);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+void setup(void);
+void loop(void);
+long map(long , long , long , long , long );
+
+#endif
+

--- /dev/null
+++ b/libraries/FastSerial/xtoa_fast.h
@@ -1,1 +1,49 @@
+/*
+   Adapted from avr-libc:
 
+   Copyright (c) 2005, Dmitry Xmelkov
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/* $Id: xtoa_fast.h 1223 2007-02-18 13:33:09Z dmix $	*/
+
+#ifndef	_XTOA_FAST_H_
+#define _XTOA_FAST_H_
+
+#ifndef	__ASSEMBLER__
+
+/* Internal function for use from `printf'.	*/
+char * __ultoa_invert (unsigned long val, char *s, int base);
+
+#endif	/* ifndef __ASSEMBLER__ */
+
+/* Next flags are to use with `base'. Unused fields are reserved.	*/
+#define XTOA_PREFIX	0x0100	/* put prefix for octal or hex	*/
+#define XTOA_UPPER	0x0200	/* use upper case letters	*/
+
+#endif	/* _XTOA_FAST_H_ */
+

--- /dev/null
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
@@ -1,1 +1,50 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+/// @file	GCS_MAVLink.cpp

+

+/*

+This provides some support code and variables for MAVLink enabled sketches

+

+This firmware is free software; you can redistribute it and/or

+modify it under the terms of the GNU Lesser General Public

+License as published by the Free Software Foundation; either

+version 2.1 of the License, or (at your option) any later version.

+*/

+

+#include <FastSerial.h>

+#include <AP_Common.h>

+#include <GCS_MAVLink.h>

+

+

+BetterStream	*mavlink_comm_0_port;

+BetterStream	*mavlink_comm_1_port;

+

+mavlink_system_t mavlink_system = {12,1,0,0}; //modified

+

+uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)

+{

+    if (sysid != mavlink_system.sysid)

+        return 1;

+    // Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem

+    // If it is addressed to our system ID we assume it is for us

+    return 0; // no error

+}

+

+// return a MAVLink variable type given a AP_Param type

+uint8_t mav_var_type(enum ap_var_type t)

+{

+    if (t == AP_PARAM_INT8) {

+	    return MAVLINK_TYPE_INT8_T;

+    }

+    if (t == AP_PARAM_INT16) {

+	    return MAVLINK_TYPE_INT16_T;

+    }

+    if (t == AP_PARAM_INT32) {

+	    return MAVLINK_TYPE_INT32_T;

+    }

+    // treat any others as float

+    return MAVLINK_TYPE_FLOAT;

+}

+

+

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.h
@@ -1,1 +1,122 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

+

+/// @file	GCS_MAVLink.h

+/// @brief	One size fits all header for MAVLink integration.

+

+#ifndef GCS_MAVLink_h

+#define GCS_MAVLink_h

+

+#include <BetterStream.h>

+

+// we have separate helpers disabled to make it possible

+// to select MAVLink 1.0 in the arduino GUI build

+//#define MAVLINK_SEPARATE_HELPERS

+

+#include "include/mavlink/v1.0/ardupilotmega/version.h"

+

+// this allows us to make mavlink_message_t much smaller

+#define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE

+

+#define MAVLINK_COMM_NUM_BUFFERS 1

+#include "include/mavlink/v1.0/mavlink_types.h"

+

+/// MAVLink stream used for HIL interaction

+extern BetterStream	*mavlink_comm_0_port;

+

+/// MAVLink stream used for ground control communication

+extern BetterStream	*mavlink_comm_1_port;

+

+/// MAVLink system definition

+extern mavlink_system_t mavlink_system;

+

+/// Send a byte to the nominated MAVLink channel

+///

+/// @param chan		Channel to send to

+/// @param ch		Byte to send

+///

+static inline void comm_send_ch(mavlink_channel_t chan, uint8_t ch)

+{

+    switch(chan) {

+	case MAVLINK_COMM_0:

+		mavlink_comm_0_port->write(ch);

+		break;

+	case MAVLINK_COMM_1:

+		mavlink_comm_1_port->write(ch);

+		break;

+	default:

+		break;

+	}

+}

+

+/// Read a byte from the nominated MAVLink channel

+///

+/// @param chan		Channel to receive on

+/// @returns		Byte read

+///

+static inline uint8_t comm_receive_ch(mavlink_channel_t chan)

+{

+    uint8_t data = 0;

+

+    switch(chan) {

+	case MAVLINK_COMM_0:

+		data = mavlink_comm_0_port->read();

+		break;

+	case MAVLINK_COMM_1:

+		data = mavlink_comm_1_port->read();

+		break;

+	default:

+		break;

+	}

+    return data;

+}

+

+/// Check for available data on the nominated MAVLink channel

+///

+/// @param chan		Channel to check

+/// @returns		Number of bytes available

+static inline uint16_t comm_get_available(mavlink_channel_t chan)

+{

+    uint16_t bytes = 0;

+    switch(chan) {

+	case MAVLINK_COMM_0:

+		bytes = mavlink_comm_0_port->available();

+		break;

+	case MAVLINK_COMM_1:

+		bytes = mavlink_comm_1_port->available();

+		break;

+	default:

+		break;

+	}

+    return bytes;

+}

+

+

+/// Check for available transmit space on the nominated MAVLink channel

+///

+/// @param chan		Channel to check

+/// @returns		Number of bytes available, -1 for error

+static inline int comm_get_txspace(mavlink_channel_t chan)

+{

+    switch(chan) {

+	case MAVLINK_COMM_0:

+		return mavlink_comm_0_port->txspace();

+		break;

+	case MAVLINK_COMM_1:

+		return mavlink_comm_1_port->txspace();

+		break;

+	default:

+		break;

+	}

+    return -1;

+}

+

+#define MAVLINK_USE_CONVENIENCE_FUNCTIONS

+#include "include/mavlink/v1.0/ardupilotmega/mavlink.h"

+

+uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid);

+

+// return a MAVLink variable type given a AP_Param type

+uint8_t mav_var_type(enum ap_var_type t);

+

+#endif // GCS_MAVLink_h

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/generate.sh
@@ -1,1 +1,21 @@
+#!/bin/sh

+# script to re-generate mavlink C code for APM

+

+mavdir="$(dirname $0)"

+dname="$(basename $mavdir)"

+[ "$dname" = "GCS_MAVLink" ] || {

+    echo "This script should be run from the libraries/GCS_MAVLink directory"

+    exit 1

+}

+

+if ! which mavgen.py > /dev/null; then

+    echo "mavgen.py must be in your PATH. Get it from http://github.com/mavlink/mavlink in the pymavlink/generator directory"

+    exit 1

+fi

+

+echo "Removing old includes"

+rm -rf "$mavdir/include/*"

+

+echo "Generating C code"

+mavgen.py --lang=C --wire-protocol=1.0 --output=$mavdir/include/mavlink/v1.0 $mavdir/message_definitions/ardupilotmega.xml

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/ardupilotmega.h
@@ -1,1 +1,155 @@
+/** @file

+ *	@brief MAVLink comm protocol generated from ardupilotmega.xml

+ *	@see http://qgroundcontrol.org/mavlink/

+ */

+#ifndef ARDUPILOTMEGA_H

+#define ARDUPILOTMEGA_H

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+// MESSAGE LENGTHS AND CRCS

+

+#ifndef MAVLINK_MESSAGE_LENGTHS

+#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}

+#endif

+

+#ifndef MAVLINK_MESSAGE_CRCS

+#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}

+#endif

+

+#ifndef MAVLINK_MESSAGE_INFO

+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG}

+#endif

+

+#include "../protocol.h"

+

+#define MAVLINK_ENABLED_ARDUPILOTMEGA

+

+// ENUM DEFINITIONS

+

+

+/** @brief Enumeration of possible mount operation modes */

+#ifndef HAVE_ENUM_MAV_MOUNT_MODE

+#define HAVE_ENUM_MAV_MOUNT_MODE

+enum MAV_MOUNT_MODE

+{

+	MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */

+	MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */

+	MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */

+	MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */

+	MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */

+	MAV_MOUNT_MODE_ENUM_END=5, /*  | */

+};

+#endif

+

+/** @brief  */

+#ifndef HAVE_ENUM_MAV_CMD

+#define HAVE_ENUM_MAV_CMD

+enum MAV_CMD

+{

+	MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing)| Latitude| Longitude| Altitude|  */

+	MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */

+	MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Turns| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */

+	MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Seconds (decimal)| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */

+	MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */

+	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */

+	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the

+            vehicle itself. This can then be used by the vehicles control

+            system to control the vehicle attitude and the attitude of various

+            sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */

+	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */

+	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */

+	MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */

+	MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude|  */

+	MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */

+	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */

+	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the

+                    vehicle itself. This can then be used by the vehicles control

+                    system to control the vehicle attitude and the attitude of various

+                    devices such as cameras.

+                 |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */

+	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */

+	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */

+	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */

+	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */

+	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty|  */

+	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */

+	MAV_CMD_ENUM_END=246, /*  | */

+};

+#endif

+

+/** @brief  */

+#ifndef HAVE_ENUM_FENCE_ACTION

+#define HAVE_ENUM_FENCE_ACTION

+enum FENCE_ACTION

+{

+	FENCE_ACTION_NONE=0, /* Disable fenced mode | */

+	FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */

+	FENCE_ACTION_ENUM_END=2, /*  | */

+};

+#endif

+

+/** @brief  */

+#ifndef HAVE_ENUM_FENCE_BREACH

+#define HAVE_ENUM_FENCE_BREACH

+enum FENCE_BREACH

+{

+	FENCE_BREACH_NONE=0, /* No last fence breach | */

+	FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */

+	FENCE_BREACH_MAXALT=2, /* Breached minimum altitude | */

+	FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */

+	FENCE_BREACH_ENUM_END=4, /*  | */

+};

+#endif

+

+#include "../common/common.h"

+

+// MAVLINK VERSION

+

+#ifndef MAVLINK_VERSION

+#define MAVLINK_VERSION 2

+#endif

+

+#if (MAVLINK_VERSION == 0)

+#undef MAVLINK_VERSION

+#define MAVLINK_VERSION 2

+#endif

+

+// MESSAGE DEFINITIONS

+#include "./mavlink_msg_sensor_offsets.h"

+#include "./mavlink_msg_set_mag_offsets.h"

+#include "./mavlink_msg_meminfo.h"

+#include "./mavlink_msg_ap_adc.h"

+#include "./mavlink_msg_digicam_configure.h"

+#include "./mavlink_msg_digicam_control.h"

+#include "./mavlink_msg_mount_configure.h"

+#include "./mavlink_msg_mount_control.h"

+#include "./mavlink_msg_mount_status.h"

+#include "./mavlink_msg_fence_point.h"

+#include "./mavlink_msg_fence_fetch_point.h"

+#include "./mavlink_msg_fence_status.h"

+#include "./mavlink_msg_ahrs.h"

+#include "./mavlink_msg_simstate.h"

+#include "./mavlink_msg_hwstatus.h"

+#include "./mavlink_msg_radio.h"

+

+#ifdef __cplusplus

+}

+#endif // __cplusplus

+#endif // ARDUPILOTMEGA_H

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink.h
@@ -1,1 +1,28 @@
+/** @file

+ *	@brief MAVLink comm protocol built from ardupilotmega.xml

+ *	@see http://pixhawk.ethz.ch/software/mavlink

+ */

+#ifndef MAVLINK_H

+#define MAVLINK_H

+

+#ifndef MAVLINK_STX

+#define MAVLINK_STX 85

+#endif

+

+#ifndef MAVLINK_ENDIAN

+#define MAVLINK_ENDIAN MAVLINK_BIG_ENDIAN

+#endif

+

+#ifndef MAVLINK_ALIGNED_FIELDS

+#define MAVLINK_ALIGNED_FIELDS 0

+#endif

+

+#ifndef MAVLINK_CRC_EXTRA

+#define MAVLINK_CRC_EXTRA 0

+#endif

+

+#include "version.h"

+#include "ardupilotmega.h"

+

+#endif // MAVLINK_H

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_ahrs.h
@@ -1,1 +1,277 @@
+// MESSAGE AHRS PACKING

+

+#define MAVLINK_MSG_ID_AHRS 163

+

+typedef struct __mavlink_ahrs_t

+{

+ float omegaIx; ///< X gyro drift estimate rad/s

+ float omegaIy; ///< Y gyro drift estimate rad/s

+ float omegaIz; ///< Z gyro drift estimate rad/s

+ float accel_weight; ///< average accel_weight

+ float renorm_val; ///< average renormalisation value

+ float error_rp; ///< average error_roll_pitch value

+ float error_yaw; ///< average error_yaw value

+} mavlink_ahrs_t;

+

+#define MAVLINK_MSG_ID_AHRS_LEN 28

+#define MAVLINK_MSG_ID_163_LEN 28

+

+

+

+#define MAVLINK_MESSAGE_INFO_AHRS { \

+	"AHRS", \

+	7, \

+	{  { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \

+         { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \

+         { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \

+         { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \

+         { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \

+         { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \

+         { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a ahrs message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param omegaIx X gyro drift estimate rad/s

+ * @param omegaIy Y gyro drift estimate rad/s

+ * @param omegaIz Z gyro drift estimate rad/s

+ * @param accel_weight average accel_weight

+ * @param renorm_val average renormalisation value

+ * @param error_rp average error_roll_pitch value

+ * @param error_yaw average error_yaw value

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[28];

+	_mav_put_float(buf, 0, omegaIx);

+	_mav_put_float(buf, 4, omegaIy);

+	_mav_put_float(buf, 8, omegaIz);

+	_mav_put_float(buf, 12, accel_weight);

+	_mav_put_float(buf, 16, renorm_val);

+	_mav_put_float(buf, 20, error_rp);

+	_mav_put_float(buf, 24, error_yaw);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);

+#else

+	mavlink_ahrs_t packet;

+	packet.omegaIx = omegaIx;

+	packet.omegaIy = omegaIy;

+	packet.omegaIz = omegaIz;

+	packet.accel_weight = accel_weight;

+	packet.renorm_val = renorm_val;

+	packet.error_rp = error_rp;

+	packet.error_yaw = error_yaw;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_AHRS;

+	return mavlink_finalize_message(msg, system_id, component_id, 28);

+}

+

+/**

+ * @brief Pack a ahrs message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param omegaIx X gyro drift estimate rad/s

+ * @param omegaIy Y gyro drift estimate rad/s

+ * @param omegaIz Z gyro drift estimate rad/s

+ * @param accel_weight average accel_weight

+ * @param renorm_val average renormalisation value

+ * @param error_rp average error_roll_pitch value

+ * @param error_yaw average error_yaw value

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[28];

+	_mav_put_float(buf, 0, omegaIx);

+	_mav_put_float(buf, 4, omegaIy);

+	_mav_put_float(buf, 8, omegaIz);

+	_mav_put_float(buf, 12, accel_weight);

+	_mav_put_float(buf, 16, renorm_val);

+	_mav_put_float(buf, 20, error_rp);

+	_mav_put_float(buf, 24, error_yaw);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);

+#else

+	mavlink_ahrs_t packet;

+	packet.omegaIx = omegaIx;

+	packet.omegaIy = omegaIy;

+	packet.omegaIz = omegaIz;

+	packet.accel_weight = accel_weight;

+	packet.renorm_val = renorm_val;

+	packet.error_rp = error_rp;

+	packet.error_yaw = error_yaw;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_AHRS;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28);

+}

+

+/**

+ * @brief Encode a ahrs struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param ahrs C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)

+{

+	return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);

+}

+

+/**

+ * @brief Send a ahrs message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param omegaIx X gyro drift estimate rad/s

+ * @param omegaIy Y gyro drift estimate rad/s

+ * @param omegaIz Z gyro drift estimate rad/s

+ * @param accel_weight average accel_weight

+ * @param renorm_val average renormalisation value

+ * @param error_rp average error_roll_pitch value

+ * @param error_yaw average error_yaw value

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[28];

+	_mav_put_float(buf, 0, omegaIx);

+	_mav_put_float(buf, 4, omegaIy);

+	_mav_put_float(buf, 8, omegaIz);

+	_mav_put_float(buf, 12, accel_weight);

+	_mav_put_float(buf, 16, renorm_val);

+	_mav_put_float(buf, 20, error_rp);

+	_mav_put_float(buf, 24, error_yaw);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28);

+#else

+	mavlink_ahrs_t packet;

+	packet.omegaIx = omegaIx;

+	packet.omegaIy = omegaIy;

+	packet.omegaIz = omegaIz;

+	packet.accel_weight = accel_weight;

+	packet.renorm_val = renorm_val;

+	packet.error_rp = error_rp;

+	packet.error_yaw = error_yaw;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28);

+#endif

+}

+

+#endif

+

+// MESSAGE AHRS UNPACKING

+

+

+/**

+ * @brief Get field omegaIx from ahrs message

+ *

+ * @return X gyro drift estimate rad/s

+ */

+static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  0);

+}

+

+/**

+ * @brief Get field omegaIy from ahrs message

+ *

+ * @return Y gyro drift estimate rad/s

+ */

+static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  4);

+}

+

+/**

+ * @brief Get field omegaIz from ahrs message

+ *

+ * @return Z gyro drift estimate rad/s

+ */

+static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  8);

+}

+

+/**

+ * @brief Get field accel_weight from ahrs message

+ *

+ * @return average accel_weight

+ */

+static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  12);

+}

+

+/**

+ * @brief Get field renorm_val from ahrs message

+ *

+ * @return average renormalisation value

+ */

+static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  16);

+}

+

+/**

+ * @brief Get field error_rp from ahrs message

+ *

+ * @return average error_roll_pitch value

+ */

+static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  20);

+}

+

+/**

+ * @brief Get field error_yaw from ahrs message

+ *

+ * @return average error_yaw value

+ */

+static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  24);

+}

+

+/**

+ * @brief Decode a ahrs message into a struct

+ *

+ * @param msg The message to decode

+ * @param ahrs C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);

+	ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);

+	ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);

+	ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);

+	ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);

+	ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);

+	ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);

+#else

+	memcpy(ahrs, _MAV_PAYLOAD(msg), 28);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_ap_adc.h
@@ -1,1 +1,255 @@
+// MESSAGE AP_ADC PACKING

+

+#define MAVLINK_MSG_ID_AP_ADC 153

+

+typedef struct __mavlink_ap_adc_t

+{

+ uint16_t adc1; ///< ADC output 1

+ uint16_t adc2; ///< ADC output 2

+ uint16_t adc3; ///< ADC output 3

+ uint16_t adc4; ///< ADC output 4

+ uint16_t adc5; ///< ADC output 5

+ uint16_t adc6; ///< ADC output 6

+} mavlink_ap_adc_t;

+

+#define MAVLINK_MSG_ID_AP_ADC_LEN 12

+#define MAVLINK_MSG_ID_153_LEN 12

+

+

+

+#define MAVLINK_MESSAGE_INFO_AP_ADC { \

+	"AP_ADC", \

+	6, \

+	{  { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \

+         { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \

+         { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \

+         { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \

+         { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \

+         { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a ap_adc message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param adc1 ADC output 1

+ * @param adc2 ADC output 2

+ * @param adc3 ADC output 3

+ * @param adc4 ADC output 4

+ * @param adc5 ADC output 5

+ * @param adc6 ADC output 6

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[12];

+	_mav_put_uint16_t(buf, 0, adc1);

+	_mav_put_uint16_t(buf, 2, adc2);

+	_mav_put_uint16_t(buf, 4, adc3);

+	_mav_put_uint16_t(buf, 6, adc4);

+	_mav_put_uint16_t(buf, 8, adc5);

+	_mav_put_uint16_t(buf, 10, adc6);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);

+#else

+	mavlink_ap_adc_t packet;

+	packet.adc1 = adc1;

+	packet.adc2 = adc2;

+	packet.adc3 = adc3;

+	packet.adc4 = adc4;

+	packet.adc5 = adc5;

+	packet.adc6 = adc6;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_AP_ADC;

+	return mavlink_finalize_message(msg, system_id, component_id, 12);

+}

+

+/**

+ * @brief Pack a ap_adc message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param adc1 ADC output 1

+ * @param adc2 ADC output 2

+ * @param adc3 ADC output 3

+ * @param adc4 ADC output 4

+ * @param adc5 ADC output 5

+ * @param adc6 ADC output 6

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[12];

+	_mav_put_uint16_t(buf, 0, adc1);

+	_mav_put_uint16_t(buf, 2, adc2);

+	_mav_put_uint16_t(buf, 4, adc3);

+	_mav_put_uint16_t(buf, 6, adc4);

+	_mav_put_uint16_t(buf, 8, adc5);

+	_mav_put_uint16_t(buf, 10, adc6);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);

+#else

+	mavlink_ap_adc_t packet;

+	packet.adc1 = adc1;

+	packet.adc2 = adc2;

+	packet.adc3 = adc3;

+	packet.adc4 = adc4;

+	packet.adc5 = adc5;

+	packet.adc6 = adc6;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_AP_ADC;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12);

+}

+

+/**

+ * @brief Encode a ap_adc struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param ap_adc C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc)

+{

+	return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6);

+}

+

+/**

+ * @brief Send a ap_adc message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param adc1 ADC output 1

+ * @param adc2 ADC output 2

+ * @param adc3 ADC output 3

+ * @param adc4 ADC output 4

+ * @param adc5 ADC output 5

+ * @param adc6 ADC output 6

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[12];

+	_mav_put_uint16_t(buf, 0, adc1);

+	_mav_put_uint16_t(buf, 2, adc2);

+	_mav_put_uint16_t(buf, 4, adc3);

+	_mav_put_uint16_t(buf, 6, adc4);

+	_mav_put_uint16_t(buf, 8, adc5);

+	_mav_put_uint16_t(buf, 10, adc6);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12);

+#else

+	mavlink_ap_adc_t packet;

+	packet.adc1 = adc1;

+	packet.adc2 = adc2;

+	packet.adc3 = adc3;

+	packet.adc4 = adc4;

+	packet.adc5 = adc5;

+	packet.adc6 = adc6;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12);

+#endif

+}

+

+#endif

+

+// MESSAGE AP_ADC UNPACKING

+

+

+/**

+ * @brief Get field adc1 from ap_adc message

+ *

+ * @return ADC output 1

+ */

+static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  0);

+}

+

+/**

+ * @brief Get field adc2 from ap_adc message

+ *

+ * @return ADC output 2

+ */

+static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  2);

+}

+

+/**

+ * @brief Get field adc3 from ap_adc message

+ *

+ * @return ADC output 3

+ */

+static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  4);

+}

+

+/**

+ * @brief Get field adc4 from ap_adc message

+ *

+ * @return ADC output 4

+ */

+static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  6);

+}

+

+/**

+ * @brief Get field adc5 from ap_adc message

+ *

+ * @return ADC output 5

+ */

+static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  8);

+}

+

+/**

+ * @brief Get field adc6 from ap_adc message

+ *

+ * @return ADC output 6

+ */

+static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  10);

+}

+

+/**

+ * @brief Decode a ap_adc message into a struct

+ *

+ * @param msg The message to decode

+ * @param ap_adc C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg);

+	ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg);

+	ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg);

+	ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg);

+	ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg);

+	ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg);

+#else

+	memcpy(ap_adc, _MAV_PAYLOAD(msg), 12);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_digicam_configure.h
@@ -1,1 +1,365 @@
+// MESSAGE DIGICAM_CONFIGURE PACKING

+

+#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154

+

+typedef struct __mavlink_digicam_configure_t

+{

+ uint8_t target_system; ///< System ID

+ uint8_t target_component; ///< Component ID

+ uint8_t mode; ///< Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)

+ uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (0 means ignore)

+ uint8_t aperture; ///< F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)

+ uint8_t iso; ///< ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)

+ uint8_t exposure_type; ///< Exposure type enumeration from 1 to N (0 means ignore)

+ uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ uint8_t engine_cut_off; ///< Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)

+ uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore)

+ float extra_value; ///< Correspondent value to given extra_param

+} mavlink_digicam_configure_t;

+

+#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15

+#define MAVLINK_MSG_ID_154_LEN 15

+

+

+

+#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \

+	"DIGICAM_CONFIGURE", \

+	11, \

+	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_digicam_configure_t, target_system) }, \

+         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_digicam_configure_t, target_component) }, \

+         { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_digicam_configure_t, mode) }, \

+         { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \

+         { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_configure_t, aperture) }, \

+         { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, iso) }, \

+         { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, exposure_type) }, \

+         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, command_id) }, \

+         { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \

+         { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, extra_param) }, \

+         { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 11, offsetof(mavlink_digicam_configure_t, extra_value) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a digicam_configure message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)

+ * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)

+ * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)

+ * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)

+ * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)

+ * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)

+ * @param extra_param Extra parameters enumeration (0 means ignore)

+ * @param extra_value Correspondent value to given extra_param

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[15];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, mode);

+	_mav_put_uint16_t(buf, 3, shutter_speed);

+	_mav_put_uint8_t(buf, 5, aperture);

+	_mav_put_uint8_t(buf, 6, iso);

+	_mav_put_uint8_t(buf, 7, exposure_type);

+	_mav_put_uint8_t(buf, 8, command_id);

+	_mav_put_uint8_t(buf, 9, engine_cut_off);

+	_mav_put_uint8_t(buf, 10, extra_param);

+	_mav_put_float(buf, 11, extra_value);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 15);

+#else

+	mavlink_digicam_configure_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.mode = mode;

+	packet.shutter_speed = shutter_speed;

+	packet.aperture = aperture;

+	packet.iso = iso;

+	packet.exposure_type = exposure_type;

+	packet.command_id = command_id;

+	packet.engine_cut_off = engine_cut_off;

+	packet.extra_param = extra_param;

+	packet.extra_value = extra_value;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;

+	return mavlink_finalize_message(msg, system_id, component_id, 15);

+}

+

+/**

+ * @brief Pack a digicam_configure message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)

+ * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)

+ * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)

+ * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)

+ * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)

+ * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)

+ * @param extra_param Extra parameters enumeration (0 means ignore)

+ * @param extra_value Correspondent value to given extra_param

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint8_t target_system,uint8_t target_component,uint8_t mode,uint16_t shutter_speed,uint8_t aperture,uint8_t iso,uint8_t exposure_type,uint8_t command_id,uint8_t engine_cut_off,uint8_t extra_param,float extra_value)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[15];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, mode);

+	_mav_put_uint16_t(buf, 3, shutter_speed);

+	_mav_put_uint8_t(buf, 5, aperture);

+	_mav_put_uint8_t(buf, 6, iso);

+	_mav_put_uint8_t(buf, 7, exposure_type);

+	_mav_put_uint8_t(buf, 8, command_id);

+	_mav_put_uint8_t(buf, 9, engine_cut_off);

+	_mav_put_uint8_t(buf, 10, extra_param);

+	_mav_put_float(buf, 11, extra_value);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 15);

+#else

+	mavlink_digicam_configure_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.mode = mode;

+	packet.shutter_speed = shutter_speed;

+	packet.aperture = aperture;

+	packet.iso = iso;

+	packet.exposure_type = exposure_type;

+	packet.command_id = command_id;

+	packet.engine_cut_off = engine_cut_off;

+	packet.extra_param = extra_param;

+	packet.extra_value = extra_value;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 15);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 15);

+}

+

+/**

+ * @brief Encode a digicam_configure struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param digicam_configure C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure)

+{

+	return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value);

+}

+

+/**

+ * @brief Send a digicam_configure message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)

+ * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)

+ * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)

+ * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)

+ * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)

+ * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)

+ * @param extra_param Extra parameters enumeration (0 means ignore)

+ * @param extra_value Correspondent value to given extra_param

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[15];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, mode);

+	_mav_put_uint16_t(buf, 3, shutter_speed);

+	_mav_put_uint8_t(buf, 5, aperture);

+	_mav_put_uint8_t(buf, 6, iso);

+	_mav_put_uint8_t(buf, 7, exposure_type);

+	_mav_put_uint8_t(buf, 8, command_id);

+	_mav_put_uint8_t(buf, 9, engine_cut_off);

+	_mav_put_uint8_t(buf, 10, extra_param);

+	_mav_put_float(buf, 11, extra_value);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, 15);

+#else

+	mavlink_digicam_configure_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.mode = mode;

+	packet.shutter_speed = shutter_speed;

+	packet.aperture = aperture;

+	packet.iso = iso;

+	packet.exposure_type = exposure_type;

+	packet.command_id = command_id;

+	packet.engine_cut_off = engine_cut_off;

+	packet.extra_param = extra_param;

+	packet.extra_value = extra_value;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, 15);

+#endif

+}

+

+#endif

+

+// MESSAGE DIGICAM_CONFIGURE UNPACKING

+

+

+/**

+ * @brief Get field target_system from digicam_configure message

+ *

+ * @return System ID

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_target_system(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  0);

+}

+

+/**

+ * @brief Get field target_component from digicam_configure message

+ *

+ * @return Component ID

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_target_component(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  1);

+}

+

+/**

+ * @brief Get field mode from digicam_configure message

+ *

+ * @return Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_mode(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  2);

+}

+

+/**

+ * @brief Get field shutter_speed from digicam_configure message

+ *

+ * @return Divisor number //e.g. 1000 means 1/1000 (0 means ignore)

+ */

+static inline uint16_t mavlink_msg_digicam_configure_get_shutter_speed(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  3);

+}

+

+/**

+ * @brief Get field aperture from digicam_configure message

+ *

+ * @return F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_aperture(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  5);

+}

+

+/**

+ * @brief Get field iso from digicam_configure message

+ *

+ * @return ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_iso(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  6);

+}

+

+/**

+ * @brief Get field exposure_type from digicam_configure message

+ *

+ * @return Exposure type enumeration from 1 to N (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_exposure_type(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  7);

+}

+

+/**

+ * @brief Get field command_id from digicam_configure message

+ *

+ * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_command_id(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  8);

+}

+

+/**

+ * @brief Get field engine_cut_off from digicam_configure message

+ *

+ * @return Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_engine_cut_off(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  9);

+}

+

+/**

+ * @brief Get field extra_param from digicam_configure message

+ *

+ * @return Extra parameters enumeration (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_configure_get_extra_param(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  10);

+}

+

+/**

+ * @brief Get field extra_value from digicam_configure message

+ *

+ * @return Correspondent value to given extra_param

+ */

+static inline float mavlink_msg_digicam_configure_get_extra_value(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  11);

+}

+

+/**

+ * @brief Decode a digicam_configure message into a struct

+ *

+ * @param msg The message to decode

+ * @param digicam_configure C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t* msg, mavlink_digicam_configure_t* digicam_configure)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	digicam_configure->target_system = mavlink_msg_digicam_configure_get_target_system(msg);

+	digicam_configure->target_component = mavlink_msg_digicam_configure_get_target_component(msg);

+	digicam_configure->mode = mavlink_msg_digicam_configure_get_mode(msg);

+	digicam_configure->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(msg);

+	digicam_configure->aperture = mavlink_msg_digicam_configure_get_aperture(msg);

+	digicam_configure->iso = mavlink_msg_digicam_configure_get_iso(msg);

+	digicam_configure->exposure_type = mavlink_msg_digicam_configure_get_exposure_type(msg);

+	digicam_configure->command_id = mavlink_msg_digicam_configure_get_command_id(msg);

+	digicam_configure->engine_cut_off = mavlink_msg_digicam_configure_get_engine_cut_off(msg);

+	digicam_configure->extra_param = mavlink_msg_digicam_configure_get_extra_param(msg);

+	digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg);

+#else

+	memcpy(digicam_configure, _MAV_PAYLOAD(msg), 15);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_digicam_control.h
@@ -1,1 +1,343 @@
+// MESSAGE DIGICAM_CONTROL PACKING

+

+#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155

+

+typedef struct __mavlink_digicam_control_t

+{

+ uint8_t target_system; ///< System ID

+ uint8_t target_component; ///< Component ID

+ uint8_t session; ///< 0: stop, 1: start or keep it up //Session control e.g. show/hide lens

+ uint8_t zoom_pos; ///< 1 to N //Zoom's absolute position (0 means ignore)

+ int8_t zoom_step; ///< -100 to 100 //Zooming step value to offset zoom from the current position

+ uint8_t focus_lock; ///< 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus

+ uint8_t shot; ///< 0: ignore, 1: shot or start filming

+ uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore)

+ float extra_value; ///< Correspondent value to given extra_param

+} mavlink_digicam_control_t;

+

+#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13

+#define MAVLINK_MSG_ID_155_LEN 13

+

+

+

+#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \

+	"DIGICAM_CONTROL", \

+	10, \

+	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_digicam_control_t, target_system) }, \

+         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_digicam_control_t, target_component) }, \

+         { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_digicam_control_t, session) }, \

+         { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_digicam_control_t, zoom_pos) }, \

+         { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 4, offsetof(mavlink_digicam_control_t, zoom_step) }, \

+         { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, focus_lock) }, \

+         { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, shot) }, \

+         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, command_id) }, \

+         { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_control_t, extra_param) }, \

+         { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_digicam_control_t, extra_value) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a digicam_control message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens

+ * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)

+ * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position

+ * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus

+ * @param shot 0: ignore, 1: shot or start filming

+ * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ * @param extra_param Extra parameters enumeration (0 means ignore)

+ * @param extra_value Correspondent value to given extra_param

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[13];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, session);

+	_mav_put_uint8_t(buf, 3, zoom_pos);

+	_mav_put_int8_t(buf, 4, zoom_step);

+	_mav_put_uint8_t(buf, 5, focus_lock);

+	_mav_put_uint8_t(buf, 6, shot);

+	_mav_put_uint8_t(buf, 7, command_id);

+	_mav_put_uint8_t(buf, 8, extra_param);

+	_mav_put_float(buf, 9, extra_value);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 13);

+#else

+	mavlink_digicam_control_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.session = session;

+	packet.zoom_pos = zoom_pos;

+	packet.zoom_step = zoom_step;

+	packet.focus_lock = focus_lock;

+	packet.shot = shot;

+	packet.command_id = command_id;

+	packet.extra_param = extra_param;

+	packet.extra_value = extra_value;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 13);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;

+	return mavlink_finalize_message(msg, system_id, component_id, 13);

+}

+

+/**

+ * @brief Pack a digicam_control message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens

+ * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)

+ * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position

+ * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus

+ * @param shot 0: ignore, 1: shot or start filming

+ * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ * @param extra_param Extra parameters enumeration (0 means ignore)

+ * @param extra_value Correspondent value to given extra_param

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint8_t target_system,uint8_t target_component,uint8_t session,uint8_t zoom_pos,int8_t zoom_step,uint8_t focus_lock,uint8_t shot,uint8_t command_id,uint8_t extra_param,float extra_value)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[13];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, session);

+	_mav_put_uint8_t(buf, 3, zoom_pos);

+	_mav_put_int8_t(buf, 4, zoom_step);

+	_mav_put_uint8_t(buf, 5, focus_lock);

+	_mav_put_uint8_t(buf, 6, shot);

+	_mav_put_uint8_t(buf, 7, command_id);

+	_mav_put_uint8_t(buf, 8, extra_param);

+	_mav_put_float(buf, 9, extra_value);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 13);

+#else

+	mavlink_digicam_control_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.session = session;

+	packet.zoom_pos = zoom_pos;

+	packet.zoom_step = zoom_step;

+	packet.focus_lock = focus_lock;

+	packet.shot = shot;

+	packet.command_id = command_id;

+	packet.extra_param = extra_param;

+	packet.extra_value = extra_value;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 13);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13);

+}

+

+/**

+ * @brief Encode a digicam_control struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param digicam_control C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control)

+{

+	return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value);

+}

+

+/**

+ * @brief Send a digicam_control message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens

+ * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)

+ * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position

+ * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus

+ * @param shot 0: ignore, 1: shot or start filming

+ * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ * @param extra_param Extra parameters enumeration (0 means ignore)

+ * @param extra_value Correspondent value to given extra_param

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[13];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, session);

+	_mav_put_uint8_t(buf, 3, zoom_pos);

+	_mav_put_int8_t(buf, 4, zoom_step);

+	_mav_put_uint8_t(buf, 5, focus_lock);

+	_mav_put_uint8_t(buf, 6, shot);

+	_mav_put_uint8_t(buf, 7, command_id);

+	_mav_put_uint8_t(buf, 8, extra_param);

+	_mav_put_float(buf, 9, extra_value);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, 13);

+#else

+	mavlink_digicam_control_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.session = session;

+	packet.zoom_pos = zoom_pos;

+	packet.zoom_step = zoom_step;

+	packet.focus_lock = focus_lock;

+	packet.shot = shot;

+	packet.command_id = command_id;

+	packet.extra_param = extra_param;

+	packet.extra_value = extra_value;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, 13);

+#endif

+}

+

+#endif

+

+// MESSAGE DIGICAM_CONTROL UNPACKING

+

+

+/**

+ * @brief Get field target_system from digicam_control message

+ *

+ * @return System ID

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_target_system(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  0);

+}

+

+/**

+ * @brief Get field target_component from digicam_control message

+ *

+ * @return Component ID

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_target_component(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  1);

+}

+

+/**

+ * @brief Get field session from digicam_control message

+ *

+ * @return 0: stop, 1: start or keep it up //Session control e.g. show/hide lens

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_session(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  2);

+}

+

+/**

+ * @brief Get field zoom_pos from digicam_control message

+ *

+ * @return 1 to N //Zoom's absolute position (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_zoom_pos(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  3);

+}

+

+/**

+ * @brief Get field zoom_step from digicam_control message

+ *

+ * @return -100 to 100 //Zooming step value to offset zoom from the current position

+ */

+static inline int8_t mavlink_msg_digicam_control_get_zoom_step(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_int8_t(msg,  4);

+}

+

+/**

+ * @brief Get field focus_lock from digicam_control message

+ *

+ * @return 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_focus_lock(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  5);

+}

+

+/**

+ * @brief Get field shot from digicam_control message

+ *

+ * @return 0: ignore, 1: shot or start filming

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_shot(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  6);

+}

+

+/**

+ * @brief Get field command_id from digicam_control message

+ *

+ * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_command_id(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  7);

+}

+

+/**

+ * @brief Get field extra_param from digicam_control message

+ *

+ * @return Extra parameters enumeration (0 means ignore)

+ */

+static inline uint8_t mavlink_msg_digicam_control_get_extra_param(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  8);

+}

+

+/**

+ * @brief Get field extra_value from digicam_control message

+ *

+ * @return Correspondent value to given extra_param

+ */

+static inline float mavlink_msg_digicam_control_get_extra_value(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  9);

+}

+

+/**

+ * @brief Decode a digicam_control message into a struct

+ *

+ * @param msg The message to decode

+ * @param digicam_control C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* msg, mavlink_digicam_control_t* digicam_control)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	digicam_control->target_system = mavlink_msg_digicam_control_get_target_system(msg);

+	digicam_control->target_component = mavlink_msg_digicam_control_get_target_component(msg);

+	digicam_control->session = mavlink_msg_digicam_control_get_session(msg);

+	digicam_control->zoom_pos = mavlink_msg_digicam_control_get_zoom_pos(msg);

+	digicam_control->zoom_step = mavlink_msg_digicam_control_get_zoom_step(msg);

+	digicam_control->focus_lock = mavlink_msg_digicam_control_get_focus_lock(msg);

+	digicam_control->shot = mavlink_msg_digicam_control_get_shot(msg);

+	digicam_control->command_id = mavlink_msg_digicam_control_get_command_id(msg);

+	digicam_control->extra_param = mavlink_msg_digicam_control_get_extra_param(msg);

+	digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg);

+#else

+	memcpy(digicam_control, _MAV_PAYLOAD(msg), 13);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_fence_fetch_point.h
@@ -1,1 +1,189 @@
+// MESSAGE FENCE_FETCH_POINT PACKING

+

+#define MAVLINK_MSG_ID_FENCE_FETCH_POINT 161

+

+typedef struct __mavlink_fence_fetch_point_t

+{

+ uint8_t target_system; ///< System ID

+ uint8_t target_component; ///< Component ID

+ uint8_t idx; ///< point index (first point is 1, 0 is for return point)

+} mavlink_fence_fetch_point_t;

+

+#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN 3

+#define MAVLINK_MSG_ID_161_LEN 3

+

+

+

+#define MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT { \

+	"FENCE_FETCH_POINT", \

+	3, \

+	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_fetch_point_t, target_system) }, \

+         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_fetch_point_t, target_component) }, \

+         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_fetch_point_t, idx) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a fence_fetch_point message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param idx point index (first point is 1, 0 is for return point)

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint8_t target_system, uint8_t target_component, uint8_t idx)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[3];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, idx);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3);

+#else

+	mavlink_fence_fetch_point_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.idx = idx;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;

+	return mavlink_finalize_message(msg, system_id, component_id, 3);

+}

+

+/**

+ * @brief Pack a fence_fetch_point message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param idx point index (first point is 1, 0 is for return point)

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint8_t target_system,uint8_t target_component,uint8_t idx)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[3];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, idx);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3);

+#else

+	mavlink_fence_fetch_point_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.idx = idx;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3);

+}

+

+/**

+ * @brief Encode a fence_fetch_point struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param fence_fetch_point C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point)

+{

+	return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx);

+}

+

+/**

+ * @brief Send a fence_fetch_point message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param idx point index (first point is 1, 0 is for return point)

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[3];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, idx);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, 3);

+#else

+	mavlink_fence_fetch_point_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.idx = idx;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, 3);

+#endif

+}

+

+#endif

+

+// MESSAGE FENCE_FETCH_POINT UNPACKING

+

+

+/**

+ * @brief Get field target_system from fence_fetch_point message

+ *

+ * @return System ID

+ */

+static inline uint8_t mavlink_msg_fence_fetch_point_get_target_system(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  0);

+}

+

+/**

+ * @brief Get field target_component from fence_fetch_point message

+ *

+ * @return Component ID

+ */

+static inline uint8_t mavlink_msg_fence_fetch_point_get_target_component(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  1);

+}

+

+/**

+ * @brief Get field idx from fence_fetch_point message

+ *

+ * @return point index (first point is 1, 0 is for return point)

+ */

+static inline uint8_t mavlink_msg_fence_fetch_point_get_idx(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  2);

+}

+

+/**

+ * @brief Decode a fence_fetch_point message into a struct

+ *

+ * @param msg The message to decode

+ * @param fence_fetch_point C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t* msg, mavlink_fence_fetch_point_t* fence_fetch_point)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	fence_fetch_point->target_system = mavlink_msg_fence_fetch_point_get_target_system(msg);

+	fence_fetch_point->target_component = mavlink_msg_fence_fetch_point_get_target_component(msg);

+	fence_fetch_point->idx = mavlink_msg_fence_fetch_point_get_idx(msg);

+#else

+	memcpy(fence_fetch_point, _MAV_PAYLOAD(msg), 3);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_fence_point.h
@@ -1,1 +1,255 @@
+// MESSAGE FENCE_POINT PACKING

+

+#define MAVLINK_MSG_ID_FENCE_POINT 160

+

+typedef struct __mavlink_fence_point_t

+{

+ uint8_t target_system; ///< System ID

+ uint8_t target_component; ///< Component ID

+ uint8_t idx; ///< point index (first point is 1, 0 is for return point)

+ uint8_t count; ///< total number of points (for sanity checking)

+ float lat; ///< Latitude of point

+ float lng; ///< Longitude of point

+} mavlink_fence_point_t;

+

+#define MAVLINK_MSG_ID_FENCE_POINT_LEN 12

+#define MAVLINK_MSG_ID_160_LEN 12

+

+

+

+#define MAVLINK_MESSAGE_INFO_FENCE_POINT { \

+	"FENCE_POINT", \

+	6, \

+	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_point_t, target_system) }, \

+         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_point_t, target_component) }, \

+         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_point_t, idx) }, \

+         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_fence_point_t, count) }, \

+         { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fence_point_t, lat) }, \

+         { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_fence_point_t, lng) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a fence_point message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param idx point index (first point is 1, 0 is for return point)

+ * @param count total number of points (for sanity checking)

+ * @param lat Latitude of point

+ * @param lng Longitude of point

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[12];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, idx);

+	_mav_put_uint8_t(buf, 3, count);

+	_mav_put_float(buf, 4, lat);

+	_mav_put_float(buf, 8, lng);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);

+#else

+	mavlink_fence_point_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.idx = idx;

+	packet.count = count;

+	packet.lat = lat;

+	packet.lng = lng;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;

+	return mavlink_finalize_message(msg, system_id, component_id, 12);

+}

+

+/**

+ * @brief Pack a fence_point message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param idx point index (first point is 1, 0 is for return point)

+ * @param count total number of points (for sanity checking)

+ * @param lat Latitude of point

+ * @param lng Longitude of point

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,float lat,float lng)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[12];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, idx);

+	_mav_put_uint8_t(buf, 3, count);

+	_mav_put_float(buf, 4, lat);

+	_mav_put_float(buf, 8, lng);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12);

+#else

+	mavlink_fence_point_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.idx = idx;

+	packet.count = count;

+	packet.lat = lat;

+	packet.lng = lng;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12);

+}

+

+/**

+ * @brief Encode a fence_point struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param fence_point C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point)

+{

+	return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng);

+}

+

+/**

+ * @brief Send a fence_point message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param target_system System ID

+ * @param target_component Component ID

+ * @param idx point index (first point is 1, 0 is for return point)

+ * @param count total number of points (for sanity checking)

+ * @param lat Latitude of point

+ * @param lng Longitude of point

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[12];

+	_mav_put_uint8_t(buf, 0, target_system);

+	_mav_put_uint8_t(buf, 1, target_component);

+	_mav_put_uint8_t(buf, 2, idx);

+	_mav_put_uint8_t(buf, 3, count);

+	_mav_put_float(buf, 4, lat);

+	_mav_put_float(buf, 8, lng);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, 12);

+#else

+	mavlink_fence_point_t packet;

+	packet.target_system = target_system;

+	packet.target_component = target_component;

+	packet.idx = idx;

+	packet.count = count;

+	packet.lat = lat;

+	packet.lng = lng;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, 12);

+#endif

+}

+

+#endif

+

+// MESSAGE FENCE_POINT UNPACKING

+

+

+/**

+ * @brief Get field target_system from fence_point message

+ *

+ * @return System ID

+ */

+static inline uint8_t mavlink_msg_fence_point_get_target_system(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  0);

+}

+

+/**

+ * @brief Get field target_component from fence_point message

+ *

+ * @return Component ID

+ */

+static inline uint8_t mavlink_msg_fence_point_get_target_component(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  1);

+}

+

+/**

+ * @brief Get field idx from fence_point message

+ *

+ * @return point index (first point is 1, 0 is for return point)

+ */

+static inline uint8_t mavlink_msg_fence_point_get_idx(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  2);

+}

+

+/**

+ * @brief Get field count from fence_point message

+ *

+ * @return total number of points (for sanity checking)

+ */

+static inline uint8_t mavlink_msg_fence_point_get_count(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  3);

+}

+

+/**

+ * @brief Get field lat from fence_point message

+ *

+ * @return Latitude of point

+ */

+static inline float mavlink_msg_fence_point_get_lat(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  4);

+}

+

+/**

+ * @brief Get field lng from fence_point message

+ *

+ * @return Longitude of point

+ */

+static inline float mavlink_msg_fence_point_get_lng(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_float(msg,  8);

+}

+

+/**

+ * @brief Decode a fence_point message into a struct

+ *

+ * @param msg The message to decode

+ * @param fence_point C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* msg, mavlink_fence_point_t* fence_point)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	fence_point->target_system = mavlink_msg_fence_point_get_target_system(msg);

+	fence_point->target_component = mavlink_msg_fence_point_get_target_component(msg);

+	fence_point->idx = mavlink_msg_fence_point_get_idx(msg);

+	fence_point->count = mavlink_msg_fence_point_get_count(msg);

+	fence_point->lat = mavlink_msg_fence_point_get_lat(msg);

+	fence_point->lng = mavlink_msg_fence_point_get_lng(msg);

+#else

+	memcpy(fence_point, _MAV_PAYLOAD(msg), 12);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_fence_status.h
@@ -1,1 +1,211 @@
+// MESSAGE FENCE_STATUS PACKING

+

+#define MAVLINK_MSG_ID_FENCE_STATUS 162

+

+typedef struct __mavlink_fence_status_t

+{

+ uint8_t breach_status; ///< 0 if currently inside fence, 1 if outside

+ uint16_t breach_count; ///< number of fence breaches

+ uint8_t breach_type; ///< last breach type (see FENCE_BREACH_* enum)

+ uint32_t breach_time; ///< time of last breach in milliseconds since boot

+} mavlink_fence_status_t;

+

+#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8

+#define MAVLINK_MSG_ID_162_LEN 8

+

+

+

+#define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \

+	"FENCE_STATUS", \

+	4, \

+	{  { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_status_t, breach_status) }, \

+         { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 1, offsetof(mavlink_fence_status_t, breach_count) }, \

+         { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_fence_status_t, breach_type) }, \

+         { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_fence_status_t, breach_time) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a fence_status message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param breach_status 0 if currently inside fence, 1 if outside

+ * @param breach_count number of fence breaches

+ * @param breach_type last breach type (see FENCE_BREACH_* enum)

+ * @param breach_time time of last breach in milliseconds since boot

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[8];

+	_mav_put_uint8_t(buf, 0, breach_status);

+	_mav_put_uint16_t(buf, 1, breach_count);

+	_mav_put_uint8_t(buf, 3, breach_type);

+	_mav_put_uint32_t(buf, 4, breach_time);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 8);

+#else

+	mavlink_fence_status_t packet;

+	packet.breach_status = breach_status;

+	packet.breach_count = breach_count;

+	packet.breach_type = breach_type;

+	packet.breach_time = breach_time;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;

+	return mavlink_finalize_message(msg, system_id, component_id, 8);

+}

+

+/**

+ * @brief Pack a fence_status message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param breach_status 0 if currently inside fence, 1 if outside

+ * @param breach_count number of fence breaches

+ * @param breach_type last breach type (see FENCE_BREACH_* enum)

+ * @param breach_time time of last breach in milliseconds since boot

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[8];

+	_mav_put_uint8_t(buf, 0, breach_status);

+	_mav_put_uint16_t(buf, 1, breach_count);

+	_mav_put_uint8_t(buf, 3, breach_type);

+	_mav_put_uint32_t(buf, 4, breach_time);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 8);

+#else

+	mavlink_fence_status_t packet;

+	packet.breach_status = breach_status;

+	packet.breach_count = breach_count;

+	packet.breach_type = breach_type;

+	packet.breach_time = breach_time;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8);

+}

+

+/**

+ * @brief Encode a fence_status struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param fence_status C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status)

+{

+	return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time);

+}

+

+/**

+ * @brief Send a fence_status message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param breach_status 0 if currently inside fence, 1 if outside

+ * @param breach_count number of fence breaches

+ * @param breach_type last breach type (see FENCE_BREACH_* enum)

+ * @param breach_time time of last breach in milliseconds since boot

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[8];

+	_mav_put_uint8_t(buf, 0, breach_status);

+	_mav_put_uint16_t(buf, 1, breach_count);

+	_mav_put_uint8_t(buf, 3, breach_type);

+	_mav_put_uint32_t(buf, 4, breach_time);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, 8);

+#else

+	mavlink_fence_status_t packet;

+	packet.breach_status = breach_status;

+	packet.breach_count = breach_count;

+	packet.breach_type = breach_type;

+	packet.breach_time = breach_time;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, 8);

+#endif

+}

+

+#endif

+

+// MESSAGE FENCE_STATUS UNPACKING

+

+

+/**

+ * @brief Get field breach_status from fence_status message

+ *

+ * @return 0 if currently inside fence, 1 if outside

+ */

+static inline uint8_t mavlink_msg_fence_status_get_breach_status(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  0);

+}

+

+/**

+ * @brief Get field breach_count from fence_status message

+ *

+ * @return number of fence breaches

+ */

+static inline uint16_t mavlink_msg_fence_status_get_breach_count(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  1);

+}

+

+/**

+ * @brief Get field breach_type from fence_status message

+ *

+ * @return last breach type (see FENCE_BREACH_* enum)

+ */

+static inline uint8_t mavlink_msg_fence_status_get_breach_type(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  3);

+}

+

+/**

+ * @brief Get field breach_time from fence_status message

+ *

+ * @return time of last breach in milliseconds since boot

+ */

+static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint32_t(msg,  4);

+}

+

+/**

+ * @brief Decode a fence_status message into a struct

+ *

+ * @param msg The message to decode

+ * @param fence_status C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, mavlink_fence_status_t* fence_status)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg);

+	fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg);

+	fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg);

+	fence_status->breach_time = mavlink_msg_fence_status_get_breach_time(msg);

+#else

+	memcpy(fence_status, _MAV_PAYLOAD(msg), 8);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_hwstatus.h
@@ -1,1 +1,167 @@
+// MESSAGE HWSTATUS PACKING

+

+#define MAVLINK_MSG_ID_HWSTATUS 165

+

+typedef struct __mavlink_hwstatus_t

+{

+ uint16_t Vcc; ///< board voltage (mV)

+ uint8_t I2Cerr; ///< I2C error count

+} mavlink_hwstatus_t;

+

+#define MAVLINK_MSG_ID_HWSTATUS_LEN 3

+#define MAVLINK_MSG_ID_165_LEN 3

+

+

+

+#define MAVLINK_MESSAGE_INFO_HWSTATUS { \

+	"HWSTATUS", \

+	2, \

+	{  { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \

+         { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a hwstatus message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param Vcc board voltage (mV)

+ * @param I2Cerr I2C error count

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint16_t Vcc, uint8_t I2Cerr)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[3];

+	_mav_put_uint16_t(buf, 0, Vcc);

+	_mav_put_uint8_t(buf, 2, I2Cerr);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3);

+#else

+	mavlink_hwstatus_t packet;

+	packet.Vcc = Vcc;

+	packet.I2Cerr = I2Cerr;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;

+	return mavlink_finalize_message(msg, system_id, component_id, 3);

+}

+

+/**

+ * @brief Pack a hwstatus message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param Vcc board voltage (mV)

+ * @param I2Cerr I2C error count

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint16_t Vcc,uint8_t I2Cerr)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[3];

+	_mav_put_uint16_t(buf, 0, Vcc);

+	_mav_put_uint8_t(buf, 2, I2Cerr);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3);

+#else

+	mavlink_hwstatus_t packet;

+	packet.Vcc = Vcc;

+	packet.I2Cerr = I2Cerr;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3);

+}

+

+/**

+ * @brief Encode a hwstatus struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param hwstatus C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus)

+{

+	return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr);

+}

+

+/**

+ * @brief Send a hwstatus message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param Vcc board voltage (mV)

+ * @param I2Cerr I2C error count

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[3];

+	_mav_put_uint16_t(buf, 0, Vcc);

+	_mav_put_uint8_t(buf, 2, I2Cerr);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, 3);

+#else

+	mavlink_hwstatus_t packet;

+	packet.Vcc = Vcc;

+	packet.I2Cerr = I2Cerr;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, 3);

+#endif

+}

+

+#endif

+

+// MESSAGE HWSTATUS UNPACKING

+

+

+/**

+ * @brief Get field Vcc from hwstatus message

+ *

+ * @return board voltage (mV)

+ */

+static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  0);

+}

+

+/**

+ * @brief Get field I2Cerr from hwstatus message

+ *

+ * @return I2C error count

+ */

+static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  2);

+}

+

+/**

+ * @brief Decode a hwstatus message into a struct

+ *

+ * @param msg The message to decode

+ * @param hwstatus C-struct to decode the message contents into

+ */

+static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus)

+{

+#if MAVLINK_NEED_BYTE_SWAP

+	hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg);

+	hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg);

+#else

+	memcpy(hwstatus, _MAV_PAYLOAD(msg), 3);

+#endif

+}

 

--- /dev/null
+++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_limits_status.h
@@ -1,1 +1,321 @@
+// MESSAGE LIMITS_STATUS PACKING

+

+#define MAVLINK_MSG_ID_LIMITS_STATUS 169

+

+typedef struct __mavlink_limits_status_t

+{

+ uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE)

+ uint32_t last_trigger; ///< time of last breach in milliseconds since boot

+ uint32_t last_action; ///< time of last recovery action in milliseconds since boot

+ uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot

+ uint32_t last_clear; ///< time of last all-clear in milliseconds since boot

+ uint16_t breach_count; ///< number of fence breaches

+ uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)

+ uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)

+ uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)

+} mavlink_limits_status_t;

+

+#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22

+#define MAVLINK_MSG_ID_169_LEN 22

+

+

+

+#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \

+	"LIMITS_STATUS", \

+	9, \

+	{  { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_limits_status_t, limits_state) }, \

+         { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 1, offsetof(mavlink_limits_status_t, last_trigger) }, \

+         { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 5, offsetof(mavlink_limits_status_t, last_action) }, \

+         { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 9, offsetof(mavlink_limits_status_t, last_recovery) }, \

+         { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 13, offsetof(mavlink_limits_status_t, last_clear) }, \

+         { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 17, offsetof(mavlink_limits_status_t, breach_count) }, \

+         { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \

+         { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \

+         { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \

+         } \

+}

+

+

+/**

+ * @brief Pack a limits_status message

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ *

+ * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)

+ * @param last_trigger time of last breach in milliseconds since boot

+ * @param last_action time of last recovery action in milliseconds since boot

+ * @param last_recovery time of last successful recovery in milliseconds since boot

+ * @param last_clear time of last all-clear in milliseconds since boot

+ * @param breach_count number of fence breaches

+ * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)

+ * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)

+ * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,

+						       uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[22];

+	_mav_put_uint8_t(buf, 0, limits_state);

+	_mav_put_uint32_t(buf, 1, last_trigger);

+	_mav_put_uint32_t(buf, 5, last_action);

+	_mav_put_uint32_t(buf, 9, last_recovery);

+	_mav_put_uint32_t(buf, 13, last_clear);

+	_mav_put_uint16_t(buf, 17, breach_count);

+	_mav_put_uint8_t(buf, 19, mods_enabled);

+	_mav_put_uint8_t(buf, 20, mods_required);

+	_mav_put_uint8_t(buf, 21, mods_triggered);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);

+#else

+	mavlink_limits_status_t packet;

+	packet.limits_state = limits_state;

+	packet.last_trigger = last_trigger;

+	packet.last_action = last_action;

+	packet.last_recovery = last_recovery;

+	packet.last_clear = last_clear;

+	packet.breach_count = breach_count;

+	packet.mods_enabled = mods_enabled;

+	packet.mods_required = mods_required;

+	packet.mods_triggered = mods_triggered;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;

+	return mavlink_finalize_message(msg, system_id, component_id, 22);

+}

+

+/**

+ * @brief Pack a limits_status message on a channel

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param chan The MAVLink channel this message was sent over

+ * @param msg The MAVLink message to compress the data into

+ * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)

+ * @param last_trigger time of last breach in milliseconds since boot

+ * @param last_action time of last recovery action in milliseconds since boot

+ * @param last_recovery time of last successful recovery in milliseconds since boot

+ * @param last_clear time of last all-clear in milliseconds since boot

+ * @param breach_count number of fence breaches

+ * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)

+ * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)

+ * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)

+ * @return length of the message in bytes (excluding serial stream start sign)

+ */

+static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,

+							   mavlink_message_t* msg,

+						           uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[22];

+	_mav_put_uint8_t(buf, 0, limits_state);

+	_mav_put_uint32_t(buf, 1, last_trigger);

+	_mav_put_uint32_t(buf, 5, last_action);

+	_mav_put_uint32_t(buf, 9, last_recovery);

+	_mav_put_uint32_t(buf, 13, last_clear);

+	_mav_put_uint16_t(buf, 17, breach_count);

+	_mav_put_uint8_t(buf, 19, mods_enabled);

+	_mav_put_uint8_t(buf, 20, mods_required);

+	_mav_put_uint8_t(buf, 21, mods_triggered);

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);

+#else

+	mavlink_limits_status_t packet;

+	packet.limits_state = limits_state;

+	packet.last_trigger = last_trigger;

+	packet.last_action = last_action;

+	packet.last_recovery = last_recovery;

+	packet.last_clear = last_clear;

+	packet.breach_count = breach_count;

+	packet.mods_enabled = mods_enabled;

+	packet.mods_required = mods_required;

+	packet.mods_triggered = mods_triggered;

+

+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);

+#endif

+

+	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;

+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22);

+}

+

+/**

+ * @brief Encode a limits_status struct into a message

+ *

+ * @param system_id ID of this system

+ * @param component_id ID of this component (e.g. 200 for IMU)

+ * @param msg The MAVLink message to compress the data into

+ * @param limits_status C-struct to read the message contents from

+ */

+static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status)

+{

+	return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered);

+}

+

+/**

+ * @brief Send a limits_status message

+ * @param chan MAVLink channel to send the message

+ *

+ * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)

+ * @param last_trigger time of last breach in milliseconds since boot

+ * @param last_action time of last recovery action in milliseconds since boot

+ * @param last_recovery time of last successful recovery in milliseconds since boot

+ * @param last_clear time of last all-clear in milliseconds since boot

+ * @param breach_count number of fence breaches

+ * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)

+ * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)

+ * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)

+ */

+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

+

+static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered)

+{

+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS

+	char buf[22];

+	_mav_put_uint8_t(buf, 0, limits_state);

+	_mav_put_uint32_t(buf, 1, last_trigger);

+	_mav_put_uint32_t(buf, 5, last_action);

+	_mav_put_uint32_t(buf, 9, last_recovery);

+	_mav_put_uint32_t(buf, 13, last_clear);

+	_mav_put_uint16_t(buf, 17, breach_count);

+	_mav_put_uint8_t(buf, 19, mods_enabled);

+	_mav_put_uint8_t(buf, 20, mods_required);

+	_mav_put_uint8_t(buf, 21, mods_triggered);

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, 22);

+#else

+	mavlink_limits_status_t packet;

+	packet.limits_state = limits_state;

+	packet.last_trigger = last_trigger;

+	packet.last_action = last_action;

+	packet.last_recovery = last_recovery;

+	packet.last_clear = last_clear;

+	packet.breach_count = breach_count;

+	packet.mods_enabled = mods_enabled;

+	packet.mods_required = mods_required;

+	packet.mods_triggered = mods_triggered;

+

+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, 22);

+#endif

+}

+

+#endif

+

+// MESSAGE LIMITS_STATUS UNPACKING

+

+

+/**

+ * @brief Get field limits_state from limits_status message

+ *

+ * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE)

+ */

+static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint8_t(msg,  0);

+}

+

+/**

+ * @brief Get field last_trigger from limits_status message

+ *

+ * @return time of last breach in milliseconds since boot

+ */

+static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint32_t(msg,  1);

+}

+

+/**

+ * @brief Get field last_action from limits_status message

+ *

+ * @return time of last recovery action in milliseconds since boot

+ */

+static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint32_t(msg,  5);

+}

+

+/**

+ * @brief Get field last_recovery from limits_status message

+ *

+ * @return time of last successful recovery in milliseconds since boot

+ */

+static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint32_t(msg,  9);

+}

+

+/**

+ * @brief Get field last_clear from limits_status message

+ *

+ * @return time of last all-clear in milliseconds since boot

+ */

+static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint32_t(msg,  13);

+}

+

+/**

+ * @brief Get field breach_count from limits_status message

+ *

+ * @return number of fence breaches

+ */

+static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg)

+{

+	return _MAV_RETURN_uint16_t(msg,  17);

+}

+

+/**

+ * @brief Get field mods_enabled from limits_status message

+ *

+ * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)

+ */

+static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg)
</