From 1c72598393a94d156ab893460636bf82962a9b3c Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Fri, 28 Oct 2011 17:30:53 +0800 Subject: [PATCH 1/5] Fix HIL DCM to match actual --- libraries/AP_DCM/AP_DCM_HIL.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM_HIL.cpp b/libraries/AP_DCM/AP_DCM_HIL.cpp index 66adbbee7e..ae7aa3ff56 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.cpp +++ b/libraries/AP_DCM/AP_DCM_HIL.cpp @@ -42,12 +42,12 @@ AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw, float sPitch = sin(pitch), cPitch = cos(pitch); float sYaw = sin(yaw), cYaw = cos(yaw); _dcm_matrix.a.x = cPitch*cYaw; - _dcm_matrix.a.y = cPitch*sYaw; - _dcm_matrix.a.z = -sPitch; - _dcm_matrix.b.x = -cRoll*sYaw+sRoll*sPitch*cYaw; + _dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw; + _dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw; + _dcm_matrix.b.x = cPitch*sYaw; _dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw; - _dcm_matrix.b.z = sRoll*cPitch; - _dcm_matrix.c.x = sRoll*sYaw+cRoll*sPitch*cYaw; - _dcm_matrix.c.y = -sRoll*cYaw+cRoll*sPitch*sYaw; + _dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw; + _dcm_matrix.c.x = -sPitch; + _dcm_matrix.c.y = sRoll*cPitch; _dcm_matrix.c.z = cRoll*cPitch; } From 4a92048ef002ec6e46168134361c4942989778da Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 28 Oct 2011 21:34:10 +1100 Subject: [PATCH 2/5] fixed "Free RAM" display on bootup --- ArduCopter/system.pde | 2 +- ArduPlane/Log.pde | 2 +- ArduPlane/system.pde | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 1979ee328a..d466a15e19 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -82,7 +82,7 @@ static void init_ardupilot() #endif Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE - "\n\nFree RAM: %lu\n"), + "\n\nFree RAM: %u\n"), memcheck_available_memory()); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 310c837cef..d661a13952 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -650,7 +650,7 @@ static void Log_Read(int start_page, int end_page) Serial.printf_P(PSTR((AIRFRAME_NAME) #endif Serial.printf_P(PSTR("\n" THISFIRMWARE - "\nFree RAM: %lu\n"), + "\nFree RAM: %u\n"), memcheck_available_memory()); DataFlash.StartRead(start_page); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 0e4f0aebb0..44bb05ebc1 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -79,7 +79,7 @@ static void init_ardupilot() Serial1.begin(38400, 128, 16); Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE - "\n\nFree RAM: %lu\n"), + "\n\nFree RAM: %u\n"), memcheck_available_memory()); // From 585507f1888a9f70deb9f9ebbdc3212f2b5e80de Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 28 Oct 2011 14:43:43 -0400 Subject: [PATCH 3/5] Clean up formatting for AP_Common --- Tools/scripts/format.sh | 3 +- libraries/AP_Common/AP_Common.h | 20 +-- libraries/AP_Common/AP_Test.h | 4 +- libraries/AP_Common/AP_Var.cpp | 80 ++++----- libraries/AP_Common/AP_Var.h | 12 +- libraries/AP_Common/c++.cpp | 22 +-- libraries/AP_Common/examples/menu/menu.pde | 22 +-- libraries/AP_Common/include/menu.h | 168 +++++++++--------- libraries/AP_Common/menu.cpp | 190 ++++++++++----------- 9 files changed, 264 insertions(+), 257 deletions(-) diff --git a/Tools/scripts/format.sh b/Tools/scripts/format.sh index 67d00a4c0c..c907f5fdf5 100755 --- a/Tools/scripts/format.sh +++ b/Tools/scripts/format.sh @@ -2,10 +2,11 @@ function format { DIR=$1 find $DIR -regex ".*\.\(h\|cpp\|pde\)" -exec astyle {} \; - find $DIR -regex ".*\.\(h\|cpp\|pde\)" -exec rm {}.orig \; + find $DIR -regex ".*\.\(h\|cpp\|pde\)" -exec rm -f {}.orig \; } format apo format ArduRover format ArduBoat format libraries/APO +format libraries/AP_Common diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index ce197fa584..370cb53e25 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -26,7 +26,7 @@ // auto-detect at compile time if a call to a string function is using // a flash-stored string or not typedef struct { - char c; + char c; } prog_char_t; #include @@ -97,17 +97,17 @@ typedef struct { static inline int strcasecmp_P(const char *str1, const prog_char_t *pstr) { - return strcasecmp_P(str1, (const prog_char *)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); + return strcmp_P(str1, (const prog_char *)pstr); } static inline size_t strlcat_P(char *buffer, const prog_char_t *pstr, size_t buffer_size) { - return strlcat_P(buffer, (const prog_char *)pstr, buffer_size); + return strlcat_P(buffer, (const prog_char *)pstr, buffer_size); } //@} @@ -145,12 +145,12 @@ static inline size_t strlcat_P(char *buffer, const prog_char_t *pstr, size_t buf //@{ 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 + 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 }; //@} diff --git a/libraries/AP_Common/AP_Test.h b/libraries/AP_Common/AP_Test.h index 26c564f2c9..5e705d20ac 100644 --- a/libraries/AP_Common/AP_Test.h +++ b/libraries/AP_Common/AP_Test.h @@ -68,8 +68,8 @@ private: /// Constructor /// Test::Test(const char *name) : - _name(name), - _fail(false) + _name(name), + _fail(false) { } diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp index f14233ab5c..56fc06c25f 100644 --- a/libraries/AP_Common/AP_Var.cpp +++ b/libraries/AP_Common/AP_Var.cpp @@ -40,10 +40,10 @@ uint16_t AP_Var::_bytes_in_use; // Constructor for standalone variables // AP_Var::AP_Var(Key p_key, const prog_char_t *name, Flags flags) : - _group(NULL), - _key(p_key | k_key_not_located), - _name(name), - _flags(flags) + _group(NULL), + _key(p_key | k_key_not_located), + _name(name), + _flags(flags) { // Insert the variable or group into the list of known variables, unless // it wants to be unlisted. @@ -57,10 +57,10 @@ AP_Var::AP_Var(Key p_key, const prog_char_t *name, Flags flags) : // Constructor for variables in a group // AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags flags) : - _group(pGroup), - _key(index), - _name(name), - _flags(flags) + _group(pGroup), + _key(index), + _name(name), + _flags(flags) { AP_Var **vp; @@ -76,10 +76,10 @@ AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags f size_t loopCount = 0; while (*vp != NULL) { - if (loopCount++>k_num_max) return; + if (loopCount++>k_num_max) return; if ((*vp)->_key >= _key) { - break; + break; } vp = &((*vp)->_link); } @@ -106,17 +106,17 @@ AP_Var::~AP_Var(void) // Scan the list and remove this if we find it { - size_t loopCount = 0; - while (*vp) { + size_t loopCount = 0; + while (*vp) { - if (loopCount++>k_num_max) return; + if (loopCount++>k_num_max) return; - if (*vp == this) { - *vp = _link; - break; - } - vp = &((*vp)->_link); - } + if (*vp == this) { + *vp = _link; + break; + } + vp = &((*vp)->_link); + } } // If we are destroying a group, remove all its variables from the list @@ -129,7 +129,7 @@ AP_Var::~AP_Var(void) while (*vp) { - if (loopCount++>k_num_max) return; + if (loopCount++>k_num_max) return; // Does the variable claim us as its group? if ((*vp)->_group == this) { @@ -166,7 +166,7 @@ AP_Var::find(const char *name) for (vp = first(); vp; vp = vp->next()) { - if (loopCount++>k_num_max) return NULL; + if (loopCount++>k_num_max) return NULL; char name_buffer[32]; @@ -189,7 +189,7 @@ AP_Var::find(Key key) AP_Var *vp; size_t loopCount = 0; for (vp = first(); vp; vp = vp->next()) { - if (loopCount++>k_num_max) return NULL; + if (loopCount++>k_num_max) return NULL; if (key == vp->key()) { return vp; } @@ -242,7 +242,7 @@ bool AP_Var::save(void) newv = eeprom_read_byte(ep); if (newv != vbuf[i]) { serialDebug("readback failed at offset %p: got %u, expected %u", - ep, newv, vbuf[i]); + ep, newv, vbuf[i]); return false; } } @@ -304,10 +304,10 @@ bool AP_Var::save_all(void) while (vp) { - if (loopCount++>k_num_max) return false; + if (loopCount++>k_num_max) return false; if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autosave - (vp->_key != k_key_none)) { // has a key + (vp->_key != k_key_none)) { // has a key if (!vp->save()) { result = false; @@ -329,10 +329,10 @@ bool AP_Var::load_all(void) while (vp) { - if (loopCount++>k_num_max) return false; + if (loopCount++>k_num_max) return false; if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autoload - (vp->_key != k_key_none)) { // has a key + (vp->_key != k_key_none)) { // has a key if (!vp->load()) { result = false; @@ -365,7 +365,7 @@ AP_Var::erase_all() while (vp) { - if (loopCount++>k_num_max) return; + if (loopCount++>k_num_max) return; vp->_key = vp->key() | k_key_not_located; vp = vp->_link; @@ -449,7 +449,7 @@ AP_Var::first_member(AP_Var_group *group) while (*vp) { - if (loopCount++>k_num_max) return NULL; + if (loopCount++>k_num_max) return NULL; serialDebug("consider %p with %p", *vp, (*vp)->_group); if ((*vp)->_group == group) { @@ -471,7 +471,7 @@ AP_Var::next_member() while (vp) { - if (loopCount++>k_num_max) return NULL; + if (loopCount++>k_num_max) return NULL; if (vp->_group == _group) { return vp; @@ -498,7 +498,7 @@ bool AP_Var::_EEPROM_scan(void) eeprom_address = 0; eeprom_read_block(&ee_header, (void *)eeprom_address, sizeof(ee_header)); if ((ee_header.magic != k_EEPROM_magic) || - (ee_header.revision != k_EEPROM_revision)) { + (ee_header.revision != k_EEPROM_revision)) { serialDebug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision); return false; @@ -514,7 +514,7 @@ bool AP_Var::_EEPROM_scan(void) while (eeprom_address < (k_EEPROM_size - sizeof(var_header) - 1)) { - if (loopCount++>k_num_max) return NULL; + if (loopCount++>k_num_max) return NULL; // Read a variable header // @@ -531,10 +531,10 @@ bool AP_Var::_EEPROM_scan(void) // Sanity-check the variable header and abort if it looks bad // if (k_EEPROM_size <= ( - eeprom_address + // current position - sizeof(var_header) + // header for this variable - var_header.size + 1 + // data for this variable - sizeof(var_header))) { // header for sentinel + eeprom_address + // current position + sizeof(var_header) + // header for this variable + var_header.size + 1 + // data for this variable + sizeof(var_header))) { // header for sentinel serialDebug("header overruns EEPROM"); return false; @@ -544,7 +544,7 @@ bool AP_Var::_EEPROM_scan(void) vp = _variables; size_t loopCount2 = 0; while(vp) { - if (loopCount2++>k_num_max) return false; + if (loopCount2++>k_num_max) return false; if (vp->key() == var_header.key) { // adjust the variable's key to point to this entry vp->_key = eeprom_address + sizeof(var_header); @@ -572,7 +572,7 @@ bool AP_Var::_EEPROM_scan(void) vp = _variables; size_t loopCount3 = 0; while(vp) { - if (loopCount3++>k_num_max) return false; + if (loopCount3++>k_num_max) return false; if (vp->_key & k_key_not_located) { vp->_key |= k_key_not_allocated; serialDebug("key %u not allocated", vp->key()); @@ -661,7 +661,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) ee_header.spare = 0; eeprom_write_block(&ee_header, (void *)0, sizeof(ee_header)); - + _tail_sentinel = sizeof(ee_header); // Write a variable-sized pad header with a reserved key value @@ -737,7 +737,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali while (vp) { - if (loopCount++>k_num_max) return false; + if (loopCount++>k_num_max) return false; // (un)serialise the group member if (do_serialize) { diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index 99823ecb17..02e3cc0ade 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -302,14 +302,18 @@ public: /// /// @return The parent group, or NULL if the variable is not grouped. /// - AP_Var_group *group(void) { return _group; } + AP_Var_group *group(void) { + return _group; + } /// Returns the first variable in the global list. /// /// @return The first variable in the global list, or NULL if /// there are none. /// - static AP_Var *first(void) { return _variables; } + static AP_Var *first(void) { + return _variables; + } /// Returns the next variable in the global list. /// @@ -367,7 +371,9 @@ public: /// /// @return The sum of sizeof(*this) for all constructed AP_Var subclass instances. /// - static uint16_t get_memory_use() { return _bytes_in_use; } + static uint16_t get_memory_use() { + return _bytes_in_use; + } protected: // Memory statistics diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index e2fc437a2f..a11b97ff9b 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -14,29 +14,29 @@ void * operator new(size_t size) { #ifdef AP_DISPLAYMEM - displayMemory(); + displayMemory(); #endif - return(calloc(size, 1)); + return(calloc(size, 1)); } void operator delete(void *p) { - if (p) free(p); + if (p) free(p); } extern "C" void __cxa_pure_virtual() { - while (1) - { - Serial.println("Error: pure virtual call"); - delay(1000); - } + while (1) + { + Serial.println("Error: pure virtual call"); + delay(1000); + } } void * operator new[](size_t size) { #ifdef AP_DISPLAYMEM - displayMemory(); + displayMemory(); #endif return(calloc(size, 1)); } @@ -50,12 +50,12 @@ __extension__ typedef int __guard __attribute__((mode (__DI__))); int __cxa_guard_acquire(__guard *g) { - return !*(char *)(g); + return !*(char *)(g); }; void __cxa_guard_release (__guard *g) { - *(char *)g = 1; + *(char *)g = 1; }; void __cxa_guard_abort (__guard *) {}; diff --git a/libraries/AP_Common/examples/menu/menu.pde b/libraries/AP_Common/examples/menu/menu.pde index 4b7f7cd8f7..a1cfd328ad 100644 --- a/libraries/AP_Common/examples/menu/menu.pde +++ b/libraries/AP_Common/examples/menu/menu.pde @@ -7,24 +7,24 @@ FastSerialPort0(Serial); int8_t menu_test(uint8_t argc, const Menu::arg *argv) { - int i; + 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 - } + 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"); + Serial.println("auto text"); } const struct Menu::command top_menu_commands[] PROGMEM = { - {"*", menu_auto}, - {"test", menu_test}, + {"*", menu_auto}, + {"test", menu_test}, }; MENU(top, "menu", top_menu_commands); @@ -32,8 +32,8 @@ MENU(top, "menu", top_menu_commands); void setup(void) { - Serial.begin(38400); - top.run(); + Serial.begin(38400); + top.run(); } void diff --git a/libraries/AP_Common/include/menu.h b/libraries/AP_Common/include/menu.h index 86d7273955..1e4e3a0d89 100644 --- a/libraries/AP_Common/include/menu.h +++ b/libraries/AP_Common/include/menu.h @@ -23,101 +23,101 @@ /// 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) - }; + /// 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 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 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]; + /// 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 - }; + /// 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); + /// 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); + /// menu runner + void run(void); private: - /// Implements the default 'help' command. - /// - void _help(void); ///< implements the 'help' command + /// 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); + /// 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 + 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 + static char _inbuf[MENU_COMMANDLINE_MAX]; ///< input buffer + static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments }; /// Macros used to define a menu. diff --git a/libraries/AP_Common/menu.cpp b/libraries/AP_Common/menu.cpp index a4ea664a4f..462c631665 100644 --- a/libraries/AP_Common/menu.cpp +++ b/libraries/AP_Common/menu.cpp @@ -19,10 +19,10 @@ 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) + _prompt(prompt), + _commands(commands), + _entries(entries), + _ppfunc(ppfunc) { } @@ -30,112 +30,112 @@ Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entri void Menu::run(void) { - uint8_t len, i, ret; - uint8_t argc; - int c; - char *s; - - // loop performing commands - for (;;) { + uint8_t len, i, ret; + uint8_t argc; + int c; + char *s; - // run the pre-prompt function, if one is defined - if ((NULL != _ppfunc) && !_ppfunc()) - return; + // loop performing commands + for (;;) { - // 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++; - } + // run the pre-prompt function, if one is defined + if ((NULL != _ppfunc) && !_ppfunc()) + return; - // 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++; - } - - // 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); - if (-2 == ret) - return; - break; - } - } + // 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; + } + } - // implicit commands - if (i == _entries) { - if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) { - _help(); - } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) { - return; - } - } - } + // 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++; + } + + // 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++; + } + + // 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); + if (-2 == ret) + return; + break; + } + } + + // implicit commands + if (i == _entries) { + if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) { + _help(); + } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) { + return; + } + } + } } // 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)); + 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; + func fn; - fn = (func)pgm_read_word(&_commands[n].func); - return(fn(argc, &_argv[0])); + fn = (func)pgm_read_word(&_commands[n].func); + return(fn(argc, &_argv[0])); } From 57301ce6472819d05dbf48285e8782296bee4d98 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 28 Oct 2011 14:52:50 -0400 Subject: [PATCH 4/5] Cleaned up AP_GPS formatting. --- Tools/scripts/format.sh | 1 + libraries/AP_GPS/AP_GPS_406.cpp | 72 +-- libraries/AP_GPS/AP_GPS_406.h | 8 +- libraries/AP_GPS/AP_GPS_Auto.cpp | 288 +++++----- libraries/AP_GPS/AP_GPS_Auto.h | 54 +- libraries/AP_GPS/AP_GPS_HIL.cpp | 18 +- libraries/AP_GPS/AP_GPS_HIL.h | 10 +- libraries/AP_GPS/AP_GPS_IMU.cpp | 282 +++++----- libraries/AP_GPS/AP_GPS_IMU.h | 66 +-- libraries/AP_GPS/AP_GPS_MTK.cpp | 208 ++++---- libraries/AP_GPS/AP_GPS_MTK.h | 74 +-- libraries/AP_GPS/AP_GPS_MTK16.cpp | 220 ++++---- libraries/AP_GPS/AP_GPS_MTK16.h | 78 +-- libraries/AP_GPS/AP_GPS_NMEA.cpp | 498 +++++++++--------- libraries/AP_GPS/AP_GPS_NMEA.h | 110 ++-- libraries/AP_GPS/AP_GPS_None.h | 8 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 262 ++++----- libraries/AP_GPS/AP_GPS_SIRF.h | 130 ++--- libraries/AP_GPS/AP_GPS_Shim.h | 38 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 276 +++++----- libraries/AP_GPS/AP_GPS_UBLOX.h | 184 +++---- libraries/AP_GPS/GPS.cpp | 42 +- libraries/AP_GPS/GPS.h | 288 +++++----- .../examples/GPS_406_test/GPS_406_test.pde | 62 +-- .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 42 +- .../examples/GPS_MTK_test/GPS_MTK_test.pde | 60 +-- .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 81 +-- .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 60 +-- 28 files changed, 1764 insertions(+), 1756 deletions(-) diff --git a/Tools/scripts/format.sh b/Tools/scripts/format.sh index c907f5fdf5..cef831de59 100755 --- a/Tools/scripts/format.sh +++ b/Tools/scripts/format.sh @@ -10,3 +10,4 @@ format ArduRover format ArduBoat format libraries/APO format libraries/AP_Common +format libraries/AP_GPS diff --git a/libraries/AP_GPS/AP_GPS_406.cpp b/libraries/AP_GPS/AP_GPS_406.cpp index 5e9cb9f675..30ca72a62c 100644 --- a/libraries/AP_GPS/AP_GPS_406.cpp +++ b/libraries/AP_GPS/AP_GPS_406.cpp @@ -23,35 +23,35 @@ AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s) // Public Methods //////////////////////////////////////////////////////////////////// void AP_GPS_406::init(void) { - _change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate - _configure_gps(); // Function to configure GPS, to output only the desired msg's + _change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate + _configure_gps(); // Function to configure GPS, to output only the desired msg's - AP_GPS_SIRF::init(); // let the superclass do anything it might need here - - idleTimeout = 1200; + AP_GPS_SIRF::init(); // let the superclass do anything it might need here + + idleTimeout = 1200; } // Private Methods ////////////////////////////////////////////////////////////// -void +void AP_GPS_406::_configure_gps(void) { - const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00}; - const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B}; - const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1}; - const uint8_t gps_ender[] = {0xB0, 0xB3}; - - for(int z = 0; z < 2; z++){ - for(int x = 0; x < 5; x++){ - _port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg.. - _port->write(gps_payload[x]); // Prints the payload, is not the same for every msg - for(int y = 0; y < 6; y++) // Prints 6 zeros - _port->write((uint8_t)0); - _port->write(gps_checksum[x]); // Print the Checksum - _port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's. - _port->write(gps_ender[1]); // ender - } - } + const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00}; + const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B}; + const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1}; + const uint8_t gps_ender[] = {0xB0, 0xB3}; + + for(int z = 0; z < 2; z++) { + for(int x = 0; x < 5; x++) { + _port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg.. + _port->write(gps_payload[x]); // Prints the payload, is not the same for every msg + for(int y = 0; y < 6; y++) // Prints 6 zeros + _port->write((uint8_t)0); + _port->write(gps_checksum[x]); // Print the Checksum + _port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's. + _port->write(gps_ender[1]); // ender + } + } } // The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary @@ -60,24 +60,24 @@ AP_GPS_406::_configure_gps(void) // The change is sticky, but only for as long as the internal supercap holds // settings (usually less than a week). // -void +void AP_GPS_406::_change_to_sirf_protocol(void) { - FastSerial *fs = (FastSerial *)_port; // this is a bit grody... + FastSerial *fs = (FastSerial *)_port; // this is a bit grody... - fs->begin(4800); - delay(300); - _port->print(init_str); - delay(300); + fs->begin(4800); + delay(300); + _port->print(init_str); + delay(300); - fs->begin(9600); - delay(300); - _port->print(init_str); - delay(300); + fs->begin(9600); + delay(300); + _port->print(init_str); + delay(300); - fs->begin(GPS_406_BITRATE); - delay(300); - _port->print(init_str); - delay(300); + fs->begin(GPS_406_BITRATE); + delay(300); + _port->print(init_str); + delay(300); } diff --git a/libraries/AP_GPS/AP_GPS_406.h b/libraries/AP_GPS/AP_GPS_406.h index 6c34b1a6a3..daad1e6ae7 100644 --- a/libraries/AP_GPS/AP_GPS_406.h +++ b/libraries/AP_GPS/AP_GPS_406.h @@ -20,12 +20,12 @@ class AP_GPS_406 : public AP_GPS_SIRF { public: // Methods - AP_GPS_406(Stream *port); - virtual void init(void); + AP_GPS_406(Stream *port); + virtual void init(void); private: - void _change_to_sirf_protocol(void); - void _configure_gps(void); + void _change_to_sirf_protocol(void); + void _configure_gps(void); }; #endif diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index 6afc23cb23..b3a93b8a68 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -25,9 +25,9 @@ const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY; AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) : - GPS(s), - _fs(s), - _gps(gps) + GPS(s), + _fs(s), + _gps(gps) { } @@ -36,8 +36,8 @@ AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) : void AP_GPS_Auto::init(void) { - idleTimeout = 1200; - if (callback == NULL) callback = delay; + idleTimeout = 1200; + if (callback == NULL) callback = delay; } // Called the first time that a client tries to kick the GPS to update. @@ -53,41 +53,41 @@ AP_GPS_Auto::init(void) bool AP_GPS_Auto::read(void) { - GPS *gps; - uint8_t i; - unsigned long then; + GPS *gps; + uint8_t i; + unsigned long then; - // Loop through possible baudrates trying to detect a GPS at one of them. - // - // Note that we need to have a FastSerial rather than a Stream here because - // Stream has no idea of line speeds. FastSerial is quite OK with us calling - // ::begin any number of times. - // - for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { + // Loop through possible baudrates trying to detect a GPS at one of them. + // + // Note that we need to have a FastSerial rather than a Stream here because + // Stream has no idea of line speeds. FastSerial is quite OK with us calling + // ::begin any number of times. + // + for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { - _fs->begin(baudrates[i]); - if (NULL != (gps = _detect())) { + _fs->begin(baudrates[i]); + if (NULL != (gps = _detect())) { - // configure the detected GPS and give it a chance to listen to its device - gps->init(); - then = millis(); - while ((millis() - then) < 1200) { - // if we get a successful update from the GPS, we are done - gps->new_data = false; - gps->update(); - if (gps->new_data) { - Serial.println_P(PSTR("OK")); - *_gps = gps; - return true; - } - } - // GPS driver failed to parse any data from GPS, - // delete the driver and continue the process. - Serial.println_P(PSTR("failed, retrying")); - delete gps; - } - } - return false; + // configure the detected GPS and give it a chance to listen to its device + gps->init(); + then = millis(); + while ((millis() - then) < 1200) { + // if we get a successful update from the GPS, we are done + gps->new_data = false; + gps->update(); + if (gps->new_data) { + Serial.println_P(PSTR("OK")); + *_gps = gps; + return true; + } + } + // GPS driver failed to parse any data from GPS, + // delete the driver and continue the process. + Serial.println_P(PSTR("failed, retrying")); + delete gps; + } + } + return false; } // @@ -96,126 +96,126 @@ AP_GPS_Auto::read(void) GPS * AP_GPS_Auto::_detect(void) { - unsigned long then; - int fingerprint[4]; - int tries; - GPS *gps; + unsigned long then; + int fingerprint[4]; + int tries; + GPS *gps; - // - // Loop attempting to detect a recognized GPS - // - Serial.print('G'); - gps = NULL; - for (tries = 0; tries < 2; tries++) { + // + // Loop attempting to detect a recognized GPS + // + Serial.print('G'); + gps = NULL; + for (tries = 0; tries < 2; tries++) { - // - // Empty the serial buffer and wait for 50ms of quiet. - // - // XXX We can detect babble by counting incoming characters, but - // what would we do about it? - // - _port->flush(); - then = millis(); - do { - callback(1); - if (_port->available()) { - then = millis(); - _port->read(); - } - } while ((millis() - then) < 50); + // + // Empty the serial buffer and wait for 50ms of quiet. + // + // XXX We can detect babble by counting incoming characters, but + // what would we do about it? + // + _port->flush(); + then = millis(); + do { + callback(1); + if (_port->available()) { + then = millis(); + _port->read(); + } + } while ((millis() - then) < 50); - // - // Collect four characters to fingerprint a device - // - // If we take more than 1200ms to receive four characters, abort. - // This will normally only be the case where there is no GPS attached. - // - while (_port->available() < 4) { - callback(1); - if ((millis() - then) > 1200) { - Serial.print('!'); - return NULL; - } - } - fingerprint[0] = _port->read(); - fingerprint[1] = _port->read(); - fingerprint[2] = _port->read(); - fingerprint[3] = _port->read(); + // + // Collect four characters to fingerprint a device + // + // If we take more than 1200ms to receive four characters, abort. + // This will normally only be the case where there is no GPS attached. + // + while (_port->available() < 4) { + callback(1); + if ((millis() - then) > 1200) { + Serial.print('!'); + return NULL; + } + } + fingerprint[0] = _port->read(); + fingerprint[1] = _port->read(); + fingerprint[2] = _port->read(); + fingerprint[3] = _port->read(); - // - // ublox or MTK in DIYD binary mode (whose smart idea was - // it to make the MTK look sort-of like it was talking UBX?) - // - if ((0xb5 == fingerprint[0]) && - (0x62 == fingerprint[1]) && - (0x01 == fingerprint[2])) { + // + // ublox or MTK in DIYD binary mode (whose smart idea was + // it to make the MTK look sort-of like it was talking UBX?) + // + if ((0xb5 == fingerprint[0]) && + (0x62 == fingerprint[1]) && + (0x01 == fingerprint[2])) { - // message 5 is MTK pretending to talk UBX - if (0x05 == fingerprint[3]) { - gps = new AP_GPS_MTK(_port); - Serial.print_P(PSTR(" MTK1.4 ")); - break; - } + // message 5 is MTK pretending to talk UBX + if (0x05 == fingerprint[3]) { + gps = new AP_GPS_MTK(_port); + Serial.print_P(PSTR(" MTK1.4 ")); + break; + } - // any other message is ublox - gps = new AP_GPS_UBLOX(_port); - Serial.print_P(PSTR(" ublox ")); - break; - } + // any other message is ublox + gps = new AP_GPS_UBLOX(_port); + Serial.print_P(PSTR(" ublox ")); + break; + } - // - // MTK v1.6 - // - if ((0xd0 == fingerprint[0]) && - (0xdd == fingerprint[1]) && - (0x20 == fingerprint[2])) { - gps = new AP_GPS_MTK16(_port); - Serial.print_P(PSTR(" MTK1.6 ")); - break; - } + // + // MTK v1.6 + // + if ((0xd0 == fingerprint[0]) && + (0xdd == fingerprint[1]) && + (0x20 == fingerprint[2])) { + gps = new AP_GPS_MTK16(_port); + Serial.print_P(PSTR(" MTK1.6 ")); + break; + } - // - // SIRF in binary mode - // - if ((0xa0 == fingerprint[0]) && - (0xa2 == fingerprint[1])) { - gps = new AP_GPS_SIRF(_port); - Serial.print_P(PSTR(" SiRF ")); - break; - } + // + // SIRF in binary mode + // + if ((0xa0 == fingerprint[0]) && + (0xa2 == fingerprint[1])) { + gps = new AP_GPS_SIRF(_port); + Serial.print_P(PSTR(" SiRF ")); + break; + } - // - // If we haven't spammed the various init strings, send them now - // and retry to avoid a false-positive on the NMEA detector. - // - if (0 == tries) { - Serial.print('*'); - // use the FastSerial port handle so that we can use PROGMEM strings - _fs->println_P((const prog_char_t *)_mtk_set_binary); - _fs->println_P((const prog_char_t *)_ublox_set_binary); - _fs->println_P((const prog_char_t *)_sirf_set_binary); + // + // If we haven't spammed the various init strings, send them now + // and retry to avoid a false-positive on the NMEA detector. + // + if (0 == tries) { + Serial.print('*'); + // use the FastSerial port handle so that we can use PROGMEM strings + _fs->println_P((const prog_char_t *)_mtk_set_binary); + _fs->println_P((const prog_char_t *)_ublox_set_binary); + _fs->println_P((const prog_char_t *)_sirf_set_binary); - // give the GPS time to react to the settings - callback(100); - continue; - } else { - Serial.print('?'); - } + // give the GPS time to react to the settings + callback(100); + continue; + } else { + Serial.print('?'); + } #if WITH_NMEA_MODE - // - // Something talking NMEA - // - if (('$' == fingerprint[0]) && - (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) { + // + // Something talking NMEA + // + if (('$' == fingerprint[0]) && + (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) { - // XXX this may be a bit presumptive, might want to give the GPS a couple of - // iterations around the loop to react to init strings? - gps = new AP_GPS_NMEA(_port); - break; - } + // XXX this may be a bit presumptive, might want to give the GPS a couple of + // iterations around the loop to react to init strings? + gps = new AP_GPS_NMEA(_port); + break; + } #endif - } - return(gps); + } + return(gps); } diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index 61be2de201..db38de96af 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -12,40 +12,40 @@ class AP_GPS_Auto : public GPS { public: - /// Constructor - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @param port Stream connected to the GPS module. - /// @param ptr Pointer to a GPS * that will be fixed up by ::init - /// when the GPS type has been detected. - /// - AP_GPS_Auto(FastSerial *s, GPS **gps); + /// Constructor + /// + /// @note The stream is expected to be set up and configured for the + /// correct bitrate before ::init is called. + /// + /// @param port Stream connected to the GPS module. + /// @param ptr Pointer to a GPS * that will be fixed up by ::init + /// when the GPS type has been detected. + /// + AP_GPS_Auto(FastSerial *s, GPS **gps); - /// Dummy init routine, does nothing - virtual void init(void); + /// Dummy init routine, does nothing + virtual void init(void); - /// Detect and initialise the attached GPS unit. Updates the - /// pointer passed into the constructor when a GPS is detected. - /// - virtual bool read(void); + /// Detect and initialise the attached GPS unit. Updates the + /// pointer passed into the constructor when a GPS is detected. + /// + virtual bool read(void); private: - /// Copy of the port, known at construction time to be a real FastSerial port. - FastSerial *_fs; + /// Copy of the port, known at construction time to be a real FastSerial port. + FastSerial *_fs; - /// global GPS driver pointer, updated by auto-detection - /// - GPS **_gps; + /// global GPS driver pointer, updated by auto-detection + /// + GPS **_gps; - /// low-level auto-detect routine - /// - GPS *_detect(void); + /// low-level auto-detect routine + /// + GPS *_detect(void); - static const prog_char _mtk_set_binary[]; - static const prog_char _ublox_set_binary[]; - static const prog_char _sirf_set_binary[]; + static const prog_char _mtk_set_binary[]; + static const prog_char _ublox_set_binary[]; + static const prog_char _sirf_set_binary[]; }; #endif diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index d55beca3f9..38db02ba05 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -21,21 +21,21 @@ AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s) // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_HIL::init(void) -{ - idleTimeout = 1200; +{ + idleTimeout = 1200; } bool AP_GPS_HIL::read(void) { - bool result = _updated; + bool result = _updated; - // return true once for each update pushed in - _updated = false; - return result; + // return true once for each update pushed in + _updated = false; + return result; } void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude, - float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) + float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { time = _time; latitude = _latitude*1.0e7; @@ -46,7 +46,7 @@ void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _al speed_3d = _speed_3d*1.0e2; num_sats = _num_sats; fix = true; - new_data = true; - _updated = true; + new_data = true; + _updated = true; } diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index 9fa644efe8..fab213d042 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -16,9 +16,9 @@ class AP_GPS_HIL : public GPS { public: - AP_GPS_HIL(Stream *s); - virtual void init(void); - virtual bool read(void); + AP_GPS_HIL(Stream *s); + virtual void init(void); + virtual bool read(void); /** * Hardware in the loop set function @@ -31,10 +31,10 @@ public: * @param altitude - altitude in meters */ virtual void setHIL(long time, float latitude, float longitude, float altitude, - float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); + float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); private: - bool _updated; + bool _updated; }; #endif // AP_GPS_HIL_H diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp index 84cf24d045..71fc6f2e23 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.cpp +++ b/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -15,7 +15,7 @@ Methods: init() : GPS initialization update() : Call this funcion as often as you want to ensure you read the incomming gps data - + Properties: lattitude : lattitude * 10000000 (long value) longitude : longitude * 10000000 (long value) @@ -25,7 +25,7 @@ new_data : 1 when a new data is received. You need to write a 0 to new_data when you read the data fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix. - + */ #include "AP_GPS_IMU.h" #include "WProgram.h" @@ -38,192 +38,192 @@ AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) // Public Methods ////////////////////////////////////////////////////////////// -void +void AP_GPS_IMU::init(void) { - // we expect the stream to already be open at the corret bitrate - idleTimeout = 1200; + // we expect the stream to already be open at the corret bitrate + idleTimeout = 1200; } // optimization : This code doesn't wait for data. It only proccess the data available. // We can call this function on the main loop (50Hz loop) // If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info. -bool +bool AP_GPS_IMU::read(void) { - byte data; - int numc = 0; + byte data; + int numc = 0; - numc = _port->available(); + numc = _port->available(); - if (numc > 0){ - for (int i=0;i 0) { + for (int i=0; iread(); - - switch(step){ //Normally we start from zero. This is a state machine - case 0: - if(data == 0x44) // IMU sync char 1 - step++; //OH first data packet is correct, so jump to the next step - break; - - case 1: - if(data == 0x49) // IMU sync char 2 - step++; //ooh! The second data packet is correct, jump to the step 2 - else - step=0; //Nop, is not correct so restart to step zero and try again. - break; - - case 2: - if(data == 0x59) // IMU sync char 3 - step++; //ooh! The second data packet is correct, jump to the step 2 - else - step=0; //Nop, is not correct so restart to step zero and try again. - break; - - case 3: - if(data == 0x64) // IMU sync char 4 - step++; //ooh! The second data packet is correct, jump to the step 2 - else - step=0; //Nop, is not correct so restart to step zero and try again. - break; - - case 4: - payload_length = data; - checksum(payload_length); - step++; - if (payload_length > 28){ - step = 0; //Bad data, so restart to step zero and try again. - payload_counter = 0; - ck_a = 0; - ck_b = 0; - //payload_error_count++; - } - break; - - case 5: - message_num = data; - checksum(data); - step++; - break; - - case 6: // Payload data read... - // We stay in this state until we reach the payload_length - buffer[payload_counter] = data; - checksum(data); - payload_counter++; - if (payload_counter >= payload_length) { - step++; - } - break; - - case 7: - GPS_ck_a = data; // First checksum byte - step++; - break; - - case 8: - GPS_ck_b = data; // Second checksum byte - - // We end the IMU/GPS read... - // Verify the received checksum with the generated checksum.. - if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) { - if (message_num == 0x02) { - join_data(); - } else if (message_num == 0x03) { - GPS_join_data(); - } else if (message_num == 0x04) { - join_data_xplane(); - } else if (message_num == 0x0a) { - //PERF_join_data(); - } else { + data = _port->read(); + + switch(step) { //Normally we start from zero. This is a state machine + case 0: + if(data == 0x44) // IMU sync char 1 + step++; //OH first data packet is correct, so jump to the next step + break; + + case 1: + if(data == 0x49) // IMU sync char 2 + step++; //ooh! The second data packet is correct, jump to the step 2 + else + step=0; //Nop, is not correct so restart to step zero and try again. + break; + + case 2: + if(data == 0x59) // IMU sync char 3 + step++; //ooh! The second data packet is correct, jump to the step 2 + else + step=0; //Nop, is not correct so restart to step zero and try again. + break; + + case 3: + if(data == 0x64) // IMU sync char 4 + step++; //ooh! The second data packet is correct, jump to the step 2 + else + step=0; //Nop, is not correct so restart to step zero and try again. + break; + + case 4: + payload_length = data; + checksum(payload_length); + step++; + if (payload_length > 28) { + step = 0; //Bad data, so restart to step zero and try again. + payload_counter = 0; + ck_a = 0; + ck_b = 0; + //payload_error_count++; + } + break; + + case 5: + message_num = data; + checksum(data); + step++; + break; + + case 6: // Payload data read... + // We stay in this state until we reach the payload_length + buffer[payload_counter] = data; + checksum(data); + payload_counter++; + if (payload_counter >= payload_length) { + step++; + } + break; + + case 7: + GPS_ck_a = data; // First checksum byte + step++; + break; + + case 8: + GPS_ck_b = data; // Second checksum byte + + // We end the IMU/GPS read... + // Verify the received checksum with the generated checksum.. + if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) { + if (message_num == 0x02) { + join_data(); + } else if (message_num == 0x03) { + GPS_join_data(); + } else if (message_num == 0x04) { + join_data_xplane(); + } else if (message_num == 0x0a) { + //PERF_join_data(); + } else { // _error("Invalid message number = %d\n", (int)message_num); - } - } else { + } + } else { // _error("XXX Checksum error\n"); //bad checksum - //imu_checksum_error_count++; - } - // Variable initialization - step = 0; - payload_counter = 0; - ck_a = 0; - ck_b = 0; - break; - } - }// End for... - } - return true; + //imu_checksum_error_count++; + } + // Variable initialization + step = 0; + payload_counter = 0; + ck_a = 0; + ck_b = 0; + break; + } + }// End for... + } + return true; } /**************************************************************** - * + * ****************************************************************/ void AP_GPS_IMU::join_data(void) { - //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. - //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. + //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. + //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. - //Storing IMU roll - roll_sensor = *(int *)&buffer[0]; + //Storing IMU roll + roll_sensor = *(int *)&buffer[0]; - //Storing IMU pitch - pitch_sensor = *(int *)&buffer[2]; + //Storing IMU pitch + pitch_sensor = *(int *)&buffer[2]; - //Storing IMU heading (yaw) - ground_course = *(int *)&buffer[4]; - imu_ok = true; + //Storing IMU heading (yaw) + ground_course = *(int *)&buffer[4]; + imu_ok = true; } void AP_GPS_IMU::join_data_xplane() { - //Storing IMU roll - roll_sensor = *(int *)&buffer[0]; - + //Storing IMU roll + roll_sensor = *(int *)&buffer[0]; - //Storing IMU pitch - pitch_sensor = *(int *)&buffer[2]; - //Storing IMU heading (yaw) - ground_course = *(unsigned int *)&buffer[4]; - - //Storing airspeed - airspeed = *(int *)&buffer[6]; + //Storing IMU pitch + pitch_sensor = *(int *)&buffer[2]; - imu_ok = true; + //Storing IMU heading (yaw) + ground_course = *(unsigned int *)&buffer[4]; + + //Storing airspeed + airspeed = *(int *)&buffer[6]; + + imu_ok = true; } void AP_GPS_IMU::GPS_join_data(void) { - longitude = *(long *)&buffer[0]; // degrees * 10e7 - latitude = *(long *)&buffer[4]; + longitude = *(long *)&buffer[0]; // degrees * 10e7 + latitude = *(long *)&buffer[4]; - //Storing GPS Height above the sea level - altitude = (long)*(int *)&buffer[8] * 10; + //Storing GPS Height above the sea level + altitude = (long)*(int *)&buffer[8] * 10; - //Storing Speed - speed_3d = ground_speed = (float)*(int *)&buffer[10]; - - //We skip the gps ground course because we use yaw value from the IMU for ground course - time = *(long *)&buffer[14]; + //Storing Speed + speed_3d = ground_speed = (float)*(int *)&buffer[10]; - imu_health = buffer[15]; - - new_data = true; - fix = true; + //We skip the gps ground course because we use yaw value from the IMU for ground course + time = *(long *)&buffer[14]; + + imu_health = buffer[15]; + + new_data = true; + fix = true; } /**************************************************************** - * + * ****************************************************************/ // checksum algorithm void AP_GPS_IMU::checksum(byte data) { - ck_a += data; - ck_b += ck_a; + ck_a += data; + ck_b += ck_a; } @@ -231,4 +231,4 @@ void AP_GPS_IMU::checksum(byte data) * Unused ****************************************************************/ void AP_GPS_IMU::setHIL(long _time, float _latitude, float _longitude, float _altitude, - float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {}; + float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {}; diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h index 1a31c615f0..ed5ee20326 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.h +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -7,42 +7,42 @@ #define MAXPAYLOAD 32 class AP_GPS_IMU : public GPS { - public: +public: // Methods - AP_GPS_IMU(Stream *s); - virtual void init(void); - virtual bool read(void); - - // Properties - long roll_sensor; // how much we're turning in deg * 100 - long pitch_sensor; // our angle of attack in deg * 100 - int airspeed; - float imu_health; - uint8_t imu_ok; - - // Unused - virtual void setHIL(long time, float latitude, float longitude, float altitude, - float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); + AP_GPS_IMU(Stream *s); + virtual void init(void); + virtual bool read(void); - private: - // Packet checksums - uint8_t ck_a; - uint8_t ck_b; - uint8_t GPS_ck_a; - uint8_t GPS_ck_b; - - uint8_t step; - uint8_t msg_class; - uint8_t message_num; - uint8_t payload_length; - uint8_t payload_counter; - uint8_t buffer[MAXPAYLOAD]; - - void join_data(); - void join_data_xplane(); - void GPS_join_data(); - void checksum(unsigned char data); + // Properties + long roll_sensor; // how much we're turning in deg * 100 + long pitch_sensor; // our angle of attack in deg * 100 + int airspeed; + float imu_health; + uint8_t imu_ok; + + // Unused + virtual void setHIL(long time, float latitude, float longitude, float altitude, + float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); + +private: + // Packet checksums + uint8_t ck_a; + uint8_t ck_b; + uint8_t GPS_ck_a; + uint8_t GPS_ck_b; + + uint8_t step; + uint8_t msg_class; + uint8_t message_num; + uint8_t payload_length; + uint8_t payload_counter; + uint8_t buffer[MAXPAYLOAD]; + + void join_data(); + void join_data_xplane(); + void GPS_join_data(); + void checksum(unsigned char data); }; #endif diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 6375f61ab6..a50088e43e 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -20,21 +20,21 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) } // Public Methods ////////////////////////////////////////////////////////////// -void +void AP_GPS_MTK::init(void) -{ - _port->flush(); - // initialize serial port for binary protocol use - // XXX should assume binary, let GPS_AUTO handle dynamic config? - _port->print(MTK_SET_BINARY); +{ + _port->flush(); + // initialize serial port for binary protocol use + // XXX should assume binary, let GPS_AUTO handle dynamic config? + _port->print(MTK_SET_BINARY); - // set 4Hz update rate - _port->print(MTK_OUTPUT_4HZ); - - // set initial epoch code - _epoch = TIME_OF_DAY; - - idleTimeout = 1200; + // set 4Hz update rate + _port->print(MTK_OUTPUT_4HZ); + + // set initial epoch code + _epoch = TIME_OF_DAY; + + idleTimeout = 1200; } // Process bytes available from the stream @@ -51,103 +51,103 @@ AP_GPS_MTK::init(void) bool AP_GPS_MTK::read(void) { - uint8_t data; - int numc; - bool parsed = false; + uint8_t data; + int numc; + bool parsed = false; - numc = _port->available(); - for (int i = 0; i < numc; i++){ // Process bytes received + numc = _port->available(); + for (int i = 0; i < numc; i++) { // Process bytes received - // read the next byte - data = _port->read(); + // read the next byte + data = _port->read(); -restart: - switch(_step){ +restart: + switch(_step) { - // Message preamble, class, ID detection - // - // If we fail to match any of the expected bytes, we - // reset the state machine and re-consider the failed - // byte as the first byte of the preamble. This - // improves our chances of recovering from a mismatch - // and makes it less likely that we will be fooled by - // the preamble appearing as data in some other message. - // - case 0: - if(PREAMBLE1 == data) - _step++; - break; - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - goto restart; - case 2: - if (MESSAGE_CLASS == data) { - _step++; - _ck_b = _ck_a = data; // reset the checksum accumulators - } else { - _step = 0; // reset and wait for a message of the right class - goto restart; - } - break; - case 3: - if (MESSAGE_ID == data) { - _step++; - _ck_b += (_ck_a += data); - _payload_counter = 0; - } else { - _step = 0; - goto restart; - } - break; + // Message preamble, class, ID detection + // + // If we fail to match any of the expected bytes, we + // reset the state machine and re-consider the failed + // byte as the first byte of the preamble. This + // improves our chances of recovering from a mismatch + // and makes it less likely that we will be fooled by + // the preamble appearing as data in some other message. + // + case 0: + if(PREAMBLE1 == data) + _step++; + break; + case 1: + if (PREAMBLE2 == data) { + _step++; + break; + } + _step = 0; + goto restart; + case 2: + if (MESSAGE_CLASS == data) { + _step++; + _ck_b = _ck_a = data; // reset the checksum accumulators + } else { + _step = 0; // reset and wait for a message of the right class + goto restart; + } + break; + case 3: + if (MESSAGE_ID == data) { + _step++; + _ck_b += (_ck_a += data); + _payload_counter = 0; + } else { + _step = 0; + goto restart; + } + break; - // Receive message data - // - case 4: - _buffer.bytes[_payload_counter++] = data; - _ck_b += (_ck_a += data); - if (_payload_counter == sizeof(_buffer)) - _step++; - break; + // Receive message data + // + case 4: + _buffer.bytes[_payload_counter++] = data; + _ck_b += (_ck_a += data); + if (_payload_counter == sizeof(_buffer)) + _step++; + break; - // Checksum and message processing - // - case 5: - _step++; - if (_ck_a != data) { - _error("GPS_MTK: checksum error\n"); - _step = 0; - } - break; - case 6: - _step = 0; - if (_ck_b != data) { - _error("GPS_MTK: checksum error\n"); - break; - } + // Checksum and message processing + // + case 5: + _step++; + if (_ck_a != data) { + _error("GPS_MTK: checksum error\n"); + _step = 0; + } + break; + case 6: + _step = 0; + if (_ck_b != data) { + _error("GPS_MTK: checksum error\n"); + break; + } - fix = (_buffer.msg.fix_type == FIX_3D); - latitude = _swapl(&_buffer.msg.latitude) * 10; - longitude = _swapl(&_buffer.msg.longitude) * 10; - altitude = _swapl(&_buffer.msg.altitude); - ground_speed = _swapl(&_buffer.msg.ground_speed); - ground_course = _swapl(&_buffer.msg.ground_course) / 10000; - num_sats = _buffer.msg.satellites; - - // time from gps is UTC, but convert here to msToD - long time_utc = _swapl(&_buffer.msg.utc_time); - long temp = (time_utc/10000000); - time_utc -= temp*10000000; - time = temp * 3600000; - temp = (time_utc/100000); - time_utc -= temp*100000; - time += temp * 60000 + time_utc; + fix = (_buffer.msg.fix_type == FIX_3D); + latitude = _swapl(&_buffer.msg.latitude) * 10; + longitude = _swapl(&_buffer.msg.longitude) * 10; + altitude = _swapl(&_buffer.msg.altitude); + ground_speed = _swapl(&_buffer.msg.ground_speed); + ground_course = _swapl(&_buffer.msg.ground_course) / 10000; + num_sats = _buffer.msg.satellites; - parsed = true; - } - } - return parsed; + // time from gps is UTC, but convert here to msToD + long time_utc = _swapl(&_buffer.msg.utc_time); + long temp = (time_utc/10000000); + time_utc -= temp*10000000; + time = temp * 3600000; + temp = (time_utc/100000); + time_utc -= temp*100000; + time += temp * 60000 + time_utc; + + parsed = true; + } + } + return parsed; } diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 8f37a635d3..8697f23a26 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -20,52 +20,52 @@ class AP_GPS_MTK : public GPS { public: - AP_GPS_MTK(Stream *s); - virtual void init(void); - virtual bool read(void); + AP_GPS_MTK(Stream *s); + virtual void init(void); + virtual bool read(void); private: // XXX this is being ignored by the compiler #pragma pack(1) - struct diyd_mtk_msg { - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_time; - }; + struct diyd_mtk_msg { + int32_t latitude; + int32_t longitude; + int32_t altitude; + int32_t ground_speed; + int32_t ground_course; + uint8_t satellites; + uint8_t fix_type; + uint32_t utc_time; + }; // #pragma pack(pop) - enum diyd_mtk_fix_type { - FIX_NONE = 1, - FIX_2D = 2, - FIX_3D = 3 - }; + enum diyd_mtk_fix_type { + FIX_NONE = 1, + FIX_2D = 2, + FIX_3D = 3 + }; - enum diyd_mtk_protocol_bytes { - PREAMBLE1 = 0xb5, - PREAMBLE2 = 0x62, - MESSAGE_CLASS = 1, - MESSAGE_ID = 5 - }; + enum diyd_mtk_protocol_bytes { + PREAMBLE1 = 0xb5, + PREAMBLE2 = 0x62, + MESSAGE_CLASS = 1, + MESSAGE_ID = 5 + }; - // Packet checksum accumulators - uint8_t _ck_a; - uint8_t _ck_b; + // Packet checksum accumulators + uint8_t _ck_a; + uint8_t _ck_b; - // State machine state - uint8_t _step; - uint8_t _payload_counter; + // State machine state + uint8_t _step; + uint8_t _payload_counter; - // Receive buffer - union { - diyd_mtk_msg msg; - uint8_t bytes[]; - } _buffer; + // Receive buffer + union { + diyd_mtk_msg msg; + uint8_t bytes[]; + } _buffer; - // Buffer parse & GPS state update - void _parse_gps(); + // Buffer parse & GPS state update + void _parse_gps(); }; #endif // AP_GPS_MTK_H diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index bf7180f112..96cc189b9c 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -20,23 +20,23 @@ AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s) } // Public Methods ////////////////////////////////////////////////////////////// -void +void AP_GPS_MTK16::init(void) -{ - _port->flush(); +{ + _port->flush(); - // initialize serial port for binary protocol use - // XXX should assume binary, let GPS_AUTO handle dynamic config? - _port->print(MTK_SET_BINARY); + // initialize serial port for binary protocol use + // XXX should assume binary, let GPS_AUTO handle dynamic config? + _port->print(MTK_SET_BINARY); - // set 4Hz update rate - _port->print(MTK_OUTPUT_4HZ); - - // set initial epoch code - _epoch = TIME_OF_DAY; - _time_offset = 0; - _offset_calculated = false; - idleTimeout = 1200; + // set 4Hz update rate + _port->print(MTK_OUTPUT_4HZ); + + // set initial epoch code + _epoch = TIME_OF_DAY; + _time_offset = 0; + _offset_calculated = false; + idleTimeout = 1200; } // Process bytes available from the stream @@ -53,109 +53,109 @@ AP_GPS_MTK16::init(void) bool AP_GPS_MTK16::read(void) { - uint8_t data; - int numc; - bool parsed = false; + uint8_t data; + int numc; + bool parsed = false; - numc = _port->available(); - for (int i = 0; i < numc; i++) { // Process bytes received + numc = _port->available(); + for (int i = 0; i < numc; i++) { // Process bytes received - // read the next byte - data = _port->read(); + // read the next byte + data = _port->read(); -restart: - switch(_step){ +restart: + switch(_step) { - // Message preamble, class, ID detection - // - // If we fail to match any of the expected bytes, we - // reset the state machine and re-consider the failed - // byte as the first byte of the preamble. This - // improves our chances of recovering from a mismatch - // and makes it less likely that we will be fooled by - // the preamble appearing as data in some other message. - // - case 0: - if(PREAMBLE1 == data) - _step++; - break; - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - goto restart; - case 2: - if (sizeof(_buffer) == data) { - _step++; - _ck_b = _ck_a = data; // reset the checksum accumulators - _payload_counter = 0; - } else { - _step = 0; // reset and wait for a message of the right class - goto restart; - } - break; + // Message preamble, class, ID detection + // + // If we fail to match any of the expected bytes, we + // reset the state machine and re-consider the failed + // byte as the first byte of the preamble. This + // improves our chances of recovering from a mismatch + // and makes it less likely that we will be fooled by + // the preamble appearing as data in some other message. + // + case 0: + if(PREAMBLE1 == data) + _step++; + break; + case 1: + if (PREAMBLE2 == data) { + _step++; + break; + } + _step = 0; + goto restart; + case 2: + if (sizeof(_buffer) == data) { + _step++; + _ck_b = _ck_a = data; // reset the checksum accumulators + _payload_counter = 0; + } else { + _step = 0; // reset and wait for a message of the right class + goto restart; + } + break; - // Receive message data - // - case 3: - _buffer.bytes[_payload_counter++] = data; - _ck_b += (_ck_a += data); - if (_payload_counter == sizeof(_buffer)) - _step++; - break; + // Receive message data + // + case 3: + _buffer.bytes[_payload_counter++] = data; + _ck_b += (_ck_a += data); + if (_payload_counter == sizeof(_buffer)) + _step++; + break; - // Checksum and message processing - // - case 4: - _step++; - if (_ck_a != data) { - _step = 0; - } - break; - case 5: - _step = 0; - if (_ck_b != data) { - break; - } + // Checksum and message processing + // + case 4: + _step++; + if (_ck_a != data) { + _step = 0; + } + break; + case 5: + _step = 0; + if (_ck_b != data) { + break; + } - fix = _buffer.msg.fix_type == FIX_3D; - latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise - longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise - altitude = _buffer.msg.altitude; - ground_speed = _buffer.msg.ground_speed; - ground_course = _buffer.msg.ground_course; - num_sats = _buffer.msg.satellites; - hdop = _buffer.msg.hdop; - date = _buffer.msg.utc_date; - - // time from gps is UTC, but convert here to msToD - long time_utc = _buffer.msg.utc_time; - long temp = (time_utc/10000000); - time_utc -= temp*10000000; - time = temp * 3600000; - temp = (time_utc/100000); - time_utc -= temp*100000; - time += temp * 60000 + time_utc; + fix = _buffer.msg.fix_type == FIX_3D; + latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise + longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise + altitude = _buffer.msg.altitude; + ground_speed = _buffer.msg.ground_speed; + ground_course = _buffer.msg.ground_course; + num_sats = _buffer.msg.satellites; + hdop = _buffer.msg.hdop; + date = _buffer.msg.utc_date; - parsed = true; - - /* Waiting on clarification of MAVLink protocol! - if(!_offset_calculated && parsed) { - long tempd1 = date; - long day = tempd1/10000; - tempd1 -= day * 10000; - long month = tempd1/100; - long year = tempd1 - month * 100; - _time_offset = _calc_epoch_offset(day, month, year); - _epoch = UNIX_EPOCH; - _offset_calculated = TRUE; - } - */ - - } - } - return parsed; + // time from gps is UTC, but convert here to msToD + long time_utc = _buffer.msg.utc_time; + long temp = (time_utc/10000000); + time_utc -= temp*10000000; + time = temp * 3600000; + temp = (time_utc/100000); + time_utc -= temp*100000; + time += temp * 60000 + time_utc; + + parsed = true; + + /* Waiting on clarification of MAVLink protocol! + if(!_offset_calculated && parsed) { + long tempd1 = date; + long day = tempd1/10000; + tempd1 -= day * 10000; + long month = tempd1/100; + long year = tempd1 - month * 100; + _time_offset = _calc_epoch_offset(day, month, year); + _epoch = UNIX_EPOCH; + _offset_calculated = TRUE; + } + */ + + } + } + return parsed; } diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h index 46a7bceb23..07d939c084 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.h +++ b/libraries/AP_GPS/AP_GPS_MTK16.h @@ -18,53 +18,53 @@ class AP_GPS_MTK16 : public GPS { public: - AP_GPS_MTK16(Stream *s); - virtual void init(void); - virtual bool read(void); + AP_GPS_MTK16(Stream *s); + virtual void init(void); + virtual bool read(void); private: // XXX this is being ignored by the compiler #pragma pack(1) - struct diyd_mtk_msg { - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_date; - uint32_t utc_time; - uint16_t hdop; - }; + struct diyd_mtk_msg { + int32_t latitude; + int32_t longitude; + int32_t altitude; + int32_t ground_speed; + int32_t ground_course; + uint8_t satellites; + uint8_t fix_type; + uint32_t utc_date; + uint32_t utc_time; + uint16_t hdop; + }; // #pragma pack(pop) - enum diyd_mtk_fix_type { - FIX_NONE = 1, - FIX_2D = 2, - FIX_3D = 3 - }; + enum diyd_mtk_fix_type { + FIX_NONE = 1, + FIX_2D = 2, + FIX_3D = 3 + }; - enum diyd_mtk_protocol_bytes { - PREAMBLE1 = 0xd0, - PREAMBLE2 = 0xdd, - }; + enum diyd_mtk_protocol_bytes { + PREAMBLE1 = 0xd0, + PREAMBLE2 = 0xdd, + }; - // Packet checksum accumulators - uint8_t _ck_a; - uint8_t _ck_b; + // Packet checksum accumulators + uint8_t _ck_a; + uint8_t _ck_b; - // State machine state - uint8_t _step; - uint8_t _payload_counter; - - // Time from UNIX Epoch offset - long _time_offset; - bool _offset_calculated; + // State machine state + uint8_t _step; + uint8_t _payload_counter; - // Receive buffer - union { - diyd_mtk_msg msg; - uint8_t bytes[]; - } _buffer; + // Time from UNIX Epoch offset + long _time_offset; + bool _offset_calculated; + + // Receive buffer + union { + diyd_mtk_msg msg; + uint8_t bytes[]; + } _buffer; }; #endif // AP_GPS_MTK16_H diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 78e01fe8ab..473601b9d6 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -41,17 +41,17 @@ // the autodetection process. // const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM = - "$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz - "$PSRF103,1,0,0,1*25\r\n" // GLL off - "$PSRF103,2,0,0,1*26\r\n" // GSA off - "$PSRF103,3,0,0,1*27\r\n" // GSV off - "$PSRF103,4,0,1,1*20\r\n" // RMC off - "$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz - "$PSRF103,6,0,0,1*22\r\n" // MSS off - "$PSRF103,8,0,0,1*2C\r\n" // ZDA off - "$PSRF151,1*3F\r\n" // WAAS on (not always supported) - "$PSRF106,21*0F\r\n" // datum = WGS84 - ""; + "$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz + "$PSRF103,1,0,0,1*25\r\n" // GLL off + "$PSRF103,2,0,0,1*26\r\n" // GSA off + "$PSRF103,3,0,0,1*27\r\n" // GSV off + "$PSRF103,4,0,1,1*20\r\n" // RMC off + "$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz + "$PSRF103,6,0,0,1*22\r\n" // MSS off + "$PSRF103,8,0,0,1*2C\r\n" // ZDA off + "$PSRF151,1*3F\r\n" // WAAS on (not always supported) + "$PSRF106,21*0F\r\n" // datum = WGS84 + ""; // MediaTek init messages ////////////////////////////////////////////////////// // @@ -59,11 +59,11 @@ const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM = // MediaTek-based GPS. // const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM = - "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix - "$PMTK330,0*2E\r\n" // datum = WGS84 - "$PMTK313,1*2E\r\n" // SBAS on - "$PMTK301,2*2E\r\n" // use SBAS data for DGPS - ""; + "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix + "$PMTK330,0*2E\r\n" // datum = WGS84 + "$PMTK313,1*2E\r\n" // SBAS on + "$PMTK301,2*2E\r\n" // use SBAS data for DGPS + ""; // ublox init messages ///////////////////////////////////////////////////////// // @@ -75,10 +75,10 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM = // and we don't know the baudrate. // const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM = - "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix - "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix - "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?) - ""; + "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix + "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix + "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?) + ""; // NMEA message identifiers //////////////////////////////////////////////////// // @@ -92,83 +92,83 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG"; // Constructors //////////////////////////////////////////////////////////////// AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : - GPS(s) + GPS(s) { - FastSerial *fs = (FastSerial *)_port; + FastSerial *fs = (FastSerial *)_port; - // Re-open the port with enough receive buffering for the messages we expect - // and very little tx buffering, since we don't care about sending. - // Leave the port speed alone as we don't actually know at what rate we're running... - // - fs->begin(0, 200, 16); + // Re-open the port with enough receive buffering for the messages we expect + // and very little tx buffering, since we don't care about sending. + // Leave the port speed alone as we don't actually know at what rate we're running... + // + fs->begin(0, 200, 16); } // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_NMEA::init(void) { - BetterStream *bs = (BetterStream *)_port; + BetterStream *bs = (BetterStream *)_port; - // send the SiRF init strings - bs->print_P((const prog_char_t *)_SiRF_init_string); + // send the SiRF init strings + bs->print_P((const prog_char_t *)_SiRF_init_string); - // send the MediaTek init strings - bs->print_P((const prog_char_t *)_MTK_init_string); + // send the MediaTek init strings + bs->print_P((const prog_char_t *)_MTK_init_string); - // send the ublox init strings - bs->print_P((const prog_char_t *)_ublox_init_string); - - idleTimeout = 1200; + // send the ublox init strings + bs->print_P((const prog_char_t *)_ublox_init_string); + + idleTimeout = 1200; } bool AP_GPS_NMEA::read(void) { - int numc; - bool parsed = false; + int numc; + bool parsed = false; - numc = _port->available(); - while (numc--) { - if (_decode(_port->read())) { - parsed = true; - } - } - return parsed; + numc = _port->available(); + while (numc--) { + if (_decode(_port->read())) { + parsed = true; + } + } + return parsed; } bool AP_GPS_NMEA::_decode(char c) { - bool valid_sentence = false; + bool valid_sentence = false; - switch (c) { - case ',': // term terminators - _parity ^= c; - case '\r': - case '\n': - case '*': - if (_term_offset < sizeof(_term)) { - _term[_term_offset] = 0; - valid_sentence = _term_complete(); - } - ++_term_number; - _term_offset = 0; - _is_checksum_term = c == '*'; - return valid_sentence; + switch (c) { + case ',': // term terminators + _parity ^= c; + case '\r': + case '\n': + case '*': + if (_term_offset < sizeof(_term)) { + _term[_term_offset] = 0; + valid_sentence = _term_complete(); + } + ++_term_number; + _term_offset = 0; + _is_checksum_term = c == '*'; + return valid_sentence; - case '$': // sentence begin - _term_number = _term_offset = 0; - _parity = 0; - _sentence_type = _GPS_SENTENCE_OTHER; - _is_checksum_term = false; - _gps_data_good = false; - return valid_sentence; - } + case '$': // sentence begin + _term_number = _term_offset = 0; + _parity = 0; + _sentence_type = _GPS_SENTENCE_OTHER; + _is_checksum_term = false; + _gps_data_good = false; + return valid_sentence; + } - // ordinary characters - if (_term_offset < sizeof(_term) - 1) - _term[_term_offset++] = c; - if (!_is_checksum_term) - _parity ^= c; + // ordinary characters + if (_term_offset < sizeof(_term) - 1) + _term[_term_offset++] = c; + if (!_is_checksum_term) + _parity ^= c; - return valid_sentence; + return valid_sentence; } // @@ -176,203 +176,203 @@ bool AP_GPS_NMEA::_decode(char c) // int AP_GPS_NMEA::_from_hex(char a) { - if (a >= 'A' && a <= 'F') - return a - 'A' + 10; - else if (a >= 'a' && a <= 'f') - return a - 'a' + 10; - else - return a - '0'; + if (a >= 'A' && a <= 'F') + return a - 'A' + 10; + else if (a >= 'a' && a <= 'f') + return a - 'a' + 10; + else + return a - '0'; } unsigned long AP_GPS_NMEA::_parse_decimal() { - char *p = _term; - unsigned long ret = 100UL * atol(p); - while (isdigit(*p)) - ++p; - if (*p == '.') { - if (isdigit(p[1])) { - ret += 10 * (p[1] - '0'); - if (isdigit(p[2])) - ret += p[2] - '0'; - } - } - return ret; + char *p = _term; + unsigned long ret = 100UL * atol(p); + while (isdigit(*p)) + ++p; + if (*p == '.') { + if (isdigit(p[1])) { + ret += 10 * (p[1] - '0'); + if (isdigit(p[2])) + ret += p[2] - '0'; + } + } + return ret; } unsigned long AP_GPS_NMEA::_parse_degrees() { - char *p, *q; - uint8_t deg = 0, min = 0; - unsigned int frac_min = 0; + char *p, *q; + uint8_t deg = 0, min = 0; + unsigned int frac_min = 0; - // scan for decimal point or end of field - for (p = _term; isdigit(*p); p++) - ; - q = _term; + // scan for decimal point or end of field + for (p = _term; isdigit(*p); p++) + ; + q = _term; - // convert degrees - while ((p - q) > 2) { - if (deg) - deg *= 10; - deg += DIGIT_TO_VAL(*q++); - } + // convert degrees + while ((p - q) > 2) { + if (deg) + deg *= 10; + deg += DIGIT_TO_VAL(*q++); + } - // convert minutes - while (p > q) { - if (min) - min *= 10; - min += DIGIT_TO_VAL(*q++); - } + // convert minutes + while (p > q) { + if (min) + min *= 10; + min += DIGIT_TO_VAL(*q++); + } - // convert fractional minutes - // expect up to four digits, result is in - // ten-thousandths of a minute - if (*p == '.') { - q = p + 1; - for (int i = 0; i < 4; i++) { - frac_min *= 10; - if (isdigit(*q)) - frac_min += *q++ - '0'; - } - } - return deg * 100000UL + (min * 10000UL + frac_min) / 6; + // convert fractional minutes + // expect up to four digits, result is in + // ten-thousandths of a minute + if (*p == '.') { + q = p + 1; + for (int i = 0; i < 4; i++) { + frac_min *= 10; + if (isdigit(*q)) + frac_min += *q++ - '0'; + } + } + return deg * 100000UL + (min * 10000UL + frac_min) / 6; } // Processes a just-completed term // Returns true if new sentence has just passed checksum test and is validated bool AP_GPS_NMEA::_term_complete() { - // handle the last term in a message - if (_is_checksum_term) { - uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]); - if (checksum == _parity) { - if (_gps_data_good) { - switch (_sentence_type) { - case _GPS_SENTENCE_GPRMC: - time = _new_time; - date = _new_date; - latitude = _new_latitude * 100; // degrees*10e5 -> 10e7 - longitude = _new_longitude * 100; // degrees*10e5 -> 10e7 - ground_speed = _new_speed; - ground_course = _new_course; - fix = true; - break; - case _GPS_SENTENCE_GPGGA: - altitude = _new_altitude; - time = _new_time; - latitude = _new_latitude * 100; // degrees*10e5 -> 10e7 - longitude = _new_longitude * 100; // degrees*10e5 -> 10e7 - num_sats = _new_satellite_count; - hdop = _new_hdop; - fix = true; - break; - case _GPS_SENTENCE_GPVTG: - ground_speed = _new_speed; - ground_course = _new_course; - // VTG has no fix indicator, can't change fix status - break; - } - } else { - switch (_sentence_type) { - case _GPS_SENTENCE_GPRMC: - case _GPS_SENTENCE_GPGGA: - // Only these sentences give us information about - // fix status. - fix = false; - } - } - // we got a good message - return true; - } - // we got a bad message, ignore it - return false; - } + // handle the last term in a message + if (_is_checksum_term) { + uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]); + if (checksum == _parity) { + if (_gps_data_good) { + switch (_sentence_type) { + case _GPS_SENTENCE_GPRMC: + time = _new_time; + date = _new_date; + latitude = _new_latitude * 100; // degrees*10e5 -> 10e7 + longitude = _new_longitude * 100; // degrees*10e5 -> 10e7 + ground_speed = _new_speed; + ground_course = _new_course; + fix = true; + break; + case _GPS_SENTENCE_GPGGA: + altitude = _new_altitude; + time = _new_time; + latitude = _new_latitude * 100; // degrees*10e5 -> 10e7 + longitude = _new_longitude * 100; // degrees*10e5 -> 10e7 + num_sats = _new_satellite_count; + hdop = _new_hdop; + fix = true; + break; + case _GPS_SENTENCE_GPVTG: + ground_speed = _new_speed; + ground_course = _new_course; + // VTG has no fix indicator, can't change fix status + break; + } + } else { + switch (_sentence_type) { + case _GPS_SENTENCE_GPRMC: + case _GPS_SENTENCE_GPGGA: + // Only these sentences give us information about + // fix status. + fix = false; + } + } + // we got a good message + return true; + } + // we got a bad message, ignore it + return false; + } - // the first term determines the sentence type - if (_term_number == 0) { - if (!strcmp_P(_term, _gprmc_string)) { - _sentence_type = _GPS_SENTENCE_GPRMC; - } else if (!strcmp_P(_term, _gpgga_string)) { - _sentence_type = _GPS_SENTENCE_GPGGA; - } else if (!strcmp_P(_term, _gpvtg_string)) { - _sentence_type = _GPS_SENTENCE_GPVTG; - // VTG may not contain a data qualifier, presume the solution is good - // unless it tells us otherwise. - _gps_data_good = true; - } else { - _sentence_type = _GPS_SENTENCE_OTHER; - } - return false; - } + // the first term determines the sentence type + if (_term_number == 0) { + if (!strcmp_P(_term, _gprmc_string)) { + _sentence_type = _GPS_SENTENCE_GPRMC; + } else if (!strcmp_P(_term, _gpgga_string)) { + _sentence_type = _GPS_SENTENCE_GPGGA; + } else if (!strcmp_P(_term, _gpvtg_string)) { + _sentence_type = _GPS_SENTENCE_GPVTG; + // VTG may not contain a data qualifier, presume the solution is good + // unless it tells us otherwise. + _gps_data_good = true; + } else { + _sentence_type = _GPS_SENTENCE_OTHER; + } + return false; + } - // 32 = RMC, 64 = GGA, 96 = VTG - if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) { - switch (_sentence_type + _term_number) { - // operational status - // - case _GPS_SENTENCE_GPRMC + 2: // validity (RMC) - _gps_data_good = _term[0] == 'A'; - break; - case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA) - _gps_data_good = _term[0] > '0'; - break; - case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field) - _gps_data_good = _term[0] != 'N'; - break; - case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA) - _new_satellite_count = atol(_term); - break; - case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA) - _new_hdop = _parse_decimal(); - break; + // 32 = RMC, 64 = GGA, 96 = VTG + if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) { + switch (_sentence_type + _term_number) { + // operational status + // + case _GPS_SENTENCE_GPRMC + 2: // validity (RMC) + _gps_data_good = _term[0] == 'A'; + break; + case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA) + _gps_data_good = _term[0] > '0'; + break; + case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field) + _gps_data_good = _term[0] != 'N'; + break; + case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA) + _new_satellite_count = atol(_term); + break; + case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA) + _new_hdop = _parse_decimal(); + break; - // time and date - // - case _GPS_SENTENCE_GPRMC + 1: // Time (RMC) - case _GPS_SENTENCE_GPGGA + 1: // Time (GGA) - _new_time = _parse_decimal(); - break; - case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC) - _new_date = atol(_term); - break; + // time and date + // + case _GPS_SENTENCE_GPRMC + 1: // Time (RMC) + case _GPS_SENTENCE_GPGGA + 1: // Time (GGA) + _new_time = _parse_decimal(); + break; + case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC) + _new_date = atol(_term); + break; - // location - // - case _GPS_SENTENCE_GPRMC + 3: // Latitude - case _GPS_SENTENCE_GPGGA + 2: - _new_latitude = _parse_degrees(); - break; - case _GPS_SENTENCE_GPRMC + 4: // N/S - case _GPS_SENTENCE_GPGGA + 3: - if (_term[0] == 'S') - _new_latitude = -_new_latitude; - break; - case _GPS_SENTENCE_GPRMC + 5: // Longitude - case _GPS_SENTENCE_GPGGA + 4: - _new_longitude = _parse_degrees(); - break; - case _GPS_SENTENCE_GPRMC + 6: // E/W - case _GPS_SENTENCE_GPGGA + 5: - if (_term[0] == 'W') - _new_longitude = -_new_longitude; - break; - case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA) - _new_altitude = _parse_decimal(); - break; + // location + // + case _GPS_SENTENCE_GPRMC + 3: // Latitude + case _GPS_SENTENCE_GPGGA + 2: + _new_latitude = _parse_degrees(); + break; + case _GPS_SENTENCE_GPRMC + 4: // N/S + case _GPS_SENTENCE_GPGGA + 3: + if (_term[0] == 'S') + _new_latitude = -_new_latitude; + break; + case _GPS_SENTENCE_GPRMC + 5: // Longitude + case _GPS_SENTENCE_GPGGA + 4: + _new_longitude = _parse_degrees(); + break; + case _GPS_SENTENCE_GPRMC + 6: // E/W + case _GPS_SENTENCE_GPGGA + 5: + if (_term[0] == 'W') + _new_longitude = -_new_longitude; + break; + case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA) + _new_altitude = _parse_decimal(); + break; - // course and speed - // - case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC) - case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG) - _new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514 - break; - case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC) - case _GPS_SENTENCE_GPVTG + 1: // Course (VTG) - _new_course = _parse_decimal(); - break; - } - } + // course and speed + // + case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC) + case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG) + _new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514 + break; + case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC) + case _GPS_SENTENCE_GPVTG + 1: // Course (VTG) + _new_course = _parse_decimal(); + break; + } + } - return false; + return false; } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index e62483b58f..546480e97e 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -51,72 +51,72 @@ class AP_GPS_NMEA : public GPS { public: - /// Constructor - /// - AP_GPS_NMEA(Stream *s); + /// Constructor + /// + AP_GPS_NMEA(Stream *s); - /// Perform a (re)initialisation of the GPS; sends the - /// protocol configuration messages. - /// - virtual void init(); + /// Perform a (re)initialisation of the GPS; sends the + /// protocol configuration messages. + /// + virtual void init(); - /// Checks the serial receive buffer for characters, - /// attempts to parse NMEA data and updates internal state - /// accordingly. - /// - virtual bool read(); + /// Checks the serial receive buffer for characters, + /// attempts to parse NMEA data and updates internal state + /// accordingly. + /// + virtual bool read(); private: - /// Coding for the GPS sentences that the parser handles + /// Coding for the GPS sentences that the parser handles enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value. - _GPS_SENTENCE_GPRMC = 32, - _GPS_SENTENCE_GPGGA = 64, - _GPS_SENTENCE_GPVTG = 96, - _GPS_SENTENCE_OTHER = 0 + _GPS_SENTENCE_GPRMC = 32, + _GPS_SENTENCE_GPGGA = 64, + _GPS_SENTENCE_GPVTG = 96, + _GPS_SENTENCE_OTHER = 0 }; - /// Update the decode state machine with a new character - /// - /// @param c The next character in the NMEA input stream - /// @returns True if processing the character has resulted in - /// an update to the GPS state - /// - bool _decode(char c); + /// Update the decode state machine with a new character + /// + /// @param c The next character in the NMEA input stream + /// @returns True if processing the character has resulted in + /// an update to the GPS state + /// + bool _decode(char c); - /// Return the numeric value of an ascii hex character - /// - /// @param a The character to be converted - /// @returns The value of the character as a hex digit - /// - int _from_hex(char a); + /// Return the numeric value of an ascii hex character + /// + /// @param a The character to be converted + /// @returns The value of the character as a hex digit + /// + int _from_hex(char a); - /// Parses the current term as a NMEA-style decimal number with - /// up to two decimal digits. - /// - /// @returns The value expressed by the string in _term, - /// multiplied by 100. - /// - unsigned long _parse_decimal(); + /// Parses the current term as a NMEA-style decimal number with + /// up to two decimal digits. + /// + /// @returns The value expressed by the string in _term, + /// multiplied by 100. + /// + unsigned long _parse_decimal(); - /// Parses the current term as a NMEA-style degrees + minutes - /// value with up to four decimal digits. - /// - /// This gives a theoretical resolution limit of around 18cm. - /// - /// @returns The value expressed by the string in _term, - /// multiplied by 10000. - /// - unsigned long _parse_degrees(); + /// Parses the current term as a NMEA-style degrees + minutes + /// value with up to four decimal digits. + /// + /// This gives a theoretical resolution limit of around 18cm. + /// + /// @returns The value expressed by the string in _term, + /// multiplied by 10000. + /// + unsigned long _parse_degrees(); - /// Processes the current term when it has been deemed to be - /// complete. - /// - /// Each GPS message is broken up into terms separated by commas. - /// Each term is then processed by this function as it is received. - /// - /// @returns True if completing the term has resulted in - /// an update to the GPS state. - bool _term_complete(); + /// Processes the current term when it has been deemed to be + /// complete. + /// + /// Each GPS message is broken up into terms separated by commas. + /// Each term is then processed by this function as it is received. + /// + /// @returns True if completing the term has resulted in + /// an update to the GPS state. + bool _term_complete(); uint8_t _parity; ///< NMEA message checksum accumulator bool _is_checksum_term; ///< current term is the checksum diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h index 6b93cd83e8..17202f5180 100644 --- a/libraries/AP_GPS/AP_GPS_None.h +++ b/libraries/AP_GPS/AP_GPS_None.h @@ -8,8 +8,10 @@ class AP_GPS_None : public GPS { public: - AP_GPS_None(Stream *s) : GPS(s) {} - virtual void init(void) {}; - virtual bool read(void) { return false; }; + AP_GPS_None(Stream *s) : GPS(s) {} + virtual void init(void) {}; + virtual bool read(void) { + return false; + }; }; #endif diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index 4b247153ca..e08b5d0eda 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -19,8 +19,8 @@ // XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them. // static uint8_t init_messages[] = { - 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3, - 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 + 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3, + 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 }; // Constructors //////////////////////////////////////////////////////////////// @@ -29,18 +29,18 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s) } // Public Methods ////////////////////////////////////////////////////////////// -void +void AP_GPS_SIRF::init(void) -{ - _port->flush(); +{ + _port->flush(); - // For modules that default to something other than SiRF binary, - // the module-specific subclass should take care of switching to binary mode - // before calling us. + // For modules that default to something other than SiRF binary, + // the module-specific subclass should take care of switching to binary mode + // before calling us. - // send SiRF binary setup messages - _port->write(init_messages, sizeof(init_messages)); - idleTimeout = 1200; + // send SiRF binary setup messages + _port->write(init_messages, sizeof(init_messages)); + idleTimeout = 1200; } // Process bytes available from the stream @@ -55,143 +55,143 @@ AP_GPS_SIRF::init(void) bool AP_GPS_SIRF::read(void) { - uint8_t data; - int numc; - bool parsed = false; + uint8_t data; + int numc; + bool parsed = false; - numc = _port->available(); - while(numc--) { + numc = _port->available(); + while(numc--) { - // read the next byte - data = _port->read(); + // read the next byte + data = _port->read(); - switch(_step){ + switch(_step) { - // Message preamble detection - // - // If we fail to match any of the expected bytes, we reset - // the state machine and re-consider the failed byte as - // the first byte of the preamble. This improves our - // chances of recovering from a mismatch and makes it less - // likely that we will be fooled by the preamble appearing - // as data in some other message. - // - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - // FALLTHROUGH - case 0: - if(PREAMBLE1 == data) - _step++; - break; + // Message preamble detection + // + // If we fail to match any of the expected bytes, we reset + // the state machine and re-consider the failed byte as + // the first byte of the preamble. This improves our + // chances of recovering from a mismatch and makes it less + // likely that we will be fooled by the preamble appearing + // as data in some other message. + // + case 1: + if (PREAMBLE2 == data) { + _step++; + break; + } + _step = 0; + // FALLTHROUGH + case 0: + if(PREAMBLE1 == data) + _step++; + break; - // Message length - // - // We always collect the length so that we can avoid being - // fooled by preamble bytes in messages. - // - case 2: - _step++; - _payload_length = (uint16_t)data << 8; - break; - case 3: - _step++; - _payload_length |= data; - _payload_counter = 0; - _checksum = 0; - break; + // Message length + // + // We always collect the length so that we can avoid being + // fooled by preamble bytes in messages. + // + case 2: + _step++; + _payload_length = (uint16_t)data << 8; + break; + case 3: + _step++; + _payload_length |= data; + _payload_counter = 0; + _checksum = 0; + break; - // Message header processing - // - // We sniff the message ID to determine whether we are going - // to gather the message bytes or just discard them. - // - case 4: - _step++; - _accumulate(data); - _payload_length--; - _gather = false; - switch(data) { - case MSG_GEONAV: - if (_payload_length == sizeof(sirf_geonav)) { - _gather = true; - _msg_id = data; - } - break; - } - break; + // Message header processing + // + // We sniff the message ID to determine whether we are going + // to gather the message bytes or just discard them. + // + case 4: + _step++; + _accumulate(data); + _payload_length--; + _gather = false; + switch(data) { + case MSG_GEONAV: + if (_payload_length == sizeof(sirf_geonav)) { + _gather = true; + _msg_id = data; + } + break; + } + break; - // Receive message data - // - // Note that we are effectively guaranteed by the protocol - // that the checksum and postamble cannot be mistaken for - // the preamble, so if we are discarding bytes in this - // message when the payload is done we return directly - // to the preamble detector rather than bothering with - // the checksum logic. - // - case 5: - if (_gather) { // gather data if requested - _accumulate(data); - _buffer.bytes[_payload_counter] = data; - if (++_payload_counter == _payload_length) - _step++; - } else { - if (++_payload_counter == _payload_length) - _step = 0; - } - break; + // Receive message data + // + // Note that we are effectively guaranteed by the protocol + // that the checksum and postamble cannot be mistaken for + // the preamble, so if we are discarding bytes in this + // message when the payload is done we return directly + // to the preamble detector rather than bothering with + // the checksum logic. + // + case 5: + if (_gather) { // gather data if requested + _accumulate(data); + _buffer.bytes[_payload_counter] = data; + if (++_payload_counter == _payload_length) + _step++; + } else { + if (++_payload_counter == _payload_length) + _step = 0; + } + break; - // Checksum and message processing - // - case 6: - _step++; - if ((_checksum >> 8) != data) { - _error("GPS_SIRF: checksum error\n"); - _step = 0; - } - break; - case 7: - _step = 0; - if ((_checksum & 0xff) != data) { - _error("GPS_SIRF: checksum error\n"); - break; - } - if (_gather) { - parsed = _parse_gps(); // Parse the new GPS packet - } - } - } - return(parsed); + // Checksum and message processing + // + case 6: + _step++; + if ((_checksum >> 8) != data) { + _error("GPS_SIRF: checksum error\n"); + _step = 0; + } + break; + case 7: + _step = 0; + if ((_checksum & 0xff) != data) { + _error("GPS_SIRF: checksum error\n"); + break; + } + if (_gather) { + parsed = _parse_gps(); // Parse the new GPS packet + } + } + } + return(parsed); } bool AP_GPS_SIRF::_parse_gps(void) { - switch(_msg_id) { - case MSG_GEONAV: - time = _swapl(&_buffer.nav.time); - //fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK)); - fix = (0 == _buffer.nav.fix_invalid); - latitude = _swapl(&_buffer.nav.latitude); - longitude = _swapl(&_buffer.nav.longitude); - altitude = _swapl(&_buffer.nav.altitude_msl); - ground_speed = _swapi(&_buffer.nav.ground_speed); - // at low speeds, ground course wanders wildly; suppress changes if we are not moving - if (ground_speed > 50) - ground_course = _swapi(&_buffer.nav.ground_course); - num_sats = _buffer.nav.satellites; + switch(_msg_id) { + case MSG_GEONAV: + time = _swapl(&_buffer.nav.time); + //fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK)); + fix = (0 == _buffer.nav.fix_invalid); + latitude = _swapl(&_buffer.nav.latitude); + longitude = _swapl(&_buffer.nav.longitude); + altitude = _swapl(&_buffer.nav.altitude_msl); + ground_speed = _swapi(&_buffer.nav.ground_speed); + // at low speeds, ground course wanders wildly; suppress changes if we are not moving + if (ground_speed > 50) + ground_course = _swapi(&_buffer.nav.ground_course); + num_sats = _buffer.nav.satellites; - return true; - } - return false; + return true; + } + return false; } void AP_GPS_SIRF::_accumulate(uint8_t val) { - _checksum = (_checksum + val) & 0x7fff; + _checksum = (_checksum + val) & 0x7fff; } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index 483d7cefca..702aea9610 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -17,80 +17,80 @@ class AP_GPS_SIRF : public GPS { public: - AP_GPS_SIRF(Stream *s); + AP_GPS_SIRF(Stream *s); - virtual void init(); - virtual bool read(); + virtual void init(); + virtual bool read(); private: // XXX this is being ignored by the compiler #pragma pack(1) - struct sirf_geonav { - uint16_t fix_invalid; - uint16_t fix_type; - uint16_t week; - uint32_t time; - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t minute; - uint16_t second; - uint32_t satellites_used; - int32_t latitude; - int32_t longitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - int8_t map_datum; - int16_t ground_speed; - int16_t ground_course; - int16_t res1; - int16_t climb_rate; - uint16_t heading_rate; - uint32_t horizontal_position_error; - uint32_t vertical_position_error; - uint32_t time_error; - int16_t horizontal_velocity_error; - int32_t clock_bias; - uint32_t clock_bias_error; - int32_t clock_drift; - uint32_t clock_drift_error; - uint32_t distance; - uint16_t distance_error; - uint16_t heading_error; - uint8_t satellites; - uint8_t hdop; - uint8_t mode_info; - }; + struct sirf_geonav { + uint16_t fix_invalid; + uint16_t fix_type; + uint16_t week; + uint32_t time; + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint16_t second; + uint32_t satellites_used; + int32_t latitude; + int32_t longitude; + int32_t altitude_ellipsoid; + int32_t altitude_msl; + int8_t map_datum; + int16_t ground_speed; + int16_t ground_course; + int16_t res1; + int16_t climb_rate; + uint16_t heading_rate; + uint32_t horizontal_position_error; + uint32_t vertical_position_error; + uint32_t time_error; + int16_t horizontal_velocity_error; + int32_t clock_bias; + uint32_t clock_bias_error; + int32_t clock_drift; + uint32_t clock_drift_error; + uint32_t distance; + uint16_t distance_error; + uint16_t heading_error; + uint8_t satellites; + uint8_t hdop; + uint8_t mode_info; + }; // #pragma pack(pop) - enum sirf_protocol_bytes { - PREAMBLE1 = 0xa0, - PREAMBLE2 = 0xa2, - POSTAMBLE1 = 0xb0, - POSTAMBLE2 = 0xb3, - MSG_GEONAV = 0x29 - }; - enum sirf_fix_type { - FIX_3D = 0x6, - FIX_MASK = 0x7 - }; + enum sirf_protocol_bytes { + PREAMBLE1 = 0xa0, + PREAMBLE2 = 0xa2, + POSTAMBLE1 = 0xb0, + POSTAMBLE2 = 0xb3, + MSG_GEONAV = 0x29 + }; + enum sirf_fix_type { + FIX_3D = 0x6, + FIX_MASK = 0x7 + }; - // State machine state - uint8_t _step; - uint16_t _checksum; - bool _gather; - uint16_t _payload_length; - uint16_t _payload_counter; - uint8_t _msg_id; + // State machine state + uint8_t _step; + uint16_t _checksum; + bool _gather; + uint16_t _payload_length; + uint16_t _payload_counter; + uint8_t _msg_id; - // Message buffer - union { - sirf_geonav nav; - uint8_t bytes[]; - } _buffer; + // Message buffer + union { + sirf_geonav nav; + uint8_t bytes[]; + } _buffer; - bool _parse_gps(void); - void _accumulate(uint8_t val); + bool _parse_gps(void); + void _accumulate(uint8_t val); }; #endif // AP_GPS_SIRF_h diff --git a/libraries/AP_GPS/AP_GPS_Shim.h b/libraries/AP_GPS/AP_GPS_Shim.h index c96ceb927e..38ffc7c568 100644 --- a/libraries/AP_GPS/AP_GPS_Shim.h +++ b/libraries/AP_GPS/AP_GPS_Shim.h @@ -17,31 +17,31 @@ class AP_GPS_Shim : public GPS { public: - AP_GPS_Shim() : GPS(NULL) {} + AP_GPS_Shim() : GPS(NULL) {} - virtual void init(void) {}; - virtual bool read(void) { - bool updated = _updated; - _updated = false; - return updated; - } + virtual void init(void) {}; + virtual bool read(void) { + bool updated = _updated; + _updated = false; + return updated; + } - /// Set-and-mark-updated macro for the public member variables; each instance - /// defines a member function set_() - /// + /// Set-and-mark-updated macro for the public member variables; each instance + /// defines a member function set_() + /// #define __GPS_SHIM_SET(__type, __name) void set_##__name(__type v) { __name = v; _updated = true; } - __GPS_SHIM_SET(long, time); - __GPS_SHIM_SET(long, latitude); - __GPS_SHIM_SET(long, longitude); - __GPS_SHIM_SET(long, altitude); - __GPS_SHIM_SET(long, ground_speed); - __GPS_SHIM_SET(long, ground_course); - __GPS_SHIM_SET(long, speed_3d); - __GPS_SHIM_SET(int, hdop); + __GPS_SHIM_SET(long, time); + __GPS_SHIM_SET(long, latitude); + __GPS_SHIM_SET(long, longitude); + __GPS_SHIM_SET(long, altitude); + __GPS_SHIM_SET(long, ground_speed); + __GPS_SHIM_SET(long, ground_course); + __GPS_SHIM_SET(long, speed_3d); + __GPS_SHIM_SET(int, hdop); #undef __GPS_SHIM_SET private: - bool _updated; ///< set anytime a member is updated, cleared when read() returns true + bool _updated; ///< set anytime a member is updated, cleared when read() returns true }; #endif // AP_GPS_HIL_H diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 74339f87de..2c991e293c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -23,13 +23,13 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) void AP_GPS_UBLOX::init(void) { - // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the - // right reporting configuration. + // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the + // right reporting configuration. - _port->flush(); - - _epoch = TIME_OF_WEEK; - idleTimeout = 1200; + _port->flush(); + + _epoch = TIME_OF_WEEK; + idleTimeout = 1200; } // Process bytes available from the stream @@ -44,122 +44,122 @@ AP_GPS_UBLOX::init(void) bool AP_GPS_UBLOX::read(void) { - uint8_t data; - int numc; - bool parsed = false; + uint8_t data; + int numc; + bool parsed = false; - numc = _port->available(); - for (int i = 0; i < numc; i++){ // Process bytes received + numc = _port->available(); + for (int i = 0; i < numc; i++) { // Process bytes received - // read the next byte - data = _port->read(); + // read the next byte + data = _port->read(); - switch(_step){ + switch(_step) { - // Message preamble detection - // - // If we fail to match any of the expected bytes, we reset - // the state machine and re-consider the failed byte as - // the first byte of the preamble. This improves our - // chances of recovering from a mismatch and makes it less - // likely that we will be fooled by the preamble appearing - // as data in some other message. - // - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - // FALLTHROUGH - case 0: - if(PREAMBLE1 == data) - _step++; - break; + // Message preamble detection + // + // If we fail to match any of the expected bytes, we reset + // the state machine and re-consider the failed byte as + // the first byte of the preamble. This improves our + // chances of recovering from a mismatch and makes it less + // likely that we will be fooled by the preamble appearing + // as data in some other message. + // + case 1: + if (PREAMBLE2 == data) { + _step++; + break; + } + _step = 0; + // FALLTHROUGH + case 0: + if(PREAMBLE1 == data) + _step++; + break; - // Message header processing - // - // We sniff the class and message ID to decide whether we - // are going to gather the message bytes or just discard - // them. - // - // We always collect the length so that we can avoid being - // fooled by preamble bytes in messages. - // - case 2: - _step++; - if (CLASS_NAV == data) { - _gather = true; // class is interesting, maybe gather - _ck_b = _ck_a = data; // reset the checksum accumulators - } else { - _gather = false; // class is not interesting, discard - } - break; - case 3: - _step++; - _ck_b += (_ck_a += data); // checksum byte - _msg_id = data; - if (_gather) { // if class was interesting - switch(data) { - case MSG_POSLLH: // message is interesting - _expect = sizeof(ubx_nav_posllh); - break; - case MSG_STATUS: - _expect = sizeof(ubx_nav_status); - break; - case MSG_SOL: - _expect = sizeof(ubx_nav_solution); - break; - case MSG_VELNED: - _expect = sizeof(ubx_nav_velned); - break; - default: - _gather = false; // message is not interesting - } - } - break; - case 4: - _step++; - _ck_b += (_ck_a += data); // checksum byte - _payload_length = data; // payload length low byte - break; - case 5: - _step++; - _ck_b += (_ck_a += data); // checksum byte - _payload_length += (uint16_t)data; // payload length high byte - _payload_counter = 0; // prepare to receive payload - if (_payload_length != _expect) - _gather = false; - break; + // Message header processing + // + // We sniff the class and message ID to decide whether we + // are going to gather the message bytes or just discard + // them. + // + // We always collect the length so that we can avoid being + // fooled by preamble bytes in messages. + // + case 2: + _step++; + if (CLASS_NAV == data) { + _gather = true; // class is interesting, maybe gather + _ck_b = _ck_a = data; // reset the checksum accumulators + } else { + _gather = false; // class is not interesting, discard + } + break; + case 3: + _step++; + _ck_b += (_ck_a += data); // checksum byte + _msg_id = data; + if (_gather) { // if class was interesting + switch(data) { + case MSG_POSLLH: // message is interesting + _expect = sizeof(ubx_nav_posllh); + break; + case MSG_STATUS: + _expect = sizeof(ubx_nav_status); + break; + case MSG_SOL: + _expect = sizeof(ubx_nav_solution); + break; + case MSG_VELNED: + _expect = sizeof(ubx_nav_velned); + break; + default: + _gather = false; // message is not interesting + } + } + break; + case 4: + _step++; + _ck_b += (_ck_a += data); // checksum byte + _payload_length = data; // payload length low byte + break; + case 5: + _step++; + _ck_b += (_ck_a += data); // checksum byte + _payload_length += (uint16_t)data; // payload length high byte + _payload_counter = 0; // prepare to receive payload + if (_payload_length != _expect) + _gather = false; + break; - // Receive message data - // - case 6: - _ck_b += (_ck_a += data); // checksum byte - if (_gather) // gather data if requested - _buffer.bytes[_payload_counter] = data; - if (++_payload_counter == _payload_length) - _step++; - break; + // Receive message data + // + case 6: + _ck_b += (_ck_a += data); // checksum byte + if (_gather) // gather data if requested + _buffer.bytes[_payload_counter] = data; + if (++_payload_counter == _payload_length) + _step++; + break; - // Checksum and message processing - // - case 7: - _step++; - if (_ck_a != data) - _step = 0; // bad checksum - break; - case 8: - _step = 0; - if (_ck_b != data) - break; // bad checksum + // Checksum and message processing + // + case 7: + _step++; + if (_ck_a != data) + _step = 0; // bad checksum + break; + case 8: + _step = 0; + if (_ck_b != data) + break; // bad checksum - if (_gather) { - parsed = _parse_gps(); // Parse the new GPS packet - } - } - } - return parsed; + if (_gather) { + parsed = _parse_gps(); // Parse the new GPS packet + } + } + } + return parsed; } // Private Methods ///////////////////////////////////////////////////////////// @@ -167,28 +167,28 @@ AP_GPS_UBLOX::read(void) bool AP_GPS_UBLOX::_parse_gps(void) { - switch (_msg_id) { - case MSG_POSLLH: - time = _buffer.posllh.time; - longitude = _buffer.posllh.longitude; - latitude = _buffer.posllh.latitude; - altitude = _buffer.posllh.altitude_msl / 10; - break; - case MSG_STATUS: - fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); - break; - case MSG_SOL: - fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); - num_sats = _buffer.solution.satellites; - hdop = _buffer.solution.position_DOP; - break; - case MSG_VELNED: - speed_3d = _buffer.velned.speed_3d; // cm/s - ground_speed = _buffer.velned.speed_2d; // cm/s - ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100 - break; - default: - return false; - } - return true; + switch (_msg_id) { + case MSG_POSLLH: + time = _buffer.posllh.time; + longitude = _buffer.posllh.longitude; + latitude = _buffer.posllh.latitude; + altitude = _buffer.posllh.altitude_msl / 10; + break; + case MSG_STATUS: + fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); + break; + case MSG_SOL: + fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); + num_sats = _buffer.solution.satellites; + hdop = _buffer.solution.position_DOP; + break; + case MSG_VELNED: + speed_3d = _buffer.velned.speed_3d; // cm/s + ground_speed = _buffer.velned.speed_2d; // cm/s + ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100 + break; + default: + return false; + } + return true; } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 02fc0f7be4..ac05169b36 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -19,106 +19,106 @@ class AP_GPS_UBLOX : public GPS { public: // Methods - AP_GPS_UBLOX(Stream *s); - virtual void init(); - virtual bool read(); + AP_GPS_UBLOX(Stream *s); + virtual void init(); + virtual bool read(); private: - // u-blox UBX protocol essentials + // u-blox UBX protocol essentials // XXX this is being ignored by the compiler #pragma pack(1) - struct ubx_nav_posllh { - uint32_t time; // GPS msToW - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; - }; - struct ubx_nav_status { - uint32_t time; // GPS msToW - uint8_t fix_type; - uint8_t fix_status; - uint8_t differential_status; - uint8_t res; - uint32_t time_to_first_fix; - uint32_t uptime; // milliseconds - }; - struct ubx_nav_solution { - uint32_t time; - int32_t time_nsec; - int16_t week; - uint8_t fix_type; - uint8_t fix_status; - int32_t ecef_x; - int32_t ecef_y; - int32_t ecef_z; - uint32_t position_accuracy_3d; - int32_t ecef_x_velocity; - int32_t ecef_y_velocity; - int32_t ecef_z_velocity; - uint32_t speed_accuracy; - uint16_t position_DOP; - uint8_t res; - uint8_t satellites; - uint32_t res2; - }; - struct ubx_nav_velned { - uint32_t time; // GPS msToW - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - uint32_t speed_3d; - uint32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; - }; + struct ubx_nav_posllh { + uint32_t time; // GPS msToW + int32_t longitude; + int32_t latitude; + int32_t altitude_ellipsoid; + int32_t altitude_msl; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; + }; + struct ubx_nav_status { + uint32_t time; // GPS msToW + uint8_t fix_type; + uint8_t fix_status; + uint8_t differential_status; + uint8_t res; + uint32_t time_to_first_fix; + uint32_t uptime; // milliseconds + }; + struct ubx_nav_solution { + uint32_t time; + int32_t time_nsec; + int16_t week; + uint8_t fix_type; + uint8_t fix_status; + int32_t ecef_x; + int32_t ecef_y; + int32_t ecef_z; + uint32_t position_accuracy_3d; + int32_t ecef_x_velocity; + int32_t ecef_y_velocity; + int32_t ecef_z_velocity; + uint32_t speed_accuracy; + uint16_t position_DOP; + uint8_t res; + uint8_t satellites; + uint32_t res2; + }; + struct ubx_nav_velned { + uint32_t time; // GPS msToW + int32_t ned_north; + int32_t ned_east; + int32_t ned_down; + uint32_t speed_3d; + uint32_t speed_2d; + int32_t heading_2d; + uint32_t speed_accuracy; + uint32_t heading_accuracy; + }; // // #pragma pack(pop) - enum ubs_protocol_bytes { - PREAMBLE1 = 0xb5, - PREAMBLE2 = 0x62, - CLASS_NAV = 0x1, - MSG_POSLLH = 0x2, - MSG_STATUS = 0x3, - MSG_SOL = 0x6, - MSG_VELNED = 0x12 - }; - enum ubs_nav_fix_type { - FIX_NONE = 0, - FIX_DEAD_RECKONING = 1, - FIX_2D = 2, - FIX_3D = 3, - FIX_GPS_DEAD_RECKONING = 4, - FIX_TIME = 5 - }; - enum ubx_nav_status_bits { - NAV_STATUS_FIX_VALID = 1 - }; + enum ubs_protocol_bytes { + PREAMBLE1 = 0xb5, + PREAMBLE2 = 0x62, + CLASS_NAV = 0x1, + MSG_POSLLH = 0x2, + MSG_STATUS = 0x3, + MSG_SOL = 0x6, + MSG_VELNED = 0x12 + }; + enum ubs_nav_fix_type { + FIX_NONE = 0, + FIX_DEAD_RECKONING = 1, + FIX_2D = 2, + FIX_3D = 3, + FIX_GPS_DEAD_RECKONING = 4, + FIX_TIME = 5 + }; + enum ubx_nav_status_bits { + NAV_STATUS_FIX_VALID = 1 + }; - // Packet checksum accumulators - uint8_t _ck_a; - uint8_t _ck_b; + // Packet checksum accumulators + uint8_t _ck_a; + uint8_t _ck_b; - // State machine state - uint8_t _step; - uint8_t _msg_id; - bool _gather; - uint16_t _expect; - uint16_t _payload_length; - uint16_t _payload_counter; + // State machine state + uint8_t _step; + uint8_t _msg_id; + bool _gather; + uint16_t _expect; + uint16_t _payload_length; + uint16_t _payload_counter; - // Receive buffer - union { - ubx_nav_posllh posllh; - ubx_nav_status status; - ubx_nav_solution solution; - ubx_nav_velned velned; - uint8_t bytes[]; - } _buffer; + // Receive buffer + union { + ubx_nav_posllh posllh; + ubx_nav_status status; + ubx_nav_solution solution; + ubx_nav_velned velned; + uint8_t bytes[]; + } _buffer; - // Buffer parse & GPS state update - bool _parse_gps(); + // Buffer parse & GPS state update + bool _parse_gps(); }; #endif diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 57502643da..db5ef00d6d 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -6,30 +6,30 @@ void GPS::update(void) { - bool result; + bool result; - // call the GPS driver to process incoming data - result = read(); + // call the GPS driver to process incoming data + result = read(); - // if we did not get a message, and the idle timer has expired, re-init - if (!result) { - if ((millis() - _idleTimer) > idleTimeout) { - _status = NO_GPS; - - init(); - // reset the idle timer - _idleTimer = millis(); - } - } else { - // we got a message, update our status correspondingly - _status = fix ? GPS_OK : NO_FIX; + // if we did not get a message, and the idle timer has expired, re-init + if (!result) { + if ((millis() - _idleTimer) > idleTimeout) { + _status = NO_GPS; - valid_read = true; - new_data = true; + init(); + // reset the idle timer + _idleTimer = millis(); + } + } else { + // we got a message, update our status correspondingly + _status = fix ? GPS_OK : NO_FIX; - // reset the idle timer - _idleTimer = millis(); - } + valid_read = true; + new_data = true; + + // reset the idle timer + _idleTimer = millis(); + } } void @@ -42,5 +42,5 @@ GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude, void GPS::_error(const char *msg) { - Serial.println(msg); + Serial.println(msg); } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 045149f1ec..397fbfad5b 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -15,189 +15,193 @@ class GPS { public: - /// Update GPS state based on possible bytes received from the module. - /// - /// This routine must be called periodically to process incoming data. - /// - /// GPS drivers should not override this function; they should implement - /// ::read instead. - /// - void update(void); + /// Update GPS state based on possible bytes received from the module. + /// + /// This routine must be called periodically to process incoming data. + /// + /// GPS drivers should not override this function; they should implement + /// ::read instead. + /// + void update(void); - void (*callback)(unsigned long t); + void (*callback)(unsigned long t); - /// GPS status codes - /// - /// \note Non-intuitive ordering for legacy reasons - /// - enum GPS_Status { - NO_GPS = 0, ///< No GPS connected/detected - NO_FIX = 1, ///< Receiving valid GPS messages but no lock - GPS_OK = 2 ///< Receiving valid messages and locked - }; + /// GPS status codes + /// + /// \note Non-intuitive ordering for legacy reasons + /// + enum GPS_Status { + NO_GPS = 0, ///< No GPS connected/detected + NO_FIX = 1, ///< Receiving valid GPS messages but no lock + GPS_OK = 2 ///< Receiving valid messages and locked + }; - /// Query GPS status - /// - /// The 'valid message' status indicates that a recognised message was - /// received from the GPS within the last 500ms. - /// - /// @returns Current GPS status - /// - GPS_Status status(void) { return _status; } + /// Query GPS status + /// + /// The 'valid message' status indicates that a recognised message was + /// received from the GPS within the last 500ms. + /// + /// @returns Current GPS status + /// + GPS_Status status(void) { + return _status; + } - /// GPS time epoch codes - /// - enum GPS_Time_Epoch { - TIME_OF_DAY = 0, ///< - TIME_OF_WEEK = 1, ///< Ublox - TIME_OF_YEAR = 2, ///< MTK, NMEA - UNIX_EPOCH = 3 ///< If available - }; ///< SIFR? + /// GPS time epoch codes + /// + enum GPS_Time_Epoch { + TIME_OF_DAY = 0, ///< + TIME_OF_WEEK = 1, ///< Ublox + TIME_OF_YEAR = 2, ///< MTK, NMEA + UNIX_EPOCH = 3 ///< If available + }; ///< SIFR? - /// Query GPS time epoch - /// - /// @returns Current GPS time epoch code - /// - GPS_Time_Epoch epoch(void) { return _epoch; } + /// Query GPS time epoch + /// + /// @returns Current GPS time epoch code + /// + GPS_Time_Epoch epoch(void) { + return _epoch; + } - /// Startup initialisation. - /// - /// This routine performs any one-off initialisation required to set the - /// GPS up for use. - /// - /// Must be implemented by the GPS driver. - /// - virtual void init(void) = 0; + /// Startup initialisation. + /// + /// This routine performs any one-off initialisation required to set the + /// GPS up for use. + /// + /// Must be implemented by the GPS driver. + /// + virtual void init(void) = 0; - // Properties - long time; ///< GPS time (milliseconds from epoch) - long date; ///< GPS date (FORMAT TBD) - long latitude; ///< latitude in degrees * 10,000,000 - long longitude; ///< longitude in degrees * 10,000,000 - long altitude; ///< altitude in cm - long ground_speed; ///< ground speed in cm/sec - long ground_course; ///< ground course in 100ths of a degree - long speed_3d; ///< 3D speed in cm/sec (not always available) - int hdop; ///< horizontal dilution of precision in cm - uint8_t num_sats; ///< Number of visible satelites + // Properties + long time; ///< GPS time (milliseconds from epoch) + long date; ///< GPS date (FORMAT TBD) + long latitude; ///< latitude in degrees * 10,000,000 + long longitude; ///< longitude in degrees * 10,000,000 + long altitude; ///< altitude in cm + long ground_speed; ///< ground speed in cm/sec + long ground_course; ///< ground course in 100ths of a degree + long speed_3d; ///< 3D speed in cm/sec (not always available) + int hdop; ///< horizontal dilution of precision in cm + uint8_t num_sats; ///< Number of visible satelites - /// Set to true when new data arrives. A client may set this - /// to false in order to avoid processing data they have - /// already seen. - bool new_data; + /// Set to true when new data arrives. A client may set this + /// to false in order to avoid processing data they have + /// already seen. + bool new_data; - // Deprecated properties - bool fix; ///< true if we have a position fix (use ::status instead) - bool valid_read; ///< true if we have seen data from the GPS (use ::status instead) + // Deprecated properties + bool fix; ///< true if we have a position fix (use ::status instead) + bool valid_read; ///< true if we have seen data from the GPS (use ::status instead) - // Debug support - bool print_errors; ///< deprecated + // Debug support + bool print_errors; ///< deprecated - // HIL support - virtual void setHIL(long time, float latitude, float longitude, float altitude, - float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); + // HIL support + virtual void setHIL(long time, float latitude, float longitude, float altitude, + float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); - /// Time in milliseconds after which we will assume the GPS is no longer - /// sending us updates and attempt a re-init. - /// - /// 1200ms allows a small amount of slack over the worst-case 1Hz update - /// rate. - /// - unsigned long idleTimeout; + /// Time in milliseconds after which we will assume the GPS is no longer + /// sending us updates and attempt a re-init. + /// + /// 1200ms allows a small amount of slack over the worst-case 1Hz update + /// rate. + /// + unsigned long idleTimeout; protected: - Stream *_port; ///< port the GPS is attached to + Stream *_port; ///< port the GPS is attached to - /// Constructor - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @param s Stream connected to the GPS module. - /// - GPS(Stream *s) : _port(s) {}; + /// Constructor + /// + /// @note The stream is expected to be set up and configured for the + /// correct bitrate before ::init is called. + /// + /// @param s Stream connected to the GPS module. + /// + GPS(Stream *s) : _port(s) {}; - /// read from the GPS stream and update properties - /// - /// Must be implemented by the GPS driver. - /// - /// @returns true if a valid message was received from the GPS - /// - virtual bool read(void) = 0; + /// read from the GPS stream and update properties + /// + /// Must be implemented by the GPS driver. + /// + /// @returns true if a valid message was received from the GPS + /// + virtual bool read(void) = 0; - /// perform an endian swap on a long - /// - /// @param bytes pointer to a buffer containing bytes representing a - /// long in the wrong byte order - /// @returns endian-swapped value - /// - long _swapl(const void *bytes); + /// perform an endian swap on a long + /// + /// @param bytes pointer to a buffer containing bytes representing a + /// long in the wrong byte order + /// @returns endian-swapped value + /// + long _swapl(const void *bytes); - /// perform an endian swap on an int - /// - /// @param bytes pointer to a buffer containing bytes representing an - /// int in the wrong byte order - /// @returns endian-swapped value - int16_t _swapi(const void *bytes); + /// perform an endian swap on an int + /// + /// @param bytes pointer to a buffer containing bytes representing an + /// int in the wrong byte order + /// @returns endian-swapped value + int16_t _swapi(const void *bytes); - /// emit an error message - /// - /// based on the value of print_errors, emits the printf-formatted message - /// in msg,... to stderr - /// - /// @param fmt printf-like format string - /// - /// @note deprecated as-is due to the difficulty of hooking up to a working - /// printf vs. the potential benefits - /// - void _error(const char *msg); + /// emit an error message + /// + /// based on the value of print_errors, emits the printf-formatted message + /// in msg,... to stderr + /// + /// @param fmt printf-like format string + /// + /// @note deprecated as-is due to the difficulty of hooking up to a working + /// printf vs. the potential benefits + /// + void _error(const char *msg); - /// Time epoch code for the gps in use - GPS_Time_Epoch _epoch; + /// Time epoch code for the gps in use + GPS_Time_Epoch _epoch; private: - /// Last time that the GPS driver got a good packet from the GPS - /// - unsigned long _idleTimer; + /// Last time that the GPS driver got a good packet from the GPS + /// + unsigned long _idleTimer; - /// Our current status - GPS_Status _status; + /// Our current status + GPS_Status _status; }; inline long GPS::_swapl(const void *bytes) { - const uint8_t *b = (const uint8_t *)bytes; - union { - long v; - uint8_t b[4]; - } u; + const uint8_t *b = (const uint8_t *)bytes; + union { + long v; + uint8_t b[4]; + } u; - u.b[0] = b[3]; - u.b[1] = b[2]; - u.b[2] = b[1]; - u.b[3] = b[0]; + u.b[0] = b[3]; + u.b[1] = b[2]; + u.b[2] = b[1]; + u.b[3] = b[0]; - return(u.v); + return(u.v); } inline int16_t GPS::_swapi(const void *bytes) { - const uint8_t *b = (const uint8_t *)bytes; - union { - int16_t v; - uint8_t b[2]; - } u; + const uint8_t *b = (const uint8_t *)bytes; + union { + int16_t v; + uint8_t b[2]; + } u; - u.b[0] = b[1]; - u.b[1] = b[0]; + u.b[0] = b[1]; + u.b[1] = b[0]; - return(u.v); + return(u.v); } #endif diff --git a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde index 533f27a635..0d2267de53 100644 --- a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde +++ b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde @@ -19,41 +19,41 @@ AP_GPS_406 gps(&Serial1); void setup() { - tone(A6, 1000, 200); + tone(A6, 1000, 200); - Serial.begin(38400, 16, 128); - Serial1.begin(57600, 128, 16); - stderr = stdout; - gps.print_errors = true; + Serial.begin(38400, 16, 128); + Serial1.begin(57600, 128, 16); + stderr = stdout; + gps.print_errors = true; - Serial.println("GPS 406 library test"); - gps.init(); // GPS Initialization - delay(1000); + Serial.println("GPS 406 library test"); + gps.init(); // GPS Initialization + delay(1000); } void loop() { - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have readed the data - } + delay(20); + gps.update(); + if (gps.new_data) { + Serial.print("gps:"); + Serial.print(" Lat:"); + Serial.print((float)gps.latitude / T7, DEC); + Serial.print(" Lon:"); + Serial.print((float)gps.longitude / T7, DEC); + Serial.print(" Alt:"); + Serial.print((float)gps.altitude / 100.0, DEC); + Serial.print(" GSP:"); + Serial.print(gps.ground_speed / 100.0); + Serial.print(" COG:"); + Serial.print(gps.ground_course / 100, DEC); + Serial.print(" SAT:"); + Serial.print(gps.num_sats, DEC); + Serial.print(" FIX:"); + Serial.print(gps.fix, DEC); + Serial.print(" TIM:"); + Serial.print(gps.time, DEC); + Serial.println(); + gps.new_data = 0; // We have readed the data + } } diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index e638f085f0..c6de4f3bf4 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -18,31 +18,31 @@ AP_GPS_Auto GPS(&Serial1, &gps); void setup() { - Serial.begin(115200); - Serial1.begin(38400); + Serial.begin(115200); + Serial1.begin(38400); - Serial.println("GPS AUTO library test"); - gps = &GPS; - gps->init(); + Serial.println("GPS AUTO library test"); + gps = &GPS; + gps->init(); } void loop() { - gps->update(); - if (gps->new_data){ - if (gps->fix) { - Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu", - (float)gps->latitude / T7, - (float)gps->longitude / T7, - (float)gps->altitude / 100.0, - (float)gps->ground_speed / 100.0, - (int)gps->ground_course / 100, - gps->num_sats, - gps->time); - } else { - Serial.println("No fix"); - } - gps->new_data = false; - } + gps->update(); + if (gps->new_data) { + if (gps->fix) { + Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu", + (float)gps->latitude / T7, + (float)gps->longitude / T7, + (float)gps->altitude / 100.0, + (float)gps->ground_speed / 100.0, + (int)gps->ground_course / 100, + gps->num_sats, + gps->time); + } else { + Serial.println("No fix"); + } + gps->new_data = false; + } } diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde index 2af983ab9f..afac202a4e 100644 --- a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde +++ b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -19,39 +19,39 @@ AP_GPS_MTK gps(&Serial1); void setup() { - Serial.begin(38400); - Serial1.begin(38400); - stderr = stdout; - gps.print_errors = true; + Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; - Serial.println("GPS MTK library test"); - gps.init(); // GPS Initialization - delay(1000); + Serial.println("GPS MTK library test"); + gps.init(); // GPS Initialization + delay(1000); } void loop() { - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100.0, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have readed the data - } + delay(20); + gps.update(); + if (gps.new_data) { + Serial.print("gps:"); + Serial.print(" Lat:"); + Serial.print((float)gps.latitude / T7, DEC); + Serial.print(" Lon:"); + Serial.print((float)gps.longitude / T7, DEC); + Serial.print(" Alt:"); + Serial.print((float)gps.altitude / 100.0, DEC); + Serial.print(" GSP:"); + Serial.print(gps.ground_speed / 100.0); + Serial.print(" COG:"); + Serial.print(gps.ground_course / 100.0, DEC); + Serial.print(" SAT:"); + Serial.print(gps.num_sats, DEC); + Serial.print(" FIX:"); + Serial.print(gps.fix, DEC); + Serial.print(" TIM:"); + Serial.print(gps.time, DEC); + Serial.println(); + gps.new_data = 0; // We have readed the data + } } diff --git a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde index 3ecfd595f5..9e0cadf912 100644 --- a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde +++ b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde @@ -18,54 +18,55 @@ GPS *gps = &NMEA_gps; #define T7 10000000 const uint8_t sirf_to_nmea[] = { 0xa0, 0xa2, // preamble - 0x00, 0x18, // message length - 0x81, 0x02, // switch to NMEA - 0x01, 0x01, // GGA on with checksum - 0x00, 0x01, // GLL off - 0x00, 0x01, // GSA off - 0x00, 0x01, // GSV off - 0x01, 0x01, // RMC on with checksum - 0x01, 0x01, // VTG on with checksum - 0x00, 0x01, // MSS off - 0x00, 0x01, // EPE off - 0x00, 0x01, // ZPA off - 0x00, 0x00, // pad - 0x96, 0x00, // 38400 - 0x01, 0x25, // checksum TBD - 0xb0, 0xb3 }; // postamble + 0x00, 0x18, // message length + 0x81, 0x02, // switch to NMEA + 0x01, 0x01, // GGA on with checksum + 0x00, 0x01, // GLL off + 0x00, 0x01, // GSA off + 0x00, 0x01, // GSV off + 0x01, 0x01, // RMC on with checksum + 0x01, 0x01, // VTG on with checksum + 0x00, 0x01, // MSS off + 0x00, 0x01, // EPE off + 0x00, 0x01, // ZPA off + 0x00, 0x00, // pad + 0x96, 0x00, // 38400 + 0x01, 0x25, // checksum TBD + 0xb0, 0xb3 + }; // postamble void setup() { - Serial.begin(38400); - Serial1.begin(38400); + Serial.begin(38400); + Serial1.begin(38400); - // try to coerce a SiRF unit that's been traumatized by - // AP_GPS_AUTO back into NMEA mode so that we can test - // it. - for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++) - Serial1.write(sirf_to_nmea[i]); + // try to coerce a SiRF unit that's been traumatized by + // AP_GPS_AUTO back into NMEA mode so that we can test + // it. + for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++) + Serial1.write(sirf_to_nmea[i]); - Serial.println("GPS NMEA library test"); - gps->init(); + Serial.println("GPS NMEA library test"); + gps->init(); } void loop() { - gps->update(); - if (gps->new_data){ - if (gps->fix) { - Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu", - (float)gps->latitude / T7, - (float)gps->longitude / T7, - (float)gps->altitude / 100.0, - (float)gps->ground_speed / 100.0, - (int)gps->ground_course / 100, - gps->num_sats, - gps->time); - } else { - Serial.println("No fix"); - } - gps->new_data = false; - } + gps->update(); + if (gps->new_data) { + if (gps->fix) { + Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu", + (float)gps->latitude / T7, + (float)gps->longitude / T7, + (float)gps->altitude / 100.0, + (float)gps->ground_speed / 100.0, + (int)gps->ground_course / 100, + gps->num_sats, + gps->time); + } else { + Serial.println("No fix"); + } + gps->new_data = false; + } } diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde index f40aba2254..edebec4c7b 100644 --- a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -19,39 +19,39 @@ AP_GPS_UBLOX gps(&Serial1); void setup() { - Serial.begin(38400); - Serial1.begin(38400); - stderr = stdout; - gps.print_errors = true; + Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; - Serial.println("GPS UBLOX library test"); - gps.init(); // GPS Initialization - delay(1000); + Serial.println("GPS UBLOX library test"); + gps.init(); // GPS Initialization + delay(1000); } void loop() { - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100.0, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have readed the data - } + delay(20); + gps.update(); + if (gps.new_data) { + Serial.print("gps:"); + Serial.print(" Lat:"); + Serial.print((float)gps.latitude / T7, DEC); + Serial.print(" Lon:"); + Serial.print((float)gps.longitude / T7, DEC); + Serial.print(" Alt:"); + Serial.print((float)gps.altitude / 100.0, DEC); + Serial.print(" GSP:"); + Serial.print(gps.ground_speed / 100.0); + Serial.print(" COG:"); + Serial.print(gps.ground_course / 100.0, DEC); + Serial.print(" SAT:"); + Serial.print(gps.num_sats, DEC); + Serial.print(" FIX:"); + Serial.print(gps.fix, DEC); + Serial.print(" TIM:"); + Serial.print(gps.time, DEC); + Serial.println(); + gps.new_data = 0; // We have readed the data + } } From 793a3b5a490466b9c30e95f010f0d9e2a9d46330 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 29 Oct 2011 11:34:38 +0800 Subject: [PATCH 5/5] APM Planner 1.0.87 tlog kml fix misc fixs --- Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs | 12 +- Tools/ArdupilotMegaPlanner/CurrentState.cs | 6 +- .../GCSViews/Configuration.Designer.cs | 137 +- .../GCSViews/Configuration.cs | 43 +- .../GCSViews/Configuration.resx | 2088 +++++++---------- .../ArdupilotMegaPlanner/GCSViews/Firmware.cs | 10 +- .../GCSViews/FlightData.cs | 6 +- .../GCSViews/Simulation.cs | 18 +- .../ArdupilotMegaPlanner/GCSViews/Terminal.cs | 7 + Tools/ArdupilotMegaPlanner/Joystick.cs | 14 +- Tools/ArdupilotMegaPlanner/MAVLink.cs | 3 +- Tools/ArdupilotMegaPlanner/MainV2.cs | 25 +- Tools/ArdupilotMegaPlanner/MavlinkLog.cs | 3 +- .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 4 +- .../Release/ArdupilotMegaPlanner.application | 2 +- .../bin/Release/GCSViews/Configuration.resx | 2088 +++++++---------- 17 files changed, 1885 insertions(+), 2583 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs index 0911ebefb6..bc034b7dc1 100644 --- a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs @@ -75,8 +75,14 @@ namespace System.IO.Ports string dest = Port; string host = "127.0.0.1"; - ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host); - ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest); + if (Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host)) + { + return; + } + if (Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest)) + { + return; + } Port = dest; client = new TcpClient(host, int.Parse(Port)); @@ -169,7 +175,7 @@ namespace System.IO.Ports VerifyConnected(); int size = client.Available; byte[] crap = new byte[size]; - Console.WriteLine("UdpSerial DiscardInBuffer {0}",size); + Console.WriteLine("TcpSerial DiscardInBuffer {0}",size); Read(crap, 0, size); } diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 9a5b0f3166..51b56ed928 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -188,17 +188,17 @@ namespace ArdupilotMega private DateTime lastupdate = DateTime.Now; private DateTime lastwindcalc = DateTime.Now; - + public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs) { UpdateCurrentSettings(bs, false, MainV2.comPort); } - + /* public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow) { UpdateCurrentSettings(bs, false, MainV2.comPort); } - + */ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface) { if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index 3fc2de663c..9682236b3a 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -142,6 +142,7 @@ this.label52 = new System.Windows.Forms.Label(); this.TabAC2 = new System.Windows.Forms.TabPage(); this.groupBox5 = new System.Windows.Forms.GroupBox(); + this.label14 = new System.Windows.Forms.Label(); this.THR_RATE_IMAX = new System.Windows.Forms.DomainUpDown(); this.THR_RATE_I = new System.Windows.Forms.DomainUpDown(); this.label20 = new System.Windows.Forms.Label(); @@ -223,6 +224,7 @@ this.RATE_RLL_P = new System.Windows.Forms.DomainUpDown(); this.label91 = new System.Windows.Forms.Label(); this.TabPlanner = new System.Windows.Forms.TabPage(); + this.label26 = new System.Windows.Forms.Label(); this.CMB_videoresolutions = new System.Windows.Forms.ComboBox(); this.label12 = new System.Windows.Forms.Label(); this.CHK_GDIPlus = new System.Windows.Forms.CheckBox(); @@ -273,8 +275,20 @@ this.BUT_load = new ArdupilotMega.MyButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.BUT_compare = new ArdupilotMega.MyButton(); - this.label14 = new System.Windows.Forms.Label(); - this.label26 = new System.Windows.Forms.Label(); + this.groupBox17 = new System.Windows.Forms.GroupBox(); + this.ACRO_PIT_IMAX = new System.Windows.Forms.DomainUpDown(); + this.label27 = new System.Windows.Forms.Label(); + this.ACRO_PIT_I = new System.Windows.Forms.DomainUpDown(); + this.label29 = new System.Windows.Forms.Label(); + this.ACRO_PIT_P = new System.Windows.Forms.DomainUpDown(); + this.label33 = new System.Windows.Forms.Label(); + this.groupBox18 = new System.Windows.Forms.GroupBox(); + this.ACRO_RLL_IMAX = new System.Windows.Forms.DomainUpDown(); + this.label40 = new System.Windows.Forms.Label(); + this.ACRO_RLL_I = new System.Windows.Forms.DomainUpDown(); + this.label44 = new System.Windows.Forms.Label(); + this.ACRO_RLL_P = new System.Windows.Forms.DomainUpDown(); + this.label48 = new System.Windows.Forms.Label(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); this.ConfigTabs.SuspendLayout(); this.TabAPM2.SuspendLayout(); @@ -304,6 +318,8 @@ this.groupBox25.SuspendLayout(); this.TabPlanner.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.NUM_tracklength)).BeginInit(); + this.groupBox17.SuspendLayout(); + this.groupBox18.SuspendLayout(); this.SuspendLayout(); // // Params @@ -995,7 +1011,9 @@ // // TabAC2 // + this.TabAC2.Controls.Add(this.groupBox17); this.TabAC2.Controls.Add(this.groupBox5); + this.TabAC2.Controls.Add(this.groupBox18); this.TabAC2.Controls.Add(this.CHK_lockrollpitch); this.TabAC2.Controls.Add(this.groupBox4); this.TabAC2.Controls.Add(this.groupBox6); @@ -1022,6 +1040,11 @@ this.groupBox5.Name = "groupBox5"; this.groupBox5.TabStop = false; // + // label14 + // + resources.ApplyResources(this.label14, "label14"); + this.label14.Name = "label14"; + // // THR_RATE_IMAX // resources.ApplyResources(this.THR_RATE_IMAX, "THR_RATE_IMAX"); @@ -1050,8 +1073,11 @@ // CHK_lockrollpitch // resources.ApplyResources(this.CHK_lockrollpitch, "CHK_lockrollpitch"); + this.CHK_lockrollpitch.Checked = true; + this.CHK_lockrollpitch.CheckState = System.Windows.Forms.CheckState.Checked; this.CHK_lockrollpitch.Name = "CHK_lockrollpitch"; this.CHK_lockrollpitch.UseVisualStyleBackColor = true; + this.CHK_lockrollpitch.CheckedChanged += new System.EventHandler(this.CHK_lockrollpitch_CheckedChanged); // // groupBox4 // @@ -1545,6 +1571,11 @@ resources.ApplyResources(this.TabPlanner, "TabPlanner"); this.TabPlanner.Name = "TabPlanner"; // + // label26 + // + resources.ApplyResources(this.label26, "label26"); + this.label26.Name = "label26"; + // // CMB_videoresolutions // this.CMB_videoresolutions.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; @@ -1911,15 +1942,89 @@ this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); // - // label14 + // groupBox17 // - resources.ApplyResources(this.label14, "label14"); - this.label14.Name = "label14"; + this.groupBox17.Controls.Add(this.ACRO_PIT_IMAX); + this.groupBox17.Controls.Add(this.label27); + this.groupBox17.Controls.Add(this.ACRO_PIT_I); + this.groupBox17.Controls.Add(this.label29); + this.groupBox17.Controls.Add(this.ACRO_PIT_P); + this.groupBox17.Controls.Add(this.label33); + resources.ApplyResources(this.groupBox17, "groupBox17"); + this.groupBox17.Name = "groupBox17"; + this.groupBox17.TabStop = false; // - // label26 + // ACRO_PIT_IMAX // - resources.ApplyResources(this.label26, "label26"); - this.label26.Name = "label26"; + resources.ApplyResources(this.ACRO_PIT_IMAX, "ACRO_PIT_IMAX"); + this.ACRO_PIT_IMAX.Name = "ACRO_PIT_IMAX"; + // + // label27 + // + resources.ApplyResources(this.label27, "label27"); + this.label27.Name = "label27"; + // + // ACRO_PIT_I + // + resources.ApplyResources(this.ACRO_PIT_I, "ACRO_PIT_I"); + this.ACRO_PIT_I.Name = "ACRO_PIT_I"; + // + // label29 + // + resources.ApplyResources(this.label29, "label29"); + this.label29.Name = "label29"; + // + // ACRO_PIT_P + // + resources.ApplyResources(this.ACRO_PIT_P, "ACRO_PIT_P"); + this.ACRO_PIT_P.Name = "ACRO_PIT_P"; + // + // label33 + // + resources.ApplyResources(this.label33, "label33"); + this.label33.Name = "label33"; + // + // groupBox18 + // + this.groupBox18.Controls.Add(this.ACRO_RLL_IMAX); + this.groupBox18.Controls.Add(this.label40); + this.groupBox18.Controls.Add(this.ACRO_RLL_I); + this.groupBox18.Controls.Add(this.label44); + this.groupBox18.Controls.Add(this.ACRO_RLL_P); + this.groupBox18.Controls.Add(this.label48); + resources.ApplyResources(this.groupBox18, "groupBox18"); + this.groupBox18.Name = "groupBox18"; + this.groupBox18.TabStop = false; + // + // ACRO_RLL_IMAX + // + resources.ApplyResources(this.ACRO_RLL_IMAX, "ACRO_RLL_IMAX"); + this.ACRO_RLL_IMAX.Name = "ACRO_RLL_IMAX"; + // + // label40 + // + resources.ApplyResources(this.label40, "label40"); + this.label40.Name = "label40"; + // + // ACRO_RLL_I + // + resources.ApplyResources(this.ACRO_RLL_I, "ACRO_RLL_I"); + this.ACRO_RLL_I.Name = "ACRO_RLL_I"; + // + // label44 + // + resources.ApplyResources(this.label44, "label44"); + this.label44.Name = "label44"; + // + // ACRO_RLL_P + // + resources.ApplyResources(this.ACRO_RLL_P, "ACRO_RLL_P"); + this.ACRO_RLL_P.Name = "ACRO_RLL_P"; + // + // label48 + // + resources.ApplyResources(this.label48, "label48"); + this.label48.Name = "label48"; // // Configuration // @@ -1965,6 +2070,8 @@ this.groupBox25.ResumeLayout(false); this.TabPlanner.ResumeLayout(false); ((System.ComponentModel.ISupportInitialize)(this.NUM_tracklength)).EndInit(); + this.groupBox17.ResumeLayout(false); + this.groupBox18.ResumeLayout(false); this.ResumeLayout(false); } @@ -2214,5 +2321,19 @@ private System.Windows.Forms.Label label109; private System.Windows.Forms.Label label14; private System.Windows.Forms.Label label26; + private System.Windows.Forms.GroupBox groupBox17; + private System.Windows.Forms.DomainUpDown ACRO_PIT_IMAX; + private System.Windows.Forms.Label label27; + private System.Windows.Forms.DomainUpDown ACRO_PIT_I; + private System.Windows.Forms.Label label29; + private System.Windows.Forms.DomainUpDown ACRO_PIT_P; + private System.Windows.Forms.Label label33; + private System.Windows.Forms.GroupBox groupBox18; + private System.Windows.Forms.DomainUpDown ACRO_RLL_IMAX; + private System.Windows.Forms.Label label40; + private System.Windows.Forms.DomainUpDown ACRO_RLL_I; + private System.Windows.Forms.Label label44; + private System.Windows.Forms.DomainUpDown ACRO_RLL_P; + private System.Windows.Forms.Label label48; } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index 79df55f2e2..a72e79be4a 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -61,24 +61,6 @@ namespace ArdupilotMega.GCSViews { InitializeComponent(); - // fix for dup name - XTRK_ANGLE_CD1.Name = "XTRK_ANGLE_CD"; - } - - private void Configuration_Load(object sender, EventArgs e) - { - // read tooltips - if (tooltips.Count == 0) - readToolTips(); - - this.SuspendLayout(); - - // prefill all fields - param = MainV2.comPort.param; - processToScreen(); - - this.ResumeLayout(); - // enable disable relevbant hardware tabs if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane) { @@ -93,6 +75,20 @@ namespace ArdupilotMega.GCSViews TabAC2.Enabled = true; } + // read tooltips + if (tooltips.Count == 0) + readToolTips(); + + // prefill all fields + param = MainV2.comPort.param; + processToScreen(); + + // fix for dup name + XTRK_ANGLE_CD1.Name = "XTRK_ANGLE_CD"; + } + + private void Configuration_Load(object sender, EventArgs e) + { // setup up camera button states if (MainV2.cam != null) { @@ -386,7 +382,7 @@ namespace ArdupilotMega.GCSViews // enable roll and pitch pairing for ac2 if (CHK_lockrollpitch.Checked) { - if (name.StartsWith("RATE_") || name.StartsWith("STB_")) + if (name.StartsWith("RATE_") || name.StartsWith("STB_") || name.StartsWith("ACRO_")) { if (name.Contains("_RLL_")) { @@ -534,6 +530,8 @@ namespace ArdupilotMega.GCSViews continue; if (name == "WP_TOTAL") continue; + if (name == "CMD_TOTAL") + continue; if (row.Cells[0].Value.ToString() == name) { if (row.Cells[1].Value.ToString() != value.ToString()) @@ -1021,6 +1019,8 @@ namespace ArdupilotMega.GCSViews continue; if (name == "WP_TOTAL") continue; + if (name == "CMD_TOTAL") + continue; param2[name] = value; } @@ -1039,5 +1039,10 @@ namespace ArdupilotMega.GCSViews MessageBox.Show("You need to restart the planner for this to take effect"); MainV2.config["CHK_GDIPlus"] = CHK_GDIPlus.Checked.ToString(); } + + private void CHK_lockrollpitch_CheckedChanged(object sender, EventArgs e) + { + + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx index 88fc8b5e1f..c5e12aff50 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx @@ -166,16 +166,13 @@ - 4, 4 - - - 4, 4, 4, 4 + 3, 3 150 - 359, 503 + 269, 409 58 @@ -196,13 +193,10 @@ Top, Bottom, Left, Right - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -223,13 +217,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 67, 16 + 50, 13 12 @@ -250,13 +241,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -277,13 +265,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 36, 16 + 27, 13 13 @@ -304,13 +289,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -331,13 +313,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 32, 16 + 24, 13 14 @@ -358,13 +337,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -385,13 +361,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 48, 16 + 36, 13 15 @@ -412,16 +385,10 @@ 7 - 540, 267 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 405, 217 - 260, 133 + 195, 108 0 @@ -442,13 +409,10 @@ 0 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 0 @@ -469,13 +433,10 @@ NoControl - 8, 107 - - - 4, 0, 4, 0 + 6, 87 - 43, 16 + 32, 13 1 @@ -496,13 +457,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 2 @@ -523,13 +481,10 @@ NoControl - 8, 73 - - - 4, 0, 4, 0 + 6, 59 - 71, 16 + 53, 13 3 @@ -550,13 +505,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 4 @@ -577,13 +529,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 67, 16 + 50, 13 5 @@ -604,13 +553,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -631,13 +577,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 85, 16 + 64, 13 6 @@ -658,16 +601,10 @@ 7 - 541, 400 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 406, 325 - 260, 133 + 195, 108 1 @@ -688,13 +625,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -715,13 +649,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 68, 16 + 51, 13 10 @@ -742,13 +673,10 @@ 1 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -769,13 +697,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 72, 16 + 54, 13 11 @@ -796,13 +721,10 @@ 3 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -823,13 +745,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 73, 16 + 55, 13 12 @@ -850,16 +769,10 @@ 5 - 273, 400 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 325 - 260, 133 + 195, 108 2 @@ -880,13 +793,10 @@ 2 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -907,13 +817,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 81, 16 + 61, 13 8 @@ -934,13 +841,10 @@ 1 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -961,13 +865,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 69, 16 + 52, 13 9 @@ -988,16 +889,10 @@ 3 - 5, 400 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 325 - 260, 133 + 195, 108 3 @@ -1018,13 +913,10 @@ 3 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 13 @@ -1045,13 +937,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 48, 16 + 36, 13 14 @@ -1072,13 +961,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -1099,13 +985,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 81, 16 + 61, 13 15 @@ -1126,13 +1009,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -1153,13 +1033,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 81, 16 + 61, 13 16 @@ -1180,16 +1057,10 @@ 5 - 273, 267 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 217 - 260, 133 + 195, 108 4 @@ -1210,13 +1081,10 @@ 4 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -1237,13 +1105,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -1264,13 +1129,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -1291,13 +1153,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -1318,13 +1177,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -1345,13 +1201,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -1372,13 +1225,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -1399,13 +1249,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -1426,16 +1273,10 @@ 7 - 5, 267 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 217 - 260, 133 + 195, 108 5 @@ -1456,13 +1297,10 @@ 5 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 0 @@ -1483,13 +1321,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 1 @@ -1510,13 +1345,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 2 @@ -1537,13 +1369,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 3 @@ -1564,13 +1393,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 4 @@ -1591,13 +1417,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -1618,13 +1441,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 6 @@ -1645,13 +1465,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 7 @@ -1672,16 +1489,10 @@ 7 - 541, 134 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 406, 109 - 260, 133 + 195, 108 6 @@ -1702,13 +1513,10 @@ 6 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 0 @@ -1729,13 +1537,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 1 @@ -1756,13 +1561,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 2 @@ -1783,13 +1585,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 3 @@ -1810,13 +1609,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 4 @@ -1837,13 +1633,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -1864,13 +1657,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 6 @@ -1891,13 +1681,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 7 @@ -1918,16 +1705,10 @@ 7 - 273, 134 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 109 - 260, 133 + 195, 108 7 @@ -1948,13 +1729,10 @@ 7 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -1975,13 +1753,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2002,13 +1777,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2029,13 +1801,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2056,13 +1825,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2083,13 +1849,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2110,13 +1873,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2137,13 +1897,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2164,16 +1921,10 @@ 7 - 5, 134 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 109 - 260, 133 + 195, 108 8 @@ -2194,13 +1945,10 @@ 8 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -2221,13 +1969,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2248,13 +1993,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2275,13 +2017,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2302,13 +2041,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2329,13 +2065,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2356,13 +2089,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2383,13 +2113,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2410,16 +2137,10 @@ 7 - 541, 1 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 406, 1 - 260, 133 + 195, 108 9 @@ -2440,13 +2161,10 @@ 9 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -2467,13 +2185,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2494,13 +2209,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2521,13 +2233,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2548,13 +2257,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2575,13 +2281,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2602,13 +2305,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2629,13 +2329,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2656,16 +2353,10 @@ 7 - 273, 1 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 1 - 260, 133 + 195, 108 10 @@ -2686,13 +2377,10 @@ 10 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -2713,13 +2401,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2740,13 +2425,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2767,13 +2449,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2794,13 +2473,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2821,13 +2497,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2848,13 +2521,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2875,13 +2545,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2902,16 +2569,10 @@ 7 - 5, 1 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 1 - 260, 133 + 195, 108 11 @@ -2938,7 +2599,7 @@ 0, 0, 0, 0 - 965, 540 + 722, 434 0 @@ -2958,17 +2619,182 @@ 0 + + 80, 63 + + + 78, 20 + + + 0 + + + ACRO_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + ACRO_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label29 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + ACRO_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label33 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 5 + + + 182, 337 + + + 170, 91 + + + 13 + + + Acro Pitch + + + groupBox17 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 0 + NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 16 @@ -2989,13 +2815,10 @@ 0 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3013,13 +2836,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3040,13 +2860,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3067,13 +2884,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3094,13 +2908,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3121,16 +2932,10 @@ 5 - 8, 292 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 6, 221 - 227, 135 + 170, 110 16 @@ -3148,19 +2953,184 @@ TabAC2 + 1 + + + 80, 63 + + + 78, 20 + + 0 + + ACRO_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 0 + + + NoControl + + + 6, 66 + + + 68, 13 + + + 1 + + + IMAX + + + label40 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + ACRO_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label44 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + ACRO_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label48 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 5 + + + 6, 337 + + + 170, 91 + + + 14 + + + Acro Roll + + + groupBox18 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 2 + True - 4, 244 - - - 4, 4, 4, 4 + 3, 198 - 198, 21 + 154, 17 13 @@ -3178,16 +3148,13 @@ TabAC2 - 1 + 3 - 107, 106 - - - 4, 4, 4, 4 + 80, 86 - 104, 22 + 78, 20 16 @@ -3208,13 +3175,10 @@ NoControl - 8, 110 - - - 4, 0, 4, 0 + 6, 89 - 72, 16 + 54, 13 17 @@ -3235,13 +3199,10 @@ 1 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3262,13 +3223,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -3289,13 +3247,10 @@ 3 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3316,13 +3271,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3343,13 +3295,10 @@ 5 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3370,13 +3319,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3397,16 +3343,10 @@ 7 - 712, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 534, 107 - 227, 133 + 170, 108 0 @@ -3424,16 +3364,13 @@ TabAC2 - 2 + 4 - 107, 106 - - - 4, 4, 4, 4 + 80, 86 - 104, 22 + 78, 20 18 @@ -3454,13 +3391,10 @@ NoControl - 8, 110 - - - 4, 0, 4, 0 + 6, 89 - 109, 16 + 82, 13 19 @@ -3481,13 +3415,10 @@ 1 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3508,13 +3439,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -3535,13 +3463,10 @@ 3 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3562,13 +3487,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3589,13 +3511,10 @@ 5 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3616,13 +3535,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3643,16 +3559,10 @@ 7 - 477, 292 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 358, 221 - 227, 135 + 170, 110 2 @@ -3670,16 +3580,13 @@ TabAC2 - 3 + 5 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3700,13 +3607,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -3727,13 +3631,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3754,13 +3655,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3781,13 +3679,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3808,13 +3703,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3835,16 +3727,10 @@ 5 - 243, 292 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 182, 221 - 227, 135 + 170, 110 3 @@ -3862,16 +3748,13 @@ TabAC2 - 4 + 6 - 107, 75 - - - 4, 4, 4, 4 + 80, 61 - 104, 22 + 78, 20 11 @@ -3892,13 +3775,10 @@ NoControl - 8, 79 - - - 4, 0, 4, 0 + 6, 64 - 87, 16 + 65, 13 12 @@ -3919,13 +3799,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3946,13 +3823,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3973,13 +3847,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4000,13 +3871,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4027,16 +3895,10 @@ 5 - 708, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 531, 6 - 227, 117 + 170, 95 6 @@ -4054,16 +3916,13 @@ TabAC2 - 5 + 7 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -4084,13 +3943,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -4111,13 +3967,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -4138,13 +3991,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -4165,13 +4015,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4192,13 +4039,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4219,16 +4063,10 @@ 5 - 477, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 358, 6 - 227, 117 + 170, 95 7 @@ -4246,16 +4084,13 @@ TabAC2 - 6 + 8 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -4276,13 +4111,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -4303,13 +4135,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -4330,13 +4159,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -4357,13 +4183,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4384,13 +4207,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4411,16 +4231,10 @@ 5 - 243, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 182, 6 - 227, 117 + 170, 95 8 @@ -4438,16 +4252,13 @@ TabAC2 - 7 + 9 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -4468,13 +4279,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -4495,13 +4303,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -4522,13 +4327,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -4549,13 +4351,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4576,13 +4375,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4603,16 +4399,10 @@ 5 - 8, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 6, 6 - 227, 117 + 170, 95 9 @@ -4630,16 +4420,13 @@ TabAC2 - 8 + 10 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 0 @@ -4660,13 +4447,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 1 @@ -4687,13 +4471,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 4 @@ -4714,13 +4495,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -4741,13 +4519,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 6 @@ -4768,13 +4543,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 7 @@ -4795,16 +4567,10 @@ 5 - 477, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 358, 107 - 227, 112 + 170, 91 10 @@ -4822,16 +4588,13 @@ TabAC2 - 9 + 11 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 0 @@ -4852,13 +4615,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 1 @@ -4879,13 +4639,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 4 @@ -4906,13 +4663,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -4933,13 +4687,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 6 @@ -4960,13 +4711,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 7 @@ -4987,16 +4735,10 @@ 5 - 243, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 182, 107 - 227, 112 + 170, 91 11 @@ -5014,16 +4756,13 @@ TabAC2 - 10 + 12 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 0 @@ -5044,13 +4783,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 91, 16 + 68, 13 1 @@ -5071,13 +4807,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 4 @@ -5098,13 +4831,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -5125,13 +4855,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 6 @@ -5152,13 +4879,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 7 @@ -5179,16 +4903,10 @@ 5 - 8, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 6, 107 - 227, 112 + 170, 91 12 @@ -5206,19 +4924,16 @@ TabAC2 - 11 + 13 4, 22 - - 4, 4, 4, 4 - - 4, 4, 4, 4 + 3, 3, 3, 3 - 965, 540 + 722, 434 1 @@ -5242,13 +4957,10 @@ NoControl - 40, 61 - - - 4, 0, 4, 0 + 30, 50 - 133, 28 + 100, 23 41 @@ -5269,13 +4981,10 @@ 0 - 185, 58 - - - 4, 4, 4, 4 + 139, 47 - 543, 24 + 408, 21 0 @@ -5296,13 +5005,10 @@ NoControl - 40, 419 - - - 4, 0, 4, 0 + 30, 340 - 81, 16 + 61, 13 39 @@ -5326,13 +5032,10 @@ NoControl - 185, 418 - - - 4, 4, 4, 4 + 139, 340 - 236, 21 + 177, 17 40 @@ -5363,13 +5066,10 @@ GDI+ = Enabled NoControl - 40, 391 - - - 4, 0, 4, 0 + 30, 318 - 81, 16 + 61, 13 37 @@ -5393,13 +5093,10 @@ GDI+ = Enabled NoControl - 185, 390 - - - 4, 4, 4, 4 + 139, 317 - 236, 21 + 177, 17 38 @@ -5423,13 +5120,10 @@ GDI+ = Enabled NoControl - 40, 360 - - - 4, 0, 4, 0 + 30, 292 - 137, 22 + 103, 18 36 @@ -5450,13 +5144,10 @@ GDI+ = Enabled 6 - 185, 358 - - - 4, 4, 4, 4 + 139, 291 - 89, 22 + 67, 20 35 @@ -5480,13 +5171,10 @@ GDI+ = Enabled NoControl - 772, 132 - - - 4, 4, 4, 4 + 579, 107 - 136, 21 + 102, 17 34 @@ -5510,13 +5198,10 @@ GDI+ = Enabled NoControl - 40, 331 - - - 4, 0, 4, 0 + 30, 269 - 81, 16 + 61, 13 0 @@ -5540,13 +5225,10 @@ GDI+ = Enabled NoControl - 185, 329 - - - 4, 4, 4, 4 + 139, 267 - 217, 21 + 163, 17 1 @@ -5573,13 +5255,10 @@ GDI+ = Enabled NoControl - 44, 506 - - - 4, 4, 4, 4 + 33, 411 - 192, 21 + 144, 17 2 @@ -5603,13 +5282,10 @@ GDI+ = Enabled NoControl - 787, 300 - - - 4, 0, 4, 0 + 590, 244 - 29, 16 + 22, 13 3 @@ -5642,13 +5318,10 @@ GDI+ = Enabled 10 - 828, 296 - - - 4, 4, 4, 4 + 621, 240 - 105, 24 + 80, 21 4 @@ -5669,13 +5342,10 @@ GDI+ = Enabled NoControl - 567, 300 - - - 4, 0, 4, 0 + 425, 244 - 92, 16 + 69, 13 5 @@ -5699,13 +5369,10 @@ GDI+ = Enabled NoControl - 373, 300 - - - 4, 0, 4, 0 + 280, 244 - 59, 16 + 44, 13 6 @@ -5729,13 +5396,10 @@ GDI+ = Enabled NoControl - 181, 300 - - - 4, 0, 4, 0 + 136, 244 - 57, 16 + 43, 13 7 @@ -5759,13 +5423,10 @@ GDI+ = Enabled NoControl - 36, 300 - - - 4, 0, 4, 0 + 27, 244 - 112, 16 + 84, 13 8 @@ -5798,13 +5459,10 @@ GDI+ = Enabled 10 - 665, 296 - - - 4, 4, 4, 4 + 499, 240 - 105, 24 + 80, 21 9 @@ -5834,13 +5492,10 @@ GDI+ = Enabled 10 - 445, 296 - - - 4, 4, 4, 4 + 334, 240 - 105, 24 + 80, 21 10 @@ -5870,13 +5525,10 @@ GDI+ = Enabled 10 - 251, 296 - - - 4, 4, 4, 4 + 188, 240 - 105, 24 + 80, 21 11 @@ -5897,13 +5549,10 @@ GDI+ = Enabled NoControl - 377, 257 - - - 4, 0, 4, 0 + 283, 209 - 536, 16 + 402, 13 12 @@ -5928,13 +5577,10 @@ GDI+ = Enabled NoControl - 40, 267 - - - 4, 0, 4, 0 + 30, 217 - 87, 16 + 65, 13 13 @@ -5958,13 +5604,10 @@ GDI+ = Enabled NoControl - 40, 233 - - - 4, 0, 4, 0 + 30, 189 - 69, 16 + 52, 13 14 @@ -5985,13 +5628,10 @@ GDI+ = Enabled 23 - 185, 263 - - - 4, 4, 4, 4 + 139, 214 - 183, 24 + 138, 21 15 @@ -6009,13 +5649,10 @@ GDI+ = Enabled 24 - 185, 230 - - - 4, 4, 4, 4 + 139, 187 - 183, 24 + 138, 21 16 @@ -6036,13 +5673,10 @@ GDI+ = Enabled NoControl - 40, 200 - - - 4, 0, 4, 0 + 30, 162 - 60, 16 + 45, 13 17 @@ -6066,13 +5700,10 @@ GDI+ = Enabled NoControl - 40, 137 - - - 4, 0, 4, 0 + 30, 111 - 59, 16 + 44, 13 18 @@ -6096,13 +5727,10 @@ GDI+ = Enabled NoControl - 628, 132 - - - 4, 4, 4, 4 + 471, 107 - 136, 21 + 102, 17 19 @@ -6126,13 +5754,10 @@ GDI+ = Enabled NoControl - 504, 132 - - - 4, 4, 4, 4 + 378, 107 - 116, 21 + 87, 17 20 @@ -6156,13 +5781,10 @@ GDI+ = Enabled NoControl - 429, 132 - - - 4, 4, 4, 4 + 322, 107 - 75, 21 + 56, 17 21 @@ -6186,13 +5808,10 @@ GDI+ = Enabled NoControl - 327, 132 - - - 4, 4, 4, 4 + 245, 107 - 95, 21 + 71, 17 22 @@ -6216,13 +5835,10 @@ GDI+ = Enabled NoControl - 40, 102 - - - 4, 0, 4, 0 + 30, 83 - 76, 16 + 57, 13 23 @@ -6243,13 +5859,10 @@ GDI+ = Enabled 32 - 185, 99 - - - 4, 4, 4, 4 + 139, 80 - 183, 23 + 138, 21 24 @@ -6267,13 +5880,10 @@ GDI+ = Enabled 33 - 185, 161 - - - 4, 4, 4, 4 + 139, 131 - 183, 24 + 138, 21 25 @@ -6294,13 +5904,10 @@ GDI+ = Enabled NoControl - 40, 166 - - - 4, 0, 4, 0 + 30, 135 - 92, 16 + 69, 13 26 @@ -6324,13 +5931,10 @@ GDI+ = Enabled NoControl - 185, 132 - - - 4, 4, 4, 4 + 139, 107 - 132, 21 + 99, 17 27 @@ -6354,13 +5958,10 @@ GDI+ = Enabled NoControl - 736, 18 - - - 4, 4, 4, 4 + 552, 15 - 167, 21 + 125, 17 28 @@ -6384,13 +5985,10 @@ GDI+ = Enabled NoControl - 40, 20 - - - 4, 0, 4, 0 + 30, 16 - 133, 28 + 100, 23 29 @@ -6411,13 +6009,10 @@ GDI+ = Enabled 38 - 185, 16 - - - 4, 4, 4, 4 + 139, 13 - 325, 24 + 245, 21 30 @@ -6438,13 +6033,10 @@ GDI+ = Enabled NoControl - 185, 194 - - - 4, 4, 4, 4 + 139, 158 - 132, 28 + 99, 23 31 @@ -6456,7 +6048,7 @@ GDI+ = Enabled BUT_Joystick - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c TabPlanner @@ -6468,13 +6060,10 @@ GDI+ = Enabled NoControl - 628, 14 - - - 4, 4, 4, 4 + 471, 11 - 100, 28 + 75, 23 32 @@ -6486,7 +6075,7 @@ GDI+ = Enabled BUT_videostop - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c TabPlanner @@ -6498,13 +6087,10 @@ GDI+ = Enabled NoControl - 520, 14 - - - 4, 4, 4, 4 + 390, 11 - 100, 28 + 75, 23 33 @@ -6516,7 +6102,7 @@ GDI+ = Enabled BUT_videostart - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c TabPlanner @@ -6527,14 +6113,11 @@ GDI+ = Enabled 4, 22 - - 4, 4, 4, 4 - - 4, 4, 4, 4 + 3, 3, 3, 3 - 965, 540 + 722, 434 2 @@ -6557,11 +6140,8 @@ GDI+ = Enabled 4, 22 - - 4, 4, 4, 4 - - 965, 540 + 722, 434 3 @@ -6585,7 +6165,7 @@ GDI+ = Enabled 52, 18 - 371, 0 + 278, 0 0, 0, 0, 0 @@ -6594,7 +6174,7 @@ GDI+ = Enabled 0, 0 - 973, 566 + 730, 460 62 @@ -6633,13 +6213,10 @@ GDI+ = Enabled NoControl - 225, 543 - - - 4, 4, 4, 4 + 169, 441 - 137, 23 + 103, 19 0 @@ -6654,7 +6231,7 @@ GDI+ = Enabled BUT_rerequestparams - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6669,13 +6246,10 @@ GDI+ = Enabled NoControl - 225, 512 - - - 4, 4, 4, 4 + 169, 416 - 137, 23 + 103, 19 63 @@ -6690,7 +6264,7 @@ GDI+ = Enabled BUT_writePIDS - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6705,13 +6279,13 @@ GDI+ = Enabled NoControl - 109, 512 + 82, 416 0, 0, 0, 0 - 100, 23 + 75, 19 64 @@ -6726,7 +6300,7 @@ GDI+ = Enabled BUT_save - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6741,13 +6315,13 @@ GDI+ = Enabled NoControl - 4, 512 + 3, 416 0, 0, 0, 0 - 100, 23 + 75, 19 65 @@ -6762,7 +6336,7 @@ GDI+ = Enabled BUT_load - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6777,13 +6351,10 @@ GDI+ = Enabled NoControl - 41, 539 - - - 4, 4, 4, 4 + 31, 438 - 137, 23 + 103, 19 66 @@ -6795,7 +6366,7 @@ GDI+ = Enabled BUT_compare - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6807,13 +6378,10 @@ GDI+ = Enabled True - 8, 16 - - - 4, 4, 4, 4 + 6, 13 - 1344, 567 + 1008, 461 Command @@ -6855,7 +6423,7 @@ GDI+ = Enabled Configuration - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c ..\Resources\MAVParam.txt;System.String, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089;Windows-1252 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index 067b30f823..a5a8129599 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -488,7 +488,13 @@ namespace ArdupilotMega.GCSViews return; } - int apmformat_version = ArduinoDetect.decodeApVar(MainV2.comportname, board); + int apmformat_version = -1; // fail continue + + try + { + apmformat_version = ArduinoDetect.decodeApVar(MainV2.comportname, board); + } + catch { } if (apmformat_version != -1 && apmformat_version != temp.k_format_version) { @@ -678,7 +684,7 @@ namespace ArdupilotMega.GCSViews Application.DoEvents(); - System.Threading.Thread.Sleep(5000); // 5 seconds - new apvar erases eeprom on new format version, this should buy us some time. + System.Threading.Thread.Sleep(10000); // 10 seconds - new apvar erases eeprom on new format version, this should buy us some time. } catch (Exception ex) { lbl_status.Text = "Failed upload"; MessageBox.Show("Check port settings or Port in use? " + ex.ToString()); port.Close(); } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index af2806bda9..eb28731546 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -364,7 +364,7 @@ namespace ArdupilotMega.GCSViews - if (CB_tuning.Checked == false) // draw if in view +// if (CB_tuning.Checked == false) // draw if in view { if (MainV2.comPort.logreadmode && MainV2.comPort.logplaybackfile != null) @@ -992,9 +992,9 @@ namespace ArdupilotMega.GCSViews CMB_setwp.Items.Add("0 (Home)"); - if (MainV2.comPort.param["WP_TOTAL"] != null) + if (MainV2.comPort.param["CMD_TOTAL"] != null) { - int wps = int.Parse(MainV2.comPort.param["WP_TOTAL"].ToString()); + int wps = int.Parse(MainV2.comPort.param["CMD_TOTAL"].ToString()); for (int z = 1; z <= wps; z++) { CMB_setwp.Items.Add(z.ToString()); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index 8a84ccf784..1824c1ecb6 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -1573,16 +1573,14 @@ namespace ArdupilotMega.GCSViews xScale.Max = time + xScale.MajorStep; xScale.Min = xScale.Max - 30.0; } - - // Make sure the Y axis is rescaled to accommodate actual data - try - { - zg1.AxisChange(); - } - catch { } - // Force a redraw - zg1.Invalidate(); - + // Make sure the Y axis is rescaled to accommodate actual data + try + { + zg1.AxisChange(); + } + catch { } + // Force a redraw + zg1.Invalidate(); } private void SaveSettings_Click(object sender, EventArgs e) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs index 7051ee6006..928804ede2 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs @@ -172,6 +172,13 @@ namespace ArdupilotMega.GCSViews comPort.Open(); + comPort.WriteLine(""); + comPort.WriteLine(""); + comPort.WriteLine(""); + comPort.WriteLine(""); + comPort.WriteLine(""); + comPort.WriteLine(""); + System.Threading.Thread t11 = new System.Threading.Thread(delegate() { threadrun = true; diff --git a/Tools/ArdupilotMegaPlanner/Joystick.cs b/Tools/ArdupilotMegaPlanner/Joystick.cs index 6f933dad57..a41f31fa85 100644 --- a/Tools/ArdupilotMegaPlanner/Joystick.cs +++ b/Tools/ArdupilotMegaPlanner/Joystick.cs @@ -505,9 +505,17 @@ namespace ArdupilotMega if (MainV2.comPort.param.Count > 0) { - min = (int)(float)(MainV2.comPort.param["RC" + chan + "_MIN"]); - max = (int)(float)(MainV2.comPort.param["RC" + chan + "_MAX"]); - trim = (int)(float)(MainV2.comPort.param["RC" + chan + "_TRIM"]); + try + { + min = (int)(float)(MainV2.comPort.param["RC" + chan + "_MIN"]); + max = (int)(float)(MainV2.comPort.param["RC" + chan + "_MAX"]); + trim = (int)(float)(MainV2.comPort.param["RC" + chan + "_TRIM"]); + } + catch { + min = 1000; + max = 2000; + trim = 1500; + } } else { diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 6399558128..47d8a989fd 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -1275,6 +1275,7 @@ namespace ArdupilotMega if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0) { param["WP_TOTAL"] = (float)wp_total - 1; + param["CMD_TOTAL"] = (float)wp_total - 1; MainV2.givecomport = false; return; } @@ -1647,7 +1648,7 @@ namespace ArdupilotMega if (bpstime.Second != DateTime.Now.Second && !logreadmode) { - Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead); + Console.WriteLine("bps {0} loss {1} left {2} mem {3}", bps1, synclost, BaseStream.BytesToRead,System.GC.GetTotalMemory(false)); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index bb8034b729..0d9cf00b0f 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -468,6 +468,7 @@ namespace ArdupilotMega private void MenuConfiguration_Click(object sender, EventArgs e) { + MyView.SuspendLayout(); MyView.Controls.Clear(); GCSViews.Terminal.threadrun = false; @@ -486,13 +487,11 @@ namespace ArdupilotMega UserControl temp = Configuration; - temp.SuspendLayout(); - fixtheme(temp); - temp.Anchor = AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right | AnchorStyles.Top; + //temp.Anchor = AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right | AnchorStyles.Top; - temp.Location = new Point(0, MainMenu.Height); + temp.Location = new Point(0, 0); temp.Dock = DockStyle.Fill; @@ -500,9 +499,11 @@ namespace ArdupilotMega temp.BackColor = Color.FromArgb(0x26, 0x27, 0x28); + temp.ResumeLayout(); + MyView.Controls.Add(temp); - temp.ResumeLayout(); + MyView.ResumeLayout(); } private void MenuSimulation_Click(object sender, EventArgs e) @@ -562,7 +563,19 @@ namespace ArdupilotMega this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect; - UserControl temp = new GCSViews.Terminal(); + // dispose of old else memory leak + if (Terminal != null) + { + try + { + Terminal.Dispose(); + } + catch { } + } + + Terminal = new GCSViews.Terminal(); + + UserControl temp = Terminal; fixtheme(temp); diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index 7be69fa57d..fe3c75f856 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -332,7 +332,7 @@ namespace ArdupilotMega cs.datetime = mine.lastlogread; - cs.UpdateCurrentSettings(null, true); + cs.UpdateCurrentSettings(null, true, mine); try { @@ -360,6 +360,7 @@ namespace ArdupilotMega writeKML(logfile + ".kml"); + progressBar1.Value = 100; } } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index f5bdf0e072..421048b251 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.86")] +[assembly: AssemblyFileVersion("1.0.87")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index e6ca06eea1..e6080503e7 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -135,7 +135,7 @@ namespace ArdupilotMega.Setup System.Threading.Thread.Sleep(5); - MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true); + MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true, MainV2.comPort); if (MainV2.cs.ch1in > 800 && MainV2.cs.ch1in < 2200) { @@ -192,7 +192,7 @@ namespace ArdupilotMega.Setup MessageBox.Show("Ensure all your sticks are centered, and click ok to continue"); - MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true); + MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true, MainV2.comPort); rctrim[0] = MainV2.cs.ch1in; rctrim[1] = MainV2.cs.ch2in; diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index 83424ad35a..a4d78a16dc 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - FgyYFBDKA+EmX+ZazsEdbI/ME7o= + S+dMQOC9TeJyQiYvhw37LpJxZU0= diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx index 88fc8b5e1f..c5e12aff50 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx @@ -166,16 +166,13 @@ - 4, 4 - - - 4, 4, 4, 4 + 3, 3 150 - 359, 503 + 269, 409 58 @@ -196,13 +193,10 @@ Top, Bottom, Left, Right - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -223,13 +217,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 67, 16 + 50, 13 12 @@ -250,13 +241,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -277,13 +265,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 36, 16 + 27, 13 13 @@ -304,13 +289,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -331,13 +313,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 32, 16 + 24, 13 14 @@ -358,13 +337,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -385,13 +361,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 48, 16 + 36, 13 15 @@ -412,16 +385,10 @@ 7 - 540, 267 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 405, 217 - 260, 133 + 195, 108 0 @@ -442,13 +409,10 @@ 0 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 0 @@ -469,13 +433,10 @@ NoControl - 8, 107 - - - 4, 0, 4, 0 + 6, 87 - 43, 16 + 32, 13 1 @@ -496,13 +457,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 2 @@ -523,13 +481,10 @@ NoControl - 8, 73 - - - 4, 0, 4, 0 + 6, 59 - 71, 16 + 53, 13 3 @@ -550,13 +505,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 4 @@ -577,13 +529,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 67, 16 + 50, 13 5 @@ -604,13 +553,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -631,13 +577,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 85, 16 + 64, 13 6 @@ -658,16 +601,10 @@ 7 - 541, 400 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 406, 325 - 260, 133 + 195, 108 1 @@ -688,13 +625,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -715,13 +649,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 68, 16 + 51, 13 10 @@ -742,13 +673,10 @@ 1 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -769,13 +697,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 72, 16 + 54, 13 11 @@ -796,13 +721,10 @@ 3 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -823,13 +745,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 73, 16 + 55, 13 12 @@ -850,16 +769,10 @@ 5 - 273, 400 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 325 - 260, 133 + 195, 108 2 @@ -880,13 +793,10 @@ 2 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -907,13 +817,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 81, 16 + 61, 13 8 @@ -934,13 +841,10 @@ 1 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -961,13 +865,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 69, 16 + 52, 13 9 @@ -988,16 +889,10 @@ 3 - 5, 400 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 325 - 260, 133 + 195, 108 3 @@ -1018,13 +913,10 @@ 3 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 13 @@ -1045,13 +937,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 48, 16 + 36, 13 14 @@ -1072,13 +961,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -1099,13 +985,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 81, 16 + 61, 13 15 @@ -1126,13 +1009,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -1153,13 +1033,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 81, 16 + 61, 13 16 @@ -1180,16 +1057,10 @@ 5 - 273, 267 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 217 - 260, 133 + 195, 108 4 @@ -1210,13 +1081,10 @@ 4 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -1237,13 +1105,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -1264,13 +1129,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -1291,13 +1153,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -1318,13 +1177,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -1345,13 +1201,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -1372,13 +1225,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -1399,13 +1249,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -1426,16 +1273,10 @@ 7 - 5, 267 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 217 - 260, 133 + 195, 108 5 @@ -1456,13 +1297,10 @@ 5 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 0 @@ -1483,13 +1321,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 1 @@ -1510,13 +1345,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 2 @@ -1537,13 +1369,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 3 @@ -1564,13 +1393,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 4 @@ -1591,13 +1417,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -1618,13 +1441,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 6 @@ -1645,13 +1465,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 7 @@ -1672,16 +1489,10 @@ 7 - 541, 134 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 406, 109 - 260, 133 + 195, 108 6 @@ -1702,13 +1513,10 @@ 6 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 0 @@ -1729,13 +1537,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 1 @@ -1756,13 +1561,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 2 @@ -1783,13 +1585,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 3 @@ -1810,13 +1609,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 4 @@ -1837,13 +1633,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -1864,13 +1657,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 6 @@ -1891,13 +1681,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 7 @@ -1918,16 +1705,10 @@ 7 - 273, 134 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 109 - 260, 133 + 195, 108 7 @@ -1948,13 +1729,10 @@ 7 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -1975,13 +1753,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2002,13 +1777,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2029,13 +1801,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2056,13 +1825,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2083,13 +1849,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2110,13 +1873,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2137,13 +1897,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2164,16 +1921,10 @@ 7 - 5, 134 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 109 - 260, 133 + 195, 108 8 @@ -2194,13 +1945,10 @@ 8 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -2221,13 +1969,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2248,13 +1993,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2275,13 +2017,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2302,13 +2041,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2329,13 +2065,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2356,13 +2089,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2383,13 +2113,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2410,16 +2137,10 @@ 7 - 541, 1 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 406, 1 - 260, 133 + 195, 108 9 @@ -2440,13 +2161,10 @@ 9 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -2467,13 +2185,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2494,13 +2209,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2521,13 +2233,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2548,13 +2257,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2575,13 +2281,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2602,13 +2305,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2629,13 +2329,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2656,16 +2353,10 @@ 7 - 273, 1 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 205, 1 - 260, 133 + 195, 108 10 @@ -2686,13 +2377,10 @@ 10 - 148, 101 - - - 4, 4, 4, 4 + 111, 82 - 104, 22 + 78, 20 11 @@ -2713,13 +2401,10 @@ NoControl - 8, 106 - - - 4, 0, 4, 0 + 6, 86 - 72, 16 + 54, 13 12 @@ -2740,13 +2425,10 @@ 1 - 148, 73 - - - 4, 4, 4, 4 + 111, 59 - 104, 22 + 78, 20 9 @@ -2767,13 +2449,10 @@ NoControl - 8, 78 - - - 4, 0, 4, 0 + 6, 63 - 20, 16 + 15, 13 13 @@ -2794,13 +2473,10 @@ 3 - 148, 44 - - - 4, 4, 4, 4 + 111, 36 - 104, 22 + 78, 20 7 @@ -2821,13 +2497,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -2848,13 +2521,10 @@ 5 - 148, 16 - - - 4, 4, 4, 4 + 111, 13 - 104, 22 + 78, 20 5 @@ -2875,13 +2545,10 @@ NoControl - 8, 21 - - - 4, 0, 4, 0 + 6, 17 - 19, 16 + 14, 13 15 @@ -2902,16 +2569,10 @@ 7 - 5, 1 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 4, 1 - 260, 133 + 195, 108 11 @@ -2938,7 +2599,7 @@ 0, 0, 0, 0 - 965, 540 + 722, 434 0 @@ -2958,17 +2619,182 @@ 0 + + 80, 63 + + + 78, 20 + + + 0 + + + ACRO_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + ACRO_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label29 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + ACRO_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label33 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 5 + + + 182, 337 + + + 170, 91 + + + 13 + + + Acro Pitch + + + groupBox17 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 0 + NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 16 @@ -2989,13 +2815,10 @@ 0 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3013,13 +2836,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3040,13 +2860,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3067,13 +2884,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3094,13 +2908,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3121,16 +2932,10 @@ 5 - 8, 292 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 6, 221 - 227, 135 + 170, 110 16 @@ -3148,19 +2953,184 @@ TabAC2 + 1 + + + 80, 63 + + + 78, 20 + + 0 + + ACRO_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 0 + + + NoControl + + + 6, 66 + + + 68, 13 + + + 1 + + + IMAX + + + label40 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + ACRO_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label44 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + ACRO_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label48 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 5 + + + 6, 337 + + + 170, 91 + + + 14 + + + Acro Roll + + + groupBox18 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 2 + True - 4, 244 - - - 4, 4, 4, 4 + 3, 198 - 198, 21 + 154, 17 13 @@ -3178,16 +3148,13 @@ TabAC2 - 1 + 3 - 107, 106 - - - 4, 4, 4, 4 + 80, 86 - 104, 22 + 78, 20 16 @@ -3208,13 +3175,10 @@ NoControl - 8, 110 - - - 4, 0, 4, 0 + 6, 89 - 72, 16 + 54, 13 17 @@ -3235,13 +3199,10 @@ 1 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3262,13 +3223,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -3289,13 +3247,10 @@ 3 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3316,13 +3271,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3343,13 +3295,10 @@ 5 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3370,13 +3319,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3397,16 +3343,10 @@ 7 - 712, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 534, 107 - 227, 133 + 170, 108 0 @@ -3424,16 +3364,13 @@ TabAC2 - 2 + 4 - 107, 106 - - - 4, 4, 4, 4 + 80, 86 - 104, 22 + 78, 20 18 @@ -3454,13 +3391,10 @@ NoControl - 8, 110 - - - 4, 0, 4, 0 + 6, 89 - 109, 16 + 82, 13 19 @@ -3481,13 +3415,10 @@ 1 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3508,13 +3439,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -3535,13 +3463,10 @@ 3 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3562,13 +3487,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3589,13 +3511,10 @@ 5 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3616,13 +3535,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3643,16 +3559,10 @@ 7 - 477, 292 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 358, 221 - 227, 135 + 170, 110 2 @@ -3670,16 +3580,13 @@ TabAC2 - 3 + 5 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -3700,13 +3607,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -3727,13 +3631,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3754,13 +3655,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3781,13 +3679,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -3808,13 +3703,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -3835,16 +3727,10 @@ 5 - 243, 292 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 182, 221 - 227, 135 + 170, 110 3 @@ -3862,16 +3748,13 @@ TabAC2 - 4 + 6 - 107, 75 - - - 4, 4, 4, 4 + 80, 61 - 104, 22 + 78, 20 11 @@ -3892,13 +3775,10 @@ NoControl - 8, 79 - - - 4, 0, 4, 0 + 6, 64 - 87, 16 + 65, 13 12 @@ -3919,13 +3799,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -3946,13 +3823,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -3973,13 +3847,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4000,13 +3871,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4027,16 +3895,10 @@ 5 - 708, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 531, 6 - 227, 117 + 170, 95 6 @@ -4054,16 +3916,13 @@ TabAC2 - 5 + 7 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -4084,13 +3943,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -4111,13 +3967,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -4138,13 +3991,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -4165,13 +4015,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4192,13 +4039,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4219,16 +4063,10 @@ 5 - 477, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 358, 6 - 227, 117 + 170, 95 7 @@ -4246,16 +4084,13 @@ TabAC2 - 6 + 8 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -4276,13 +4111,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -4303,13 +4135,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -4330,13 +4159,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -4357,13 +4183,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4384,13 +4207,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4411,16 +4231,10 @@ 5 - 243, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 182, 6 - 227, 117 + 170, 95 8 @@ -4438,16 +4252,13 @@ TabAC2 - 7 + 9 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 11 @@ -4468,13 +4279,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 12 @@ -4495,13 +4303,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 7 @@ -4522,13 +4327,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 14 @@ -4549,13 +4351,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 5 @@ -4576,13 +4375,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 15 @@ -4603,16 +4399,10 @@ 5 - 8, 7 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 6, 6 - 227, 117 + 170, 95 9 @@ -4630,16 +4420,13 @@ TabAC2 - 8 + 10 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 0 @@ -4660,13 +4447,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 1 @@ -4687,13 +4471,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 4 @@ -4714,13 +4495,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -4741,13 +4519,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 6 @@ -4768,13 +4543,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 7 @@ -4795,16 +4567,10 @@ 5 - 477, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 358, 107 - 227, 112 + 170, 91 10 @@ -4822,16 +4588,13 @@ TabAC2 - 9 + 11 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 0 @@ -4852,13 +4615,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 87, 16 + 65, 13 1 @@ -4879,13 +4639,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 4 @@ -4906,13 +4663,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -4933,13 +4687,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 6 @@ -4960,13 +4711,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 7 @@ -4987,16 +4735,10 @@ 5 - 243, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 182, 107 - 227, 112 + 170, 91 11 @@ -5014,16 +4756,13 @@ TabAC2 - 10 + 12 - 107, 78 - - - 4, 4, 4, 4 + 80, 63 - 104, 22 + 78, 20 0 @@ -5044,13 +4783,10 @@ NoControl - 8, 81 - - - 4, 0, 4, 0 + 6, 66 - 91, 16 + 68, 13 1 @@ -5071,13 +4807,10 @@ 1 - 107, 46 - - - 4, 4, 4, 4 + 80, 37 - 104, 22 + 78, 20 4 @@ -5098,13 +4831,10 @@ NoControl - 8, 49 - - - 4, 0, 4, 0 + 6, 40 - 13, 16 + 10, 13 5 @@ -5125,13 +4855,10 @@ 3 - 107, 16 - - - 4, 4, 4, 4 + 80, 13 - 104, 22 + 78, 20 6 @@ -5152,13 +4879,10 @@ NoControl - 8, 20 - - - 4, 0, 4, 0 + 6, 16 - 19, 16 + 14, 13 7 @@ -5179,16 +4903,10 @@ 5 - 8, 132 - - - 4, 4, 4, 4 - - - 4, 4, 4, 4 + 6, 107 - 227, 112 + 170, 91 12 @@ -5206,19 +4924,16 @@ TabAC2 - 11 + 13 4, 22 - - 4, 4, 4, 4 - - 4, 4, 4, 4 + 3, 3, 3, 3 - 965, 540 + 722, 434 1 @@ -5242,13 +4957,10 @@ NoControl - 40, 61 - - - 4, 0, 4, 0 + 30, 50 - 133, 28 + 100, 23 41 @@ -5269,13 +4981,10 @@ 0 - 185, 58 - - - 4, 4, 4, 4 + 139, 47 - 543, 24 + 408, 21 0 @@ -5296,13 +5005,10 @@ NoControl - 40, 419 - - - 4, 0, 4, 0 + 30, 340 - 81, 16 + 61, 13 39 @@ -5326,13 +5032,10 @@ NoControl - 185, 418 - - - 4, 4, 4, 4 + 139, 340 - 236, 21 + 177, 17 40 @@ -5363,13 +5066,10 @@ GDI+ = Enabled NoControl - 40, 391 - - - 4, 0, 4, 0 + 30, 318 - 81, 16 + 61, 13 37 @@ -5393,13 +5093,10 @@ GDI+ = Enabled NoControl - 185, 390 - - - 4, 4, 4, 4 + 139, 317 - 236, 21 + 177, 17 38 @@ -5423,13 +5120,10 @@ GDI+ = Enabled NoControl - 40, 360 - - - 4, 0, 4, 0 + 30, 292 - 137, 22 + 103, 18 36 @@ -5450,13 +5144,10 @@ GDI+ = Enabled 6 - 185, 358 - - - 4, 4, 4, 4 + 139, 291 - 89, 22 + 67, 20 35 @@ -5480,13 +5171,10 @@ GDI+ = Enabled NoControl - 772, 132 - - - 4, 4, 4, 4 + 579, 107 - 136, 21 + 102, 17 34 @@ -5510,13 +5198,10 @@ GDI+ = Enabled NoControl - 40, 331 - - - 4, 0, 4, 0 + 30, 269 - 81, 16 + 61, 13 0 @@ -5540,13 +5225,10 @@ GDI+ = Enabled NoControl - 185, 329 - - - 4, 4, 4, 4 + 139, 267 - 217, 21 + 163, 17 1 @@ -5573,13 +5255,10 @@ GDI+ = Enabled NoControl - 44, 506 - - - 4, 4, 4, 4 + 33, 411 - 192, 21 + 144, 17 2 @@ -5603,13 +5282,10 @@ GDI+ = Enabled NoControl - 787, 300 - - - 4, 0, 4, 0 + 590, 244 - 29, 16 + 22, 13 3 @@ -5642,13 +5318,10 @@ GDI+ = Enabled 10 - 828, 296 - - - 4, 4, 4, 4 + 621, 240 - 105, 24 + 80, 21 4 @@ -5669,13 +5342,10 @@ GDI+ = Enabled NoControl - 567, 300 - - - 4, 0, 4, 0 + 425, 244 - 92, 16 + 69, 13 5 @@ -5699,13 +5369,10 @@ GDI+ = Enabled NoControl - 373, 300 - - - 4, 0, 4, 0 + 280, 244 - 59, 16 + 44, 13 6 @@ -5729,13 +5396,10 @@ GDI+ = Enabled NoControl - 181, 300 - - - 4, 0, 4, 0 + 136, 244 - 57, 16 + 43, 13 7 @@ -5759,13 +5423,10 @@ GDI+ = Enabled NoControl - 36, 300 - - - 4, 0, 4, 0 + 27, 244 - 112, 16 + 84, 13 8 @@ -5798,13 +5459,10 @@ GDI+ = Enabled 10 - 665, 296 - - - 4, 4, 4, 4 + 499, 240 - 105, 24 + 80, 21 9 @@ -5834,13 +5492,10 @@ GDI+ = Enabled 10 - 445, 296 - - - 4, 4, 4, 4 + 334, 240 - 105, 24 + 80, 21 10 @@ -5870,13 +5525,10 @@ GDI+ = Enabled 10 - 251, 296 - - - 4, 4, 4, 4 + 188, 240 - 105, 24 + 80, 21 11 @@ -5897,13 +5549,10 @@ GDI+ = Enabled NoControl - 377, 257 - - - 4, 0, 4, 0 + 283, 209 - 536, 16 + 402, 13 12 @@ -5928,13 +5577,10 @@ GDI+ = Enabled NoControl - 40, 267 - - - 4, 0, 4, 0 + 30, 217 - 87, 16 + 65, 13 13 @@ -5958,13 +5604,10 @@ GDI+ = Enabled NoControl - 40, 233 - - - 4, 0, 4, 0 + 30, 189 - 69, 16 + 52, 13 14 @@ -5985,13 +5628,10 @@ GDI+ = Enabled 23 - 185, 263 - - - 4, 4, 4, 4 + 139, 214 - 183, 24 + 138, 21 15 @@ -6009,13 +5649,10 @@ GDI+ = Enabled 24 - 185, 230 - - - 4, 4, 4, 4 + 139, 187 - 183, 24 + 138, 21 16 @@ -6036,13 +5673,10 @@ GDI+ = Enabled NoControl - 40, 200 - - - 4, 0, 4, 0 + 30, 162 - 60, 16 + 45, 13 17 @@ -6066,13 +5700,10 @@ GDI+ = Enabled NoControl - 40, 137 - - - 4, 0, 4, 0 + 30, 111 - 59, 16 + 44, 13 18 @@ -6096,13 +5727,10 @@ GDI+ = Enabled NoControl - 628, 132 - - - 4, 4, 4, 4 + 471, 107 - 136, 21 + 102, 17 19 @@ -6126,13 +5754,10 @@ GDI+ = Enabled NoControl - 504, 132 - - - 4, 4, 4, 4 + 378, 107 - 116, 21 + 87, 17 20 @@ -6156,13 +5781,10 @@ GDI+ = Enabled NoControl - 429, 132 - - - 4, 4, 4, 4 + 322, 107 - 75, 21 + 56, 17 21 @@ -6186,13 +5808,10 @@ GDI+ = Enabled NoControl - 327, 132 - - - 4, 4, 4, 4 + 245, 107 - 95, 21 + 71, 17 22 @@ -6216,13 +5835,10 @@ GDI+ = Enabled NoControl - 40, 102 - - - 4, 0, 4, 0 + 30, 83 - 76, 16 + 57, 13 23 @@ -6243,13 +5859,10 @@ GDI+ = Enabled 32 - 185, 99 - - - 4, 4, 4, 4 + 139, 80 - 183, 23 + 138, 21 24 @@ -6267,13 +5880,10 @@ GDI+ = Enabled 33 - 185, 161 - - - 4, 4, 4, 4 + 139, 131 - 183, 24 + 138, 21 25 @@ -6294,13 +5904,10 @@ GDI+ = Enabled NoControl - 40, 166 - - - 4, 0, 4, 0 + 30, 135 - 92, 16 + 69, 13 26 @@ -6324,13 +5931,10 @@ GDI+ = Enabled NoControl - 185, 132 - - - 4, 4, 4, 4 + 139, 107 - 132, 21 + 99, 17 27 @@ -6354,13 +5958,10 @@ GDI+ = Enabled NoControl - 736, 18 - - - 4, 4, 4, 4 + 552, 15 - 167, 21 + 125, 17 28 @@ -6384,13 +5985,10 @@ GDI+ = Enabled NoControl - 40, 20 - - - 4, 0, 4, 0 + 30, 16 - 133, 28 + 100, 23 29 @@ -6411,13 +6009,10 @@ GDI+ = Enabled 38 - 185, 16 - - - 4, 4, 4, 4 + 139, 13 - 325, 24 + 245, 21 30 @@ -6438,13 +6033,10 @@ GDI+ = Enabled NoControl - 185, 194 - - - 4, 4, 4, 4 + 139, 158 - 132, 28 + 99, 23 31 @@ -6456,7 +6048,7 @@ GDI+ = Enabled BUT_Joystick - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c TabPlanner @@ -6468,13 +6060,10 @@ GDI+ = Enabled NoControl - 628, 14 - - - 4, 4, 4, 4 + 471, 11 - 100, 28 + 75, 23 32 @@ -6486,7 +6075,7 @@ GDI+ = Enabled BUT_videostop - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c TabPlanner @@ -6498,13 +6087,10 @@ GDI+ = Enabled NoControl - 520, 14 - - - 4, 4, 4, 4 + 390, 11 - 100, 28 + 75, 23 33 @@ -6516,7 +6102,7 @@ GDI+ = Enabled BUT_videostart - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c TabPlanner @@ -6527,14 +6113,11 @@ GDI+ = Enabled 4, 22 - - 4, 4, 4, 4 - - 4, 4, 4, 4 + 3, 3, 3, 3 - 965, 540 + 722, 434 2 @@ -6557,11 +6140,8 @@ GDI+ = Enabled 4, 22 - - 4, 4, 4, 4 - - 965, 540 + 722, 434 3 @@ -6585,7 +6165,7 @@ GDI+ = Enabled 52, 18 - 371, 0 + 278, 0 0, 0, 0, 0 @@ -6594,7 +6174,7 @@ GDI+ = Enabled 0, 0 - 973, 566 + 730, 460 62 @@ -6633,13 +6213,10 @@ GDI+ = Enabled NoControl - 225, 543 - - - 4, 4, 4, 4 + 169, 441 - 137, 23 + 103, 19 0 @@ -6654,7 +6231,7 @@ GDI+ = Enabled BUT_rerequestparams - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6669,13 +6246,10 @@ GDI+ = Enabled NoControl - 225, 512 - - - 4, 4, 4, 4 + 169, 416 - 137, 23 + 103, 19 63 @@ -6690,7 +6264,7 @@ GDI+ = Enabled BUT_writePIDS - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6705,13 +6279,13 @@ GDI+ = Enabled NoControl - 109, 512 + 82, 416 0, 0, 0, 0 - 100, 23 + 75, 19 64 @@ -6726,7 +6300,7 @@ GDI+ = Enabled BUT_save - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6741,13 +6315,13 @@ GDI+ = Enabled NoControl - 4, 512 + 3, 416 0, 0, 0, 0 - 100, 23 + 75, 19 65 @@ -6762,7 +6336,7 @@ GDI+ = Enabled BUT_load - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6777,13 +6351,10 @@ GDI+ = Enabled NoControl - 41, 539 - - - 4, 4, 4, 4 + 31, 438 - 137, 23 + 103, 19 66 @@ -6795,7 +6366,7 @@ GDI+ = Enabled BUT_compare - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c $this @@ -6807,13 +6378,10 @@ GDI+ = Enabled True - 8, 16 - - - 4, 4, 4, 4 + 6, 13 - 1344, 567 + 1008, 461 Command @@ -6855,7 +6423,7 @@ GDI+ = Enabled Configuration - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c ..\Resources\MAVParam.txt;System.String, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089;Windows-1252