mavlink_gateway:MAVLink Gateway 是一个 arduino 应用程序，用于将 MAVLink 协议消...

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // // This is free software; you can redistribute it and/or modify it under // the terms of the GNU Lesser General Public License as published by the // Free Software Foundation; either version 2.1 of the License, or (at // your option) any later version. // // total up and check overflow // check size of group var_info /// @file AP_Param.cpp /// @brief The AP variable store. #include <FastSerial.h> #include <AP_Common.h> #include <AP_Math.h> #include <math.h> #include <string.h> // #define ENABLE_FASTSERIAL_DEBUG #ifdef ENABLE_FASTSERIAL_DEBUG # define serialDebug(fmt, args...) if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) #else # define serialDebug(fmt, args...) #endif // some useful progmem macros #define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr) #define PGM_UINT16(addr) pgm_read_word((const uint16_t *)addr) #define PGM_POINTER(addr) pgm_read_pointer((const void *)addr) // the 'GROUP_ID' of a element of a group is the 8 bit identifier used // to distinguish between this element of the group and other elements // of the same group. It is calculated using a bit shift per level of // nesting, so the first level of nesting gets 4 bits, and the next // level gets the next 4 bits. This limits groups to having at most 16 // elements. #define GROUP_ID(grpinfo, base, i, shift) ((base)+(((uint16_t)PGM_UINT8(&grpinfo[i].idx))<<(shift))) // Note about AP_Vector3f handling. // The code has special cases for AP_Vector3f to allow it to be viewed // as both a single 3 element vector and as a set of 3 AP_Float // variables. This is done to make it possible for MAVLink to see // vectors as parameters, which allows users to save their compass // offsets in MAVLink parameter files. The code involves quite a few // special cases which could be generalised to any vector/matrix type // if we end up needing this behaviour for other than AP_Vector3f // static member variables for AP_Param. // // max EEPROM write size. This is usually less than the physical // size as only part of the EEPROM is reserved for parameters uint16_t AP_Param::_eeprom_size; // number of rows in the _var_info[] table uint8_t AP_Param::_num_vars; // storage and naming information about all types that can be saved const AP_Param::Info *AP_Param::_var_info; // write to EEPROM, checking each byte to avoid writing // bytes that are already correct void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size) { const uint8_t *b = (const uint8_t *)ptr; while (size--) { uint8_t v = eeprom_read_byte((const uint8_t *)ofs); if (v != *b) { eeprom_write_byte((uint8_t *)ofs, *b); } b++; ofs++; } } // write a sentinal value at the given offset void AP_Param::write_sentinal(uint16_t ofs) { struct Param_header phdr; phdr.type = _sentinal_type; phdr.key = _sentinal_key; phdr.group_element = _sentinal_group; eeprom_write_check(&phdr, ofs, sizeof(phdr)); } // erase all EEPROM variables by re-writing the header and adding // a sentinal void AP_Param::erase_all(void) { struct EEPROM_header hdr; serialDebug("erase_all"); // write the header hdr.magic[0] = k_EEPROM_magic0; hdr.magic[1] = k_EEPROM_magic1; hdr.revision = k_EEPROM_revision; hdr.spare = 0; eeprom_write_check(&hdr, 0, sizeof(hdr)); // add a sentinal directly after the header write_sentinal(sizeof(struct EEPROM_header)); } // validate a group info table bool AP_Param::check_group_info(const struct AP_Param::GroupInfo *group_info, uint16_t *total_size, uint8_t group_shift) { uint8_t type; int8_t max_idx = -1; for (uint8_t i=0; (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; i++) { #ifdef AP_NESTED_GROUPS_ENABLED if (type == AP_PARAM_GROUP) { // a nested group const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); if (group_shift + _group_level_shift >= _group_bits) { // double nesting of groups is not allowed return false; } if (ginfo == NULL || !check_group_info(ginfo, total_size, group_shift + _group_level_shift)) { return false; } continue; } #endif // AP_NESTED_GROUPS_ENABLED uint8_t idx = PGM_UINT8(&group_info[i].idx); if (idx >= (1<<_group_level_shift)) { // passed limit on table size return false; } if ((int8_t)idx <= max_idx) { // the indexes must be in increasing order return false; } max_idx = (int8_t)idx; uint8_t size = type_size((enum ap_var_type)type); if (size == 0) { // not a valid type return false; } (*total_size) += size + sizeof(struct Param_header); } return true; } // check for duplicate key values bool AP_Param::duplicate_key(uint8_t vindex, uint8_t key) { for (uint8_t i=vindex+1; i<_num_vars; i++) { uint8_t key2 = PGM_UINT8(&_var_info[i].key); if (key2 == key) { // no duplicate keys allowed return true; } } return false; } // validate the _var_info[] table bool AP_Param::check_var_info(void) { uint16_t total_size = sizeof(struct EEPROM_header); for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type); uint8_t key = PGM_UINT8(&_var_info[i].key); if (type == AP_PARAM_GROUP) { if (i == 0) { // first element can't be a group, for first() call return false; } const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); if (group_info == NULL || !check_group_info(group_info, &total_size, 0)) { return false; } } else { uint8_t size = type_size((enum ap_var_type)type); if (size == 0) { // not a valid type - the top level list can't contain // AP_PARAM_NONE return false; } total_size += size + sizeof(struct Param_header); } if (duplicate_key(i, key)) { return false; } } if (total_size > _eeprom_size) { serialDebug("total_size %u exceeds _eeprom_size %u", total_size, _eeprom_size); return false; } return true; } // setup the _var_info[] table bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eeprom_size) { struct EEPROM_header hdr; _eeprom_size = eeprom_size; _var_info = info; _num_vars = num_vars; if (!check_var_info()) { return false; } serialDebug("setup %u vars", (unsigned)num_vars); // check the header eeprom_read_block(&hdr, 0, sizeof(hdr)); if (hdr.magic[0] != k_EEPROM_magic0 || hdr.magic[1] != k_EEPROM_magic1 || hdr.revision != k_EEPROM_revision) { // header doesn't match. We can't recover any variables. Wipe // the header and setup the sentinal directly after the header serialDebug("bad header in setup - erasing"); erase_all(); } return true; } // check if AP_Param has been initialised bool AP_Param::initialised(void) { return _var_info != NULL; } // find the info structure given a header and a group_info table // return the Info structure and a pointer to the variables storage const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,