From 3c8e78e9c069b712ef3d1dfac4edf96a6795f4ff Mon Sep 17 00:00:00 2001 From: BLash Date: Fri, 5 Jul 2024 12:49:52 -0500 Subject: [PATCH] AP_ExternalAHRS: VectorNav: Make formatting consistent. No functional changes --- .../AP_ExternalAHRS_VectorNav.cpp | 455 +++++++++--------- .../AP_ExternalAHRS_VectorNav.h | 70 ++- 2 files changed, 255 insertions(+), 270 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 3abec240670cff..faf25f060e186b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -36,7 +36,7 @@ #include #include -extern const AP_HAL::HAL &hal; +extern const AP_HAL::HAL& hal; /* header for 50Hz INS data @@ -66,8 +66,8 @@ extern const AP_HAL::HAL &hal; VelU */ -static const uint8_t vn_ins_pkt1_header[] { 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 }; -#define VN_INS_PKT1_LENGTH 170 // includes header and CRC +static const uint8_t vn_ins_pkt1_header[]{0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06}; +#define VN_INS_PKT1_LENGTH 170 // includes header and CRC struct PACKED VN_INS_packet1 { float uncompMag[3]; @@ -87,7 +87,8 @@ struct PACKED VN_INS_packet1 { }; // check packet size for 4 groups -static_assert(sizeof(VN_INS_packet1)+2+3*2+2 == VN_INS_PKT1_LENGTH, "incorrect VN_INS_packet1 length"); +static_assert(sizeof(VN_INS_packet1) + 2 + 3 * 2 + 2 == VN_INS_PKT1_LENGTH, + "incorrect VN_INS_packet1 length"); /* header for 5Hz GNSS data @@ -113,8 +114,8 @@ static_assert(sizeof(VN_INS_packet1)+2+3*2+2 == VN_INS_PKT1_LENGTH, "incorrect V NumSats Fix */ -static const uint8_t vn_ins_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x00 }; -#define VN_INS_PKT2_LENGTH 92 // includes header and CRC +static const uint8_t vn_ins_pkt2_header[]{0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x00}; +#define VN_INS_PKT2_LENGTH 92 // includes header and CRC struct PACKED VN_INS_packet2 { uint64_t timeGPS; @@ -129,10 +130,11 @@ struct PACKED VN_INS_packet2 { }; // check packet size for 4 groups -static_assert(sizeof(VN_INS_packet2)+2+4*2+2 == VN_INS_PKT2_LENGTH, "incorrect VN_INS_packet2 length"); +static_assert(sizeof(VN_INS_packet2) + 2 + 4 * 2 + 2 == VN_INS_PKT2_LENGTH, + "incorrect VN_INS_packet2 length"); /* -header for 50Hz IMU data, used in TYPE::VN_AHRS only +header for 50Hz IMU data, used in TYPE::VN_AHRS only assumes the following VN-100 config: $VNWRG,75,3,80,14,073E,0004*66 @@ -152,8 +154,8 @@ header for 50Hz IMU data, used in TYPE::VN_AHRS only 0x0004: Quaternion */ -static const uint8_t vn_ahrs_pkt1_header[] { 0x14, 0x3E, 0x07, 0x04, 0x00 }; -#define VN_AHRS_PKT1_LENGTH 104 // includes header and CRC +static const uint8_t vn_ahrs_pkt1_header[]{0x14, 0x3E, 0x07, 0x04, 0x00}; +#define VN_AHRS_PKT1_LENGTH 104 // includes header and CRC struct PACKED VN_AHRS_packet1 { float uncompMag[3]; @@ -167,32 +169,34 @@ struct PACKED VN_AHRS_packet1 { float quaternion[4]; }; -static_assert(sizeof(VN_AHRS_packet1)+2+2*2+2 == VN_AHRS_PKT1_LENGTH, "incorrect VN_AHRS_packet1 length"); +static_assert(sizeof(VN_AHRS_packet1) + 2 + 2 * 2 + 2 == VN_AHRS_PKT1_LENGTH, + "incorrect VN_AHRS_packet1 length"); // constructor -AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend, - AP_ExternalAHRS::state_t &_state) : - AP_ExternalAHRS_backend(_frontend, _state) -{ - auto &sm = AP::serialmanager(); - uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); - if (!uart) { +AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS* _frontend, + AP_ExternalAHRS::state_t& _state) + : AP_ExternalAHRS_backend(_frontend, _state) { + auto& sm = AP::serialmanager(); + _uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); + if (!_uart) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VectorNav ExternalAHRS no UART"); return; } - baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); - port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + _baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + _port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); - bufsize = MAX(MAX(VN_INS_PKT1_LENGTH, VN_INS_PKT2_LENGTH), VN_AHRS_PKT1_LENGTH); - pktbuf = NEW_NOTHROW uint8_t[bufsize]; - last_ins_pkt1 = NEW_NOTHROW VN_INS_packet1; - last_ins_pkt2 = NEW_NOTHROW VN_INS_packet2; + _bufsize = MAX(MAX(VN_INS_PKT1_LENGTH, VN_INS_PKT2_LENGTH), VN_AHRS_PKT1_LENGTH); + _pktbuf = NEW_NOTHROW uint8_t[_bufsize]; + _last_ins_pkt1 = NEW_NOTHROW VN_INS_packet1; + _last_ins_pkt2 = NEW_NOTHROW VN_INS_packet2; - if (!pktbuf || !last_ins_pkt1 || !last_ins_pkt2) { + if (!_pktbuf || !_last_ins_pkt1 || !_last_ins_pkt2) { AP_BoardConfig::allocation_error("VectorNav ExternalAHRS"); } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + if (!hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, + AP_HAL::Scheduler::PRIORITY_SPI, 0)) { AP_HAL::panic("VectorNav Failed to start ExternalAHRS update thread"); } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VectorNav ExternalAHRS initialised"); @@ -203,71 +207,73 @@ AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend, returns true if the function should be called again straight away */ #define SYNC_BYTE 0xFA -bool AP_ExternalAHRS_VectorNav::check_uart() -{ - if (!setup_complete) { +bool AP_ExternalAHRS_VectorNav::check_uart() { + if (!_setup_complete) { return false; } WITH_SEMAPHORE(state.sem); // ensure we own the uart - uart->begin(0); - uint32_t n = uart->available(); + _uart->begin(0); + uint32_t n = _uart->available(); if (n == 0) { return false; } - if (pktoffset < bufsize) { - ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset))); + if (_pktoffset < _bufsize) { + ssize_t nread = _uart->read(&_pktbuf[_pktoffset], MIN(n, unsigned(_bufsize - _pktoffset))); if (nread <= 0) { return false; } - pktoffset += nread; + _pktoffset += nread; } bool match_header1 = false; bool match_header2 = false; bool match_header3 = false; - if (pktbuf[0] != SYNC_BYTE) { + if (_pktbuf[0] != SYNC_BYTE) { goto reset; } if (type == TYPE::VN_INS) { - match_header1 = (0 == memcmp(&pktbuf[1], vn_ins_pkt1_header, MIN(sizeof(vn_ins_pkt1_header), unsigned(pktoffset-1)))); - match_header2 = (0 == memcmp(&pktbuf[1], vn_ins_pkt2_header, MIN(sizeof(vn_ins_pkt2_header), unsigned(pktoffset-1)))); + match_header1 = (0 == memcmp(&_pktbuf[1], vn_ins_pkt1_header, + MIN(sizeof(vn_ins_pkt1_header), unsigned(_pktoffset - 1)))); + match_header2 = (0 == memcmp(&_pktbuf[1], vn_ins_pkt2_header, + MIN(sizeof(vn_ins_pkt2_header), unsigned(_pktoffset - 1)))); } else { - match_header3 = (0 == memcmp(&pktbuf[1], vn_ahrs_pkt1_header, MIN(sizeof(vn_ahrs_pkt1_header), unsigned(pktoffset-1)))); + match_header3 = (0 == memcmp(&_pktbuf[1], vn_ahrs_pkt1_header, + MIN(sizeof(vn_ahrs_pkt1_header), unsigned(_pktoffset - 1)))); } if (!match_header1 && !match_header2 && !match_header3) { goto reset; } - if (match_header1 && pktoffset >= VN_INS_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT1_LENGTH-1, 0); + if (match_header1 && _pktoffset >= VN_INS_PKT1_LENGTH) { + uint16_t crc = crc16_ccitt(&_pktbuf[1], VN_INS_PKT1_LENGTH - 1, 0); if (crc == 0) { // got pkt1 - process_ins_packet1(&pktbuf[sizeof(vn_ins_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_INS_PKT1_LENGTH], pktoffset-VN_INS_PKT1_LENGTH); - pktoffset -= VN_INS_PKT1_LENGTH; + process_ins_packet1(&_pktbuf[sizeof(vn_ins_pkt1_header) + 1]); + memmove(&_pktbuf[0], &_pktbuf[VN_INS_PKT1_LENGTH], _pktoffset - VN_INS_PKT1_LENGTH); + _pktoffset -= VN_INS_PKT1_LENGTH; } else { goto reset; } - } else if (match_header2 && pktoffset >= VN_INS_PKT2_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT2_LENGTH-1, 0); + } else if (match_header2 && _pktoffset >= VN_INS_PKT2_LENGTH) { + uint16_t crc = crc16_ccitt(&_pktbuf[1], VN_INS_PKT2_LENGTH - 1, 0); if (crc == 0) { // got pkt2 - process_ins_packet2(&pktbuf[sizeof(vn_ins_pkt2_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_INS_PKT2_LENGTH], pktoffset-VN_INS_PKT2_LENGTH); - pktoffset -= VN_INS_PKT2_LENGTH; + process_ins_packet2(&_pktbuf[sizeof(vn_ins_pkt2_header) + 1]); + memmove(&_pktbuf[0], &_pktbuf[VN_INS_PKT2_LENGTH], _pktoffset - VN_INS_PKT2_LENGTH); + _pktoffset -= VN_INS_PKT2_LENGTH; } else { goto reset; } - } else if (match_header3 && pktoffset >= VN_AHRS_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_PKT1_LENGTH-1, 0); + } else if (match_header3 && _pktoffset >= VN_AHRS_PKT1_LENGTH) { + uint16_t crc = crc16_ccitt(&_pktbuf[1], VN_AHRS_PKT1_LENGTH - 1, 0); if (crc == 0) { // got AHRS pkt - process_ahrs_packet(&pktbuf[sizeof(vn_ahrs_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_AHRS_PKT1_LENGTH], pktoffset-VN_AHRS_PKT1_LENGTH); - pktoffset -= VN_AHRS_PKT1_LENGTH; + process_ahrs_packet(&_pktbuf[sizeof(vn_ahrs_pkt1_header) + 1]); + memmove(&_pktbuf[0], &_pktbuf[VN_AHRS_PKT1_LENGTH], _pktoffset - VN_AHRS_PKT1_LENGTH); + _pktoffset -= VN_AHRS_PKT1_LENGTH; } else { goto reset; } @@ -275,13 +281,13 @@ bool AP_ExternalAHRS_VectorNav::check_uart() return true; reset: - uint8_t *p = (uint8_t *)memchr(&pktbuf[1], SYNC_BYTE, pktoffset-1); + uint8_t* p = (uint8_t*)memchr(&_pktbuf[1], SYNC_BYTE, _pktoffset - 1); if (p) { - uint8_t newlen = pktoffset - (p - pktbuf); - memmove(&pktbuf[0], p, newlen); - pktoffset = newlen; + uint8_t newlen = _pktoffset - (p - _pktbuf); + memmove(&_pktbuf[0], p, newlen); + _pktoffset = newlen; } else { - pktoffset = 0; + _pktoffset = 0; } return true; } @@ -289,21 +295,20 @@ bool AP_ExternalAHRS_VectorNav::check_uart() // Send command and wait for response // Only run from thread! This blocks until a response is received #define READ_REQUEST_RETRY_MS 500 -void AP_ExternalAHRS_VectorNav::send_command_blocking() -{ +void AP_ExternalAHRS_VectorNav::send_command_blocking() { uint32_t request_sent = 0; while (true) { hal.scheduler->delay(1); const uint32_t now = AP_HAL::millis(); if (now - request_sent > READ_REQUEST_RETRY_MS) { - nmea_printf(uart, "$%s", message_to_send); + nmea_printf(_uart, "$%s", _message_to_send); request_sent = now; } - int16_t nbytes = uart->available(); + int16_t nbytes = _uart->available(); while (nbytes-- > 0) { - char c = uart->read(); + char c = _uart->read(); if (decode(c)) { return; } @@ -313,55 +318,53 @@ void AP_ExternalAHRS_VectorNav::send_command_blocking() // add a single character to the buffer and attempt to decode // returns true if a complete sentence was successfully decoded -bool AP_ExternalAHRS_VectorNav::decode(char c) -{ +bool AP_ExternalAHRS_VectorNav::decode(char c) { switch (c) { - case ',': - // end of a term, add to checksum - nmea.checksum ^= c; - FALLTHROUGH; - case '\r': - case '\n': - case '*': - { - if (nmea.sentence_done) { - return false; - } - if (nmea.term_is_checksum) { - nmea.sentence_done = true; - uint8_t checksum = 16 * char_to_hex(nmea.term[0]) + char_to_hex(nmea.term[1]); - return ((checksum == nmea.checksum) && nmea.sentence_valid); - } + case ',': + // end of a term, add to checksum + _nmea.checksum ^= c; + FALLTHROUGH; + case '\r': + case '\n': + case '*': { + if (_nmea.sentence_done) { + return false; + } + if (_nmea.term_is_checksum) { + _nmea.sentence_done = true; + uint8_t checksum = 16 * char_to_hex(_nmea.term[0]) + char_to_hex(_nmea.term[1]); + return ((checksum == _nmea.checksum) && _nmea.sentence_valid); + } - // null terminate and decode latest term - nmea.term[nmea.term_offset] = 0; - if (nmea.sentence_valid) { - nmea.sentence_valid = decode_latest_term(); - } + // null terminate and decode latest term + _nmea.term[_nmea.term_offset] = 0; + if (_nmea.sentence_valid) { + _nmea.sentence_valid = decode_latest_term(); + } - // move onto next term - nmea.term_number++; - nmea.term_offset = 0; - nmea.term_is_checksum = (c == '*'); - return false; - } + // move onto next term + _nmea.term_number++; + _nmea.term_offset = 0; + _nmea.term_is_checksum = (c == '*'); + return false; + } - case '$': // sentence begin - nmea.sentence_valid = true; - nmea.term_number = 0; - nmea.term_offset = 0; - nmea.checksum = 0; - nmea.term_is_checksum = false; - nmea.sentence_done = false; - return false; + case '$': // sentence begin + _nmea.sentence_valid = true; + _nmea.term_number = 0; + _nmea.term_offset = 0; + _nmea.checksum = 0; + _nmea.term_is_checksum = false; + _nmea.sentence_done = false; + return false; } // ordinary characters are added to term - if (nmea.term_offset < sizeof(nmea.term) - 1) { - nmea.term[nmea.term_offset++] = c; + if (_nmea.term_offset < sizeof(_nmea.term) - 1) { + _nmea.term[_nmea.term_offset++] = c; } - if (!nmea.term_is_checksum) { - nmea.checksum ^= c; + if (!_nmea.term_is_checksum) { + _nmea.checksum ^= c; } return false; @@ -369,29 +372,28 @@ bool AP_ExternalAHRS_VectorNav::decode(char c) // decode the most recently consumed term // returns true if new term is valid -bool AP_ExternalAHRS_VectorNav::decode_latest_term() -{ +bool AP_ExternalAHRS_VectorNav::decode_latest_term() { // Check the first two terms (In most cases header + reg number) that they match the sent // message. If not, the response is invalid. - switch (nmea.term_number) { + switch (_nmea.term_number) { case 0: - if (strncmp(nmea.term, message_to_send, nmea.term_offset) != + if (strncmp(_nmea.term, _message_to_send, _nmea.term_offset) != 0) { // ignore sync byte in comparison return false; - } else if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) { + } else if (strncmp(_nmea.term, "VNERR", _nmea.term_offset) == 0) { AP_HAL::panic("VectorNav received VN error response"); } break; - case 1: - if (strncmp(nmea.term, message_to_send + 6, - nmea.term_offset) != 0) { // Start after "VNXXX," + case 1: + if (strncmp(_nmea.term, _message_to_send + 6, + _nmea.term_offset) != 0) { // Start after "VNXXX," return false; } break; - case 2: - if (strncmp(nmea.term, "VN-", 3) == 0) { + case 2: + if (strncmp(_nmea.term, "VN-", 3) == 0) { // This term is the model number - strncpy(model_name, nmea.term, sizeof(model_name)); + strncpy(_model_name, _nmea.term, sizeof(_model_name)); break; } break; @@ -403,54 +405,55 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term() void AP_ExternalAHRS_VectorNav::initialize() { // Open port in the thread - uart->begin(baudrate, 1024, 512); + _uart->begin(_baudrate, 1024, 512); // Pause asynchronous communications to simplify packet finding - hal.util->snprintf(message_to_send, sizeof(message_to_send), "$VNASY,0"); + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "$VNASY,0"); send_command_blocking(); // Stop ASCII async outputs for both UARTs. If only active UART is disabled, we get a baudrate // overflow on the other UART when configuring binary outputs (reg 75 and 76) to both UARTs - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,1"); + hal.util->snprintf(_message_to_send, sizeof(message_to_send), "VNWRG,06,1"); send_command_blocking(); - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNWRG,06,2"); + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "VNWRG,06,2"); send_command_blocking(); // Read Model Number Register, ID 1 - hal.util->snprintf(message_to_send, sizeof(message_to_send), "VNRRG,01"); + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "VNRRG,01"); send_command_blocking(); // Setup for messages respective model types (on both UARTs) - if (strncmp(model_name, "VN-1", 4) == 0) { + if (strncmp(_model_name, "VN-1", 4) == 0) { // VN-1X0 type = TYPE::VN_AHRS; // This assumes unit is still configured at its default rate of 800hz - hal.util->snprintf(message_to_send, sizeof(message_to_send), + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate())); send_command_blocking(); } else { // Default to setup for sensors other than VN-100 or VN-110 - // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others + // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz + // for others uint16_t imu_rate = 800; // Default for everything but VN-300 - if (strncmp(model_name, "VN-300", 6) == 0) { + if (strncmp(_model_name, "VN-300", 6) == 0) { imu_rate = 400; } - if (strncmp(model_name, "VN-3", 4) == 0) { - has_dual_gnss = true; + if (strncmp(_model_name, "VN-3", 4) == 0) { + _has_dual_gnss = true; } - hal.util->snprintf(message_to_send, sizeof(message_to_send), + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate())); send_command_blocking(); - hal.util->snprintf(message_to_send, sizeof(message_to_send), + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5)); send_command_blocking(); } // Resume asynchronous communications - hal.util->snprintf(message_to_send, sizeof(message_to_send), "$VNASY,1\0"); + hal.util->snprintf(_message_to_send, sizeof(_message_to_send), "$VNASY,1"); send_command_blocking(); - setup_complete = true; + _setup_complete = true; } void AP_ExternalAHRS_VectorNav::update_thread() { @@ -462,10 +465,9 @@ void AP_ExternalAHRS_VectorNav::update_thread() { } } -const char* AP_ExternalAHRS_VectorNav::get_name() const -{ - if (setup_complete) { - return model_name; +const char* AP_ExternalAHRS_VectorNav::get_name() const { + if (_setup_complete) { + return _model_name; } return nullptr; } @@ -473,13 +475,12 @@ const char* AP_ExternalAHRS_VectorNav::get_name() const /* process INS mode INS packet */ -void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) -{ - const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)b; - const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2; +void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t* b) { + const struct VN_INS_packet1& pkt1 = *(struct VN_INS_packet1*)b; + const struct VN_INS_packet2& pkt2 = *_last_ins_pkt2; - last_pkt1_ms = AP_HAL::millis(); - *last_ins_pkt1 = pkt1; + _last_pkt1_ms = AP_HAL::millis(); + *_last_ins_pkt1 = pkt1; const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU); @@ -487,31 +488,32 @@ void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) WITH_SEMAPHORE(state.sem); if (use_uncomp) { state.accel = Vector3f{pkt1.uncompAccel[0], pkt1.uncompAccel[1], pkt1.uncompAccel[2]}; - state.gyro = Vector3f{pkt1.uncompAngRate[0], pkt1.uncompAngRate[1], pkt1.uncompAngRate[2]}; + state.gyro = + Vector3f{pkt1.uncompAngRate[0], pkt1.uncompAngRate[1], pkt1.uncompAngRate[2]}; } else { state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; - state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; + state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; } - state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]}; + state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], + pkt1.quaternion[2]}; state.have_quaternion = true; - state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]}; + state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]}; state.have_velocity = true; - state.location = Location{int32_t(pkt1.positionLLA[0] * 1.0e7), - int32_t(pkt1.positionLLA[1] * 1.0e7), - int32_t(pkt1.positionLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; + state.location = + Location{int32_t(pkt1.positionLLA[0] * 1.0e7), int32_t(pkt1.positionLLA[1] * 1.0e7), + int32_t(pkt1.positionLLA[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; state.last_location_update_us = AP_HAL::micros(); - state.have_location = true; + state.have_location = true; } #if AP_BARO_EXTERNALAHRS_ENABLED { AP_ExternalAHRS::baro_data_message_t baro; - baro.instance = 0; - baro.pressure_pa = pkt1.pressure*1e3; + baro.instance = 0; + baro.pressure_pa = pkt1.pressure * 1e3; baro.temperature = pkt2.temp; AP::baro().handle_external(baro); @@ -522,7 +524,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) { AP_ExternalAHRS::mag_data_message_t mag; mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]}; - mag.field *= 1000; // to mGauss + mag.field *= 1000; // to mGauss AP::compass().handle_external(mag); } @@ -531,8 +533,8 @@ void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) { AP_ExternalAHRS::ins_data_message_t ins; - ins.accel = state.accel; - ins.gyro = state.gyro; + ins.accel = state.accel; + ins.gyro = state.gyro; ins.temperature = pkt2.temp; AP::ins().handle_external(ins); @@ -542,43 +544,41 @@ void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) /* process INS mode GNSS packet */ -void AP_ExternalAHRS_VectorNav::process_ins_packet2(const uint8_t *b) -{ - const struct VN_INS_packet2 &pkt2 = *(struct VN_INS_packet2 *)b; - const struct VN_INS_packet1 &pkt1 = *last_ins_pkt1; +void AP_ExternalAHRS_VectorNav::process_ins_packet2(const uint8_t* b) { + const struct VN_INS_packet2& pkt2 = *(struct VN_INS_packet2*)b; + const struct VN_INS_packet1& pkt1 = *_last_ins_pkt1; - last_pkt2_ms = AP_HAL::millis(); - *last_ins_pkt2 = pkt2; + _last_pkt2_ms = AP_HAL::millis(); + *_last_ins_pkt2 = pkt2; AP_ExternalAHRS::gps_data_message_t gps; // get ToW in milliseconds - gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL); - gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL); - gps.fix_type = pkt2.GPS1Fix; + gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL); + gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60 * 60 * 24 * 7 * 1000ULL); + gps.fix_type = pkt2.GPS1Fix; gps.satellites_in_view = pkt2.numGPS1Sats; gps.horizontal_pos_accuracy = pkt1.posU; - gps.vertical_pos_accuracy = pkt1.posU; + gps.vertical_pos_accuracy = pkt1.posU; gps.horizontal_vel_accuracy = pkt1.velU; gps.hdop = pkt2.GPS1DOP[4]; gps.vdop = pkt2.GPS1DOP[3]; - gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7; - gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7; + gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7; + gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7; gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2; gps.ned_vel_north = pkt2.GPS1velNED[0]; - gps.ned_vel_east = pkt2.GPS1velNED[1]; - gps.ned_vel_down = pkt2.GPS1velNED[2]; + gps.ned_vel_east = pkt2.GPS1velNED[1]; + gps.ned_vel_down = pkt2.GPS1velNED[2]; if (gps.fix_type >= 3 && !state.have_origin) { WITH_SEMAPHORE(state.sem); - state.origin = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7), - int32_t(pkt2.GPS1posLLA[1] * 1.0e7), - int32_t(pkt2.GPS1posLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; + state.origin = + Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7), int32_t(pkt2.GPS1posLLA[1] * 1.0e7), + int32_t(pkt2.GPS1posLLA[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; state.have_origin = true; } uint8_t instance; @@ -590,11 +590,10 @@ void AP_ExternalAHRS_VectorNav::process_ins_packet2(const uint8_t *b) /* process AHRS mode AHRS packet */ -void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) -{ - const struct VN_AHRS_packet1 &pkt = *(struct VN_AHRS_packet1 *)b; +void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t* b) { + const struct VN_AHRS_packet1& pkt = *(struct VN_AHRS_packet1*)b; - last_pkt1_ms = AP_HAL::millis(); + _last_pkt1_ms = AP_HAL::millis(); const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU); @@ -605,18 +604,19 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) state.gyro = Vector3f{pkt.uncompAngRate[0], pkt.uncompAngRate[1], pkt.uncompAngRate[2]}; } else { state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]}; - state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}; + state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}; } - state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; + state.quat = + Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; state.have_quaternion = true; } #if AP_BARO_EXTERNALAHRS_ENABLED { AP_ExternalAHRS::baro_data_message_t baro; - baro.instance = 0; - baro.pressure_pa = pkt.pressure*1e3; + baro.instance = 0; + baro.pressure_pa = pkt.pressure * 1e3; baro.temperature = pkt.temp; AP::baro().handle_external(baro); @@ -631,7 +631,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) } else { mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; } - mag.field *= 1000; // to mGauss + mag.field *= 1000; // to mGauss AP::compass().handle_external(mag); } @@ -640,8 +640,8 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) { AP_ExternalAHRS::ins_data_message_t ins; - ins.accel = state.accel; - ins.gyro = state.gyro; + ins.accel = state.accel; + ins.gyro = state.gyro; ins.temperature = pkt.temp; AP::ins().handle_external(ins); @@ -667,54 +667,46 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) // @Field: Q3: Attitude quaternion 3 // @Field: Q4: Attitude quaternion 4 - AP::logger().WriteStreaming("EAH3", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ,Q1,Q2,Q3,Q4", - "sdPGGGoooEEE----", "F000000000000000", - "Qfffffffffffffff", - AP_HAL::micros64(), - pkt.temp, pkt.pressure*1e3, - use_uncomp ? pkt.uncompMag[0] : pkt.mag[0], - use_uncomp ? pkt.uncompMag[1] : pkt.mag[1], - use_uncomp ? pkt.uncompMag[2] : pkt.mag[2], - state.accel[0], state.accel[1], state.accel[2], - state.gyro[0], state.gyro[1], state.gyro[2], - state.quat[0], state.quat[1], state.quat[2], state.quat[3]); + AP::logger().WriteStreaming( + "EAH3", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ,Q1,Q2,Q3,Q4", "sdPGGGoooEEE----", + "F000000000000000", "Qfffffffffffffff", AP_HAL::micros64(), pkt.temp, pkt.pressure * 1e3, + use_uncomp ? pkt.uncompMag[0] : pkt.mag[0], use_uncomp ? pkt.uncompMag[1] : pkt.mag[1], + use_uncomp ? pkt.uncompMag[2] : pkt.mag[2], state.accel[0], state.accel[1], state.accel[2], + state.gyro[0], state.gyro[1], state.gyro[2], state.quat[0], state.quat[1], state.quat[2], + state.quat[3]); + #endif // HAL_LOGGING_ENABLED } - // get serial port number for the uart -int8_t AP_ExternalAHRS_VectorNav::get_port(void) const -{ - if (!uart) { +int8_t AP_ExternalAHRS_VectorNav::get_port(void) const { + if (!_uart) { return -1; } - return port_num; + return _port_num; }; // accessors for AP_AHRS -bool AP_ExternalAHRS_VectorNav::healthy(void) const -{ +bool AP_ExternalAHRS_VectorNav::healthy(void) const { const uint32_t now = AP_HAL::millis(); if (type == TYPE::VN_AHRS) { - return (now - last_pkt1_ms < 40); + return (now - _last_pkt1_ms < 40); } - return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500); + return (now - _last_pkt1_ms < 40 && now - _last_pkt2_ms < 500); } -bool AP_ExternalAHRS_VectorNav::initialised(void) const -{ - if (!setup_complete) { +bool AP_ExternalAHRS_VectorNav::initialised(void) const { + if (!_setup_complete) { return false; } if (type == TYPE::VN_AHRS) { - return last_pkt1_ms != 0; + return _last_pkt1_ms != 0; } - return last_pkt1_ms != 0 && last_pkt2_ms != 0; + return _last_pkt1_ms != 0 && _last_pkt2_ms != 0; } -bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const -{ - if (!setup_complete) { +bool AP_ExternalAHRS_VectorNav::pre_arm_check(char* failure_msg, uint8_t failure_msg_len) const { + if (!_setup_complete) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav setup failed"); return false; } @@ -723,11 +715,11 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure return false; } if (type == TYPE::VN_INS) { - if (last_ins_pkt2->GPS1Fix < 3) { + if (_last_ins_pkt2->GPS1Fix < 3) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock"); return false; } - if (has_dual_gnss && (last_ins_pkt2->GPS2Fix < 3)) { + if (_has_dual_gnss && (_last_ins_pkt2->GPS2Fix < 3)) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock"); return false; } @@ -739,26 +731,25 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure get filter status. We don't know the meaning of the status bits yet, so assume all OK if we have GPS lock */ -void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) const -{ +void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status& status) const { memset(&status, 0, sizeof(status)); if (type == TYPE::VN_INS) { - if (last_ins_pkt1 && last_ins_pkt2) { + if (_last_ins_pkt1 && _last_ins_pkt2) { status.flags.initalized = true; } - if (healthy() && last_ins_pkt2) { + if (healthy() && _last_ins_pkt2) { status.flags.attitude = true; status.flags.vert_vel = true; status.flags.vert_pos = true; - const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2; + const struct VN_INS_packet2& pkt2 = *_last_ins_pkt2; if (pkt2.GPS1Fix >= 3) { - status.flags.horiz_vel = true; - status.flags.horiz_pos_rel = true; - status.flags.horiz_pos_abs = true; + status.flags.horiz_vel = true; + status.flags.horiz_pos_rel = true; + status.flags.horiz_pos_abs = true; status.flags.pred_horiz_pos_rel = true; status.flags.pred_horiz_pos_abs = true; - status.flags.using_gps = true; + status.flags.using_gps = true; } } } else { @@ -770,9 +761,8 @@ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) con } // send an EKF_STATUS message to GCS -void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const -{ - if (!last_ins_pkt1) { +void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK& link) const { + if (!_last_ins_pkt1) { return; } // prepare flags @@ -814,14 +804,13 @@ void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const } // send message - const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)last_ins_pkt1; - const float vel_gate = 5; - const float pos_gate = 5; - const float hgt_gate = 5; - const float mag_var = 0; - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate, - mag_var, 0, 0); + const struct VN_INS_packet1& pkt1 = *(struct VN_INS_packet1*)_last_ins_pkt1; + const float vel_gate = 5; + const float pos_gate = 5; + const float hgt_gate = 5; + const float mag_var = 0; + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, pkt1.velU / vel_gate, + pkt1.posU / pos_gate, pkt1.posU / hgt_gate, mag_var, 0, 0); } #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index b6f4216b55a731..6930229f8cec38 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -25,9 +25,8 @@ #include "AP_ExternalAHRS_backend.h" class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { - public: - AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + AP_ExternalAHRS_VectorNav(AP_ExternalAHRS* frontend, AP_ExternalAHRS::state_t& state); // get serial port number, -1 for not enabled int8_t get_port(void) const override; @@ -35,71 +34,68 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { // accessors for AP_AHRS bool healthy(void) const override; bool initialised(void) const override; - bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; - void get_filter_status(nav_filter_status &status) const override; - void send_status_report(class GCS_MAVLINK &link) const override; + bool pre_arm_check(char* failure_msg, uint8_t failure_msg_len) const override; + void get_filter_status(nav_filter_status& status) const override; + void send_status_report(class GCS_MAVLINK& link) const override; // check for new data - void update() override { - check_uart(); - } + void update() override { check_uart(); } // Get model/type name const char* get_name() const override; protected: + uint8_t num_gps_sensors(void) const override { return 1; } - uint8_t num_gps_sensors(void) const override { - return 1; - } private: - AP_HAL::UARTDriver *uart; - int8_t port_num; - bool setup_complete; - uint32_t baudrate; + AP_HAL::UARTDriver* _uart; + int8_t _port_num; + bool _setup_complete; + uint32_t _baudrate; void update_thread(); bool check_uart(); void initialize(); - void process_ins_packet1(const uint8_t *b); - void process_ins_packet2(const uint8_t *b); - void process_ahrs_packet(const uint8_t *b); + void process_ins_packet1(const uint8_t* b); + void process_ins_packet2(const uint8_t* b); + void process_ahrs_packet(const uint8_t* b); void send_command_blocking(); - uint8_t *pktbuf; - uint16_t pktoffset; - uint16_t bufsize; + uint8_t* _pktbuf; + uint16_t _pktoffset; + uint16_t _bufsize; - struct VN_INS_packet1 *last_ins_pkt1; - struct VN_INS_packet2 *last_ins_pkt2; + struct VN_INS_packet1* _last_ins_pkt1; + struct VN_INS_packet2* _last_ins_pkt2; - uint32_t last_pkt1_ms; - uint32_t last_pkt2_ms; + uint32_t _last_pkt1_ms; + uint32_t _last_pkt2_ms; enum class TYPE { - VN_INS, // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0 + VN_INS, // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0 VN_AHRS, // IMU-only mode, used by VN-1X0 } type; - bool has_dual_gnss = false; + bool _has_dual_gnss = false; - char model_name[20]; + char _model_name[20]; - char message_to_send[50]; + char _message_to_send[50]; // NMEA parsing for setup bool decode(char c); bool decode_latest_term(); struct NMEA_parser { - char term[20]; // buffer for the current term within the current sentence - uint8_t term_offset; // offset within the _term buffer where the next character should be placed - uint8_t term_number; // term index within the current sentence - uint8_t checksum; // checksum accumulator - bool term_is_checksum; // current term is the checksum - bool sentence_valid; // is current sentence valid so far - bool sentence_done; // true if this sentence has already been decoded - } nmea; + char term[20]; // buffer for the current term within the current sentence + uint8_t term_offset; // offset within the _term buffer where the next character should be + // placed + uint8_t term_number; // term index within the current sentence + uint8_t checksum; // checksum accumulator + bool term_is_checksum; // current term is the checksum + bool sentence_valid; // is current sentence valid so far + bool sentence_done; // true if this sentence has already been decoded + } _nmea; }; #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED