2021-03-24 03:24:01 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
2023-10-11 04:41:52 -03:00
|
|
|
support for serial connected AHRS systems
|
2021-03-24 03:24:01 -03:00
|
|
|
*/
|
|
|
|
|
|
|
|
#define ALLOW_DOUBLE_MATH_FUNCTIONS
|
|
|
|
|
2023-04-24 06:54:51 -03:00
|
|
|
#include "AP_ExternalAHRS_config.h"
|
|
|
|
|
|
|
|
#if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|
|
|
|
|
2021-03-24 03:24:01 -03:00
|
|
|
#include "AP_ExternalAHRS_VectorNav.h"
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include <AP_Math/crc.h>
|
|
|
|
#include <AP_Baro/AP_Baro.h>
|
|
|
|
#include <AP_Compass/AP_Compass.h>
|
|
|
|
#include <AP_GPS/AP_GPS.h>
|
|
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2023-08-28 23:26:09 -03:00
|
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
2021-03-24 03:24:01 -03:00
|
|
|
#include <AP_Common/NMEA.h>
|
|
|
|
#include <stdio.h>
|
2021-09-17 14:38:42 -03:00
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
|
|
|
|
/*
|
2024-07-03 00:07:10 -03:00
|
|
|
header for 50Hz INS data
|
2021-03-24 03:24:01 -03:00
|
|
|
assumes the following config for VN-300:
|
2022-12-22 16:57:30 -04:00
|
|
|
$VNWRG,75,3,8,34,072E,0106,0612*0C
|
2022-12-19 12:27:51 -04:00
|
|
|
|
2022-12-22 16:50:38 -04:00
|
|
|
0x34: Groups 3,5,6
|
2022-12-19 12:27:51 -04:00
|
|
|
Group 3 (IMU):
|
2022-12-22 16:57:30 -04:00
|
|
|
0x072E:
|
|
|
|
UncompMag
|
2022-12-19 12:27:51 -04:00
|
|
|
UncompAccel
|
|
|
|
UncompGyro
|
|
|
|
Pres
|
|
|
|
Mag
|
|
|
|
Accel
|
|
|
|
AngularRate
|
|
|
|
Group 5 (Attitude):
|
2022-12-22 16:50:38 -04:00
|
|
|
0x0106:
|
2022-12-19 12:27:51 -04:00
|
|
|
YawPitchRoll
|
|
|
|
Quaternion
|
|
|
|
YprU
|
|
|
|
Group 6 (INS):
|
2022-12-22 16:50:38 -04:00
|
|
|
0x0612:
|
2022-12-19 12:27:51 -04:00
|
|
|
PosLLa
|
|
|
|
VelNed
|
|
|
|
PosU
|
2022-12-22 16:50:38 -04:00
|
|
|
VelU
|
2022-12-19 12:27:51 -04:00
|
|
|
|
2021-03-24 03:24:01 -03:00
|
|
|
*/
|
2024-07-03 00:07:10 -03:00
|
|
|
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
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
struct PACKED VN_INS_packet1 {
|
2022-12-22 16:57:30 -04:00
|
|
|
float uncompMag[3];
|
2021-03-24 03:24:01 -03:00
|
|
|
float uncompAccel[3];
|
|
|
|
float uncompAngRate[3];
|
|
|
|
float pressure;
|
|
|
|
float mag[3];
|
|
|
|
float accel[3];
|
|
|
|
float gyro[3];
|
|
|
|
float ypr[3];
|
|
|
|
float quaternion[4];
|
|
|
|
float yprU[3];
|
|
|
|
double positionLLA[3];
|
|
|
|
float velNED[3];
|
|
|
|
float posU;
|
|
|
|
float velU;
|
|
|
|
};
|
|
|
|
|
|
|
|
// check packet size for 4 groups
|
2024-07-03 00:07:10 -03:00
|
|
|
static_assert(sizeof(VN_INS_packet1)+2+3*2+2 == VN_INS_PKT1_LENGTH, "incorrect VN_INS_packet1 length");
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
/*
|
2024-07-03 00:07:10 -03:00
|
|
|
header for 5Hz GNSS data
|
2021-03-24 03:24:01 -03:00
|
|
|
assumes the following VN-300 config:
|
2022-12-22 16:50:38 -04:00
|
|
|
$VNWRG,76,3,80,4E,0002,0010,20B8,0018*63
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
0x4E: Groups 2,3,4,7
|
|
|
|
Group 2 (Time):
|
|
|
|
0x0002:
|
|
|
|
TimeGps
|
|
|
|
Group 3 (IMU):
|
|
|
|
0x0010:
|
|
|
|
Temp
|
|
|
|
Group 4 (GPS1):
|
|
|
|
0x20B8:
|
|
|
|
NumSats
|
|
|
|
Fix
|
|
|
|
PosLLa
|
|
|
|
VelNed
|
|
|
|
DOP
|
|
|
|
Group 7 (GPS2):
|
2022-12-22 16:50:38 -04:00
|
|
|
0x0018:
|
2022-12-19 12:27:51 -04:00
|
|
|
NumSats
|
|
|
|
Fix
|
2021-03-24 03:24:01 -03:00
|
|
|
*/
|
2024-07-03 00:07:10 -03:00
|
|
|
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
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
struct PACKED VN_INS_packet2 {
|
2021-03-24 03:24:01 -03:00
|
|
|
uint64_t timeGPS;
|
|
|
|
float temp;
|
|
|
|
uint8_t numGPS1Sats;
|
|
|
|
uint8_t GPS1Fix;
|
|
|
|
double GPS1posLLA[3];
|
|
|
|
float GPS1velNED[3];
|
|
|
|
float GPS1DOP[7];
|
|
|
|
uint8_t numGPS2Sats;
|
|
|
|
uint8_t GPS2Fix;
|
|
|
|
};
|
|
|
|
|
|
|
|
// check packet size for 4 groups
|
2024-07-03 00:07:10 -03:00
|
|
|
static_assert(sizeof(VN_INS_packet2)+2+4*2+2 == VN_INS_PKT2_LENGTH, "incorrect VN_INS_packet2 length");
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
/*
|
2024-07-03 00:07:10 -03:00
|
|
|
header for 50Hz IMU data, used in TYPE::VN_AHRS only
|
|
|
|
assumes the following VN-100 config:
|
2022-12-22 16:57:30 -04:00
|
|
|
$VNWRG,75,3,80,14,073E,0004*66
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
Alternate first packet for VN-100
|
|
|
|
0x14: Groups 3, 5
|
|
|
|
Group 3 (IMU):
|
2022-12-22 16:57:30 -04:00
|
|
|
0x073E:
|
|
|
|
UncompMag
|
|
|
|
UncompAccel
|
|
|
|
UncompGyro
|
2022-12-19 12:27:51 -04:00
|
|
|
Temp
|
|
|
|
Pres
|
|
|
|
Mag
|
|
|
|
Accel
|
|
|
|
Gyro
|
|
|
|
Group 5 (Attitude):
|
|
|
|
0x0004:
|
|
|
|
Quaternion
|
|
|
|
*/
|
2024-07-03 00:07:10 -03:00
|
|
|
static const uint8_t vn_ahrs_pkt1_header[] { 0x14, 0x3E, 0x07, 0x04, 0x00 };
|
|
|
|
#define VN_AHRS_PKT1_LENGTH 104 // includes header and CRC
|
2022-12-19 12:27:51 -04:00
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
struct PACKED VN_AHRS_packet1 {
|
2022-12-22 16:57:30 -04:00
|
|
|
float uncompMag[3];
|
|
|
|
float uncompAccel[3];
|
|
|
|
float uncompAngRate[3];
|
2022-12-19 12:27:51 -04:00
|
|
|
float temp;
|
|
|
|
float pressure;
|
|
|
|
float mag[3];
|
|
|
|
float accel[3];
|
|
|
|
float gyro[3];
|
|
|
|
float quaternion[4];
|
|
|
|
};
|
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
static_assert(sizeof(VN_AHRS_packet1)+2+2*2+2 == VN_AHRS_PKT1_LENGTH, "incorrect VN_AHRS_packet1 length");
|
2022-12-19 12:27:51 -04:00
|
|
|
|
2021-03-24 03:24:01 -03:00
|
|
|
// 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) {
|
2023-11-11 02:01:10 -04:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VectorNav ExternalAHRS no UART");
|
2021-03-24 03:24:01 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
|
|
|
|
port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
|
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
bufsize = MAX(MAX(VN_INS_PKT1_LENGTH, VN_INS_PKT2_LENGTH), VN_AHRS_PKT1_LENGTH);
|
2024-05-26 22:24:10 -03:00
|
|
|
pktbuf = NEW_NOTHROW uint8_t[bufsize];
|
2024-07-03 00:07:10 -03:00
|
|
|
last_ins_pkt1 = NEW_NOTHROW VN_INS_packet1;
|
|
|
|
last_ins_pkt2 = NEW_NOTHROW VN_INS_packet2;
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
if (!pktbuf || !last_ins_pkt1 || !last_ins_pkt2) {
|
2023-11-11 02:01:10 -04:00
|
|
|
AP_BoardConfig::allocation_error("VectorNav ExternalAHRS");
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
|
2023-11-11 02:01:10 -04:00
|
|
|
AP_HAL::panic("VectorNav Failed to start ExternalAHRS update thread");
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
2023-11-11 02:01:10 -04:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VectorNav ExternalAHRS initialised");
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
check the UART for more data
|
|
|
|
returns true if the function should be called again straight away
|
|
|
|
*/
|
2022-12-19 12:27:51 -04:00
|
|
|
#define SYNC_BYTE 0xFA
|
2021-03-24 03:24:01 -03:00
|
|
|
bool AP_ExternalAHRS_VectorNav::check_uart()
|
|
|
|
{
|
2022-12-19 12:27:51 -04:00
|
|
|
if (!setup_complete) {
|
2021-03-24 03:24:01 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
WITH_SEMAPHORE(state.sem);
|
2023-12-07 00:27:16 -04:00
|
|
|
// ensure we own the uart
|
|
|
|
uart->begin(0);
|
2021-03-24 03:24:01 -03:00
|
|
|
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 (nread <= 0) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
pktoffset += nread;
|
|
|
|
}
|
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
bool match_header1 = false;
|
|
|
|
bool match_header2 = false;
|
|
|
|
bool match_header3 = false;
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
if (pktbuf[0] != SYNC_BYTE) {
|
2021-03-24 03:24:01 -03:00
|
|
|
goto reset;
|
|
|
|
}
|
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
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))));
|
2022-12-19 12:27:51 -04:00
|
|
|
} else {
|
2024-07-03 00:07:10 -03:00
|
|
|
match_header3 = (0 == memcmp(&pktbuf[1], vn_ahrs_pkt1_header, MIN(sizeof(vn_ahrs_pkt1_header), unsigned(pktoffset-1))));
|
2022-12-19 12:27:51 -04:00
|
|
|
}
|
|
|
|
if (!match_header1 && !match_header2 && !match_header3) {
|
2021-03-24 03:24:01 -03:00
|
|
|
goto reset;
|
|
|
|
}
|
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
if (match_header1 && pktoffset >= VN_INS_PKT1_LENGTH) {
|
|
|
|
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT1_LENGTH-1, 0);
|
2021-03-24 03:24:01 -03:00
|
|
|
if (crc == 0) {
|
|
|
|
// got pkt1
|
2024-07-03 00:07:10 -03:00
|
|
|
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;
|
2021-03-24 03:24:01 -03:00
|
|
|
} else {
|
|
|
|
goto reset;
|
|
|
|
}
|
2024-07-03 00:07:10 -03:00
|
|
|
} else if (match_header2 && pktoffset >= VN_INS_PKT2_LENGTH) {
|
|
|
|
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT2_LENGTH-1, 0);
|
2021-03-24 03:24:01 -03:00
|
|
|
if (crc == 0) {
|
|
|
|
// got pkt2
|
2024-07-03 00:07:10 -03:00
|
|
|
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;
|
2021-03-24 03:24:01 -03:00
|
|
|
} else {
|
|
|
|
goto reset;
|
|
|
|
}
|
2024-07-03 00:07:10 -03:00
|
|
|
} else if (match_header3 && pktoffset >= VN_AHRS_PKT1_LENGTH) {
|
|
|
|
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_PKT1_LENGTH-1, 0);
|
2022-12-19 12:27:51 -04:00
|
|
|
if (crc == 0) {
|
2024-07-03 00:07:10 -03:00
|
|
|
// 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;
|
2022-12-19 12:27:51 -04:00
|
|
|
} else {
|
|
|
|
goto reset;
|
|
|
|
}
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
return true;
|
|
|
|
|
|
|
|
reset:
|
2022-12-19 12:27:51 -04:00
|
|
|
uint8_t *p = (uint8_t *)memchr(&pktbuf[1], SYNC_BYTE, pktoffset-1);
|
2021-03-24 03:24:01 -03:00
|
|
|
if (p) {
|
|
|
|
uint8_t newlen = pktoffset - (p - pktbuf);
|
|
|
|
memmove(&pktbuf[0], p, newlen);
|
|
|
|
pktoffset = newlen;
|
|
|
|
} else {
|
|
|
|
pktoffset = 0;
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2024-07-05 13:40:20 -03:00
|
|
|
// Send command and wait for response
|
2024-07-09 12:53:16 -03:00
|
|
|
// Only run from thread! This blocks and retries until a non-error response is received
|
2022-12-19 12:27:51 -04:00
|
|
|
#define READ_REQUEST_RETRY_MS 500
|
2024-07-10 14:19:17 -03:00
|
|
|
void AP_ExternalAHRS_VectorNav::run_command(const char * fmt, ...)
|
2022-12-19 12:27:51 -04:00
|
|
|
{
|
2024-07-10 14:19:17 -03:00
|
|
|
va_list ap;
|
|
|
|
|
|
|
|
va_start(ap, fmt);
|
|
|
|
hal.util->vsnprintf(message_to_send, sizeof(message_to_send), fmt, ap);
|
|
|
|
va_end(ap);
|
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
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) {
|
2024-07-05 13:40:20 -03:00
|
|
|
nmea_printf(uart, "$%s", message_to_send);
|
2022-12-19 12:27:51 -04:00
|
|
|
request_sent = now;
|
|
|
|
}
|
|
|
|
|
|
|
|
int16_t nbytes = uart->available();
|
|
|
|
while (nbytes-- > 0) {
|
|
|
|
char c = uart->read();
|
|
|
|
if (decode(c)) {
|
2024-07-10 14:19:17 -03:00
|
|
|
if (nmea.error_response && nmea.sentence_done) {
|
2024-07-08 20:38:49 -03:00
|
|
|
// Received a valid VNERR. Try to resend after the timeout length
|
|
|
|
break;
|
|
|
|
}
|
2022-12-19 12:27:51 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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)
|
|
|
|
{
|
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
|
|
|
|
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;
|
2024-07-08 20:38:49 -03:00
|
|
|
nmea.error_response = false;
|
2022-12-19 12:27:51 -04:00
|
|
|
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_is_checksum) {
|
|
|
|
nmea.checksum ^= c;
|
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// decode the most recently consumed term
|
2024-07-05 13:40:20 -03:00
|
|
|
// returns true if new term is valid
|
2022-12-19 12:27:51 -04:00
|
|
|
bool AP_ExternalAHRS_VectorNav::decode_latest_term()
|
|
|
|
{
|
2024-07-05 13:40:20 -03:00
|
|
|
// Check the first two terms (In most cases header + reg number) that they match the sent
|
|
|
|
// message. If not, the response is invalid.
|
2022-12-19 12:27:51 -04:00
|
|
|
switch (nmea.term_number) {
|
|
|
|
case 0:
|
2024-07-09 12:53:16 -03:00
|
|
|
if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) {
|
2024-07-08 20:38:49 -03:00
|
|
|
nmea.error_response = true; // Message will be printed on next term
|
2024-07-10 14:19:17 -03:00
|
|
|
} else if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) {
|
|
|
|
return false;
|
2022-12-19 12:27:51 -04:00
|
|
|
}
|
2024-07-09 12:53:16 -03:00
|
|
|
return true;
|
2024-07-05 13:40:20 -03:00
|
|
|
case 1:
|
2024-07-08 20:38:49 -03:00
|
|
|
if (nmea.error_response) {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
|
2024-07-10 14:19:17 -03:00
|
|
|
} else if (strlen(message_to_send) > 6 &&
|
|
|
|
strncmp(nmea.term, &message_to_send[6], nmea.term_offset != 0)) { // Start after "VNXXX,"
|
2022-12-19 12:27:51 -04:00
|
|
|
return false;
|
|
|
|
}
|
2024-07-09 12:53:16 -03:00
|
|
|
return true;
|
2024-07-05 13:40:20 -03:00
|
|
|
case 2:
|
|
|
|
if (strncmp(nmea.term, "VN-", 3) == 0) {
|
|
|
|
// This term is the model number
|
|
|
|
strncpy(model_name, nmea.term, sizeof(model_name));
|
|
|
|
}
|
2024-07-09 12:53:16 -03:00
|
|
|
return true;
|
2022-12-19 12:27:51 -04:00
|
|
|
default:
|
2024-07-09 12:53:16 -03:00
|
|
|
return true;
|
2022-12-19 12:27:51 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2024-07-05 13:40:20 -03:00
|
|
|
void AP_ExternalAHRS_VectorNav::initialize() {
|
2022-12-19 12:27:51 -04:00
|
|
|
// Open port in the thread
|
|
|
|
uart->begin(baudrate, 1024, 512);
|
|
|
|
|
2024-07-05 13:40:51 -03:00
|
|
|
// Pause asynchronous communications to simplify packet finding
|
2024-07-10 14:19:17 -03:00
|
|
|
run_command("VNASY,0");
|
2024-07-05 13:40:51 -03:00
|
|
|
|
|
|
|
// 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
|
2024-07-10 14:19:17 -03:00
|
|
|
run_command("VNWRG,06,0,1");
|
|
|
|
run_command("VNWRG,06,0,2");
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
// Read Model Number Register, ID 1
|
2024-07-10 14:19:17 -03:00
|
|
|
run_command("VNRRG,01");
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
// Setup for messages respective model types (on both UARTs)
|
2024-07-03 00:07:10 -03:00
|
|
|
if (strncmp(model_name, "VN-1", 4) == 0) {
|
2024-07-05 13:40:20 -03:00
|
|
|
// VN-1X0
|
2024-07-03 00:07:10 -03:00
|
|
|
type = TYPE::VN_AHRS;
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
// This assumes unit is still configured at its default rate of 800hz
|
2024-07-10 14:19:17 -03:00
|
|
|
run_command("VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate()));
|
2022-12-19 12:27:51 -04:00
|
|
|
} else {
|
2024-07-03 00:07:10 -03:00
|
|
|
// 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
|
|
|
|
uint16_t imu_rate = 800; // Default for everything but VN-300
|
|
|
|
if (strncmp(model_name, "VN-300", 6) == 0) {
|
|
|
|
imu_rate = 400;
|
|
|
|
}
|
|
|
|
if (strncmp(model_name, "VN-3", 4) == 0) {
|
|
|
|
has_dual_gnss = true;
|
|
|
|
}
|
2024-07-10 14:19:17 -03:00
|
|
|
run_command("VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate()));
|
|
|
|
run_command("VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5));
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
|
2024-07-05 13:40:51 -03:00
|
|
|
// Resume asynchronous communications
|
2024-07-10 14:19:17 -03:00
|
|
|
run_command("VNASY,1");
|
2022-12-19 12:27:51 -04:00
|
|
|
setup_complete = true;
|
2024-07-05 13:40:20 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_ExternalAHRS_VectorNav::update_thread() {
|
|
|
|
initialize();
|
2021-03-24 03:24:01 -03:00
|
|
|
while (true) {
|
|
|
|
if (!check_uart()) {
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
const char* AP_ExternalAHRS_VectorNav::get_name() const
|
|
|
|
{
|
|
|
|
if (setup_complete) {
|
|
|
|
return model_name;
|
|
|
|
}
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
2021-03-24 03:24:01 -03:00
|
|
|
/*
|
2024-07-03 00:07:10 -03:00
|
|
|
process INS mode INS packet
|
2021-03-24 03:24:01 -03:00
|
|
|
*/
|
2024-07-03 00:07:10 -03:00
|
|
|
void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b)
|
2021-03-24 03:24:01 -03:00
|
|
|
{
|
2024-07-03 00:07:10 -03:00
|
|
|
const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)b;
|
|
|
|
const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2;
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
last_pkt1_ms = AP_HAL::millis();
|
2024-07-03 00:07:10 -03:00
|
|
|
*last_ins_pkt1 = pkt1;
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2022-12-22 17:16:10 -04:00
|
|
|
const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU);
|
|
|
|
|
2021-03-24 03:24:01 -03:00
|
|
|
{
|
|
|
|
WITH_SEMAPHORE(state.sem);
|
2022-12-22 17:16:10 -04:00
|
|
|
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]};
|
|
|
|
} 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]};
|
|
|
|
}
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
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.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};
|
2023-12-01 21:10:08 -04:00
|
|
|
state.last_location_update_us = AP_HAL::micros();
|
2021-03-24 03:24:01 -03:00
|
|
|
state.have_location = true;
|
|
|
|
}
|
2022-05-24 03:33:48 -03:00
|
|
|
|
|
|
|
#if AP_BARO_EXTERNALAHRS_ENABLED
|
2021-03-24 03:24:01 -03:00
|
|
|
{
|
|
|
|
AP_ExternalAHRS::baro_data_message_t baro;
|
|
|
|
baro.instance = 0;
|
|
|
|
baro.pressure_pa = pkt1.pressure*1e3;
|
|
|
|
baro.temperature = pkt2.temp;
|
|
|
|
|
|
|
|
AP::baro().handle_external(baro);
|
|
|
|
}
|
2022-05-24 03:33:48 -03:00
|
|
|
#endif
|
2021-03-24 03:24:01 -03:00
|
|
|
|
2023-02-19 21:05:20 -04:00
|
|
|
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
2021-03-24 03:24:01 -03:00
|
|
|
{
|
|
|
|
AP_ExternalAHRS::mag_data_message_t mag;
|
|
|
|
mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]};
|
|
|
|
mag.field *= 1000; // to mGauss
|
|
|
|
|
|
|
|
AP::compass().handle_external(mag);
|
|
|
|
}
|
2023-02-19 21:05:20 -04:00
|
|
|
#endif
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
{
|
|
|
|
AP_ExternalAHRS::ins_data_message_t ins;
|
|
|
|
|
|
|
|
ins.accel = state.accel;
|
|
|
|
ins.gyro = state.gyro;
|
|
|
|
ins.temperature = pkt2.temp;
|
|
|
|
|
|
|
|
AP::ins().handle_external(ins);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2024-07-03 00:07:10 -03:00
|
|
|
process INS mode GNSS packet
|
2021-03-24 03:24:01 -03:00
|
|
|
*/
|
2024-07-03 00:07:10 -03:00
|
|
|
void AP_ExternalAHRS_VectorNav::process_ins_packet2(const uint8_t *b)
|
2021-03-24 03:24:01 -03:00
|
|
|
{
|
2024-07-03 00:07:10 -03:00
|
|
|
const struct VN_INS_packet2 &pkt2 = *(struct VN_INS_packet2 *)b;
|
|
|
|
const struct VN_INS_packet1 &pkt1 = *last_ins_pkt1;
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
last_pkt2_ms = AP_HAL::millis();
|
2024-07-03 00:07:10 -03:00
|
|
|
*last_ins_pkt2 = pkt2;
|
2021-03-24 03:24:01 -03:00
|
|
|
|
|
|
|
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.satellites_in_view = pkt2.numGPS1Sats;
|
|
|
|
|
|
|
|
gps.horizontal_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.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];
|
|
|
|
|
|
|
|
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.have_origin = true;
|
|
|
|
}
|
2023-08-18 14:17:22 -03:00
|
|
|
uint8_t instance;
|
|
|
|
if (AP::gps().get_first_external_instance(instance)) {
|
|
|
|
AP::gps().handle_external(gps, instance);
|
|
|
|
}
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
/*
|
2024-07-03 00:07:10 -03:00
|
|
|
process AHRS mode AHRS packet
|
2022-12-19 12:27:51 -04:00
|
|
|
*/
|
2024-07-03 00:07:10 -03:00
|
|
|
void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b)
|
2022-12-19 12:27:51 -04:00
|
|
|
{
|
2024-07-03 00:07:10 -03:00
|
|
|
const struct VN_AHRS_packet1 &pkt = *(struct VN_AHRS_packet1 *)b;
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
last_pkt1_ms = AP_HAL::millis();
|
2022-12-22 17:16:10 -04:00
|
|
|
|
|
|
|
const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU);
|
|
|
|
|
2022-12-19 12:27:51 -04:00
|
|
|
{
|
|
|
|
WITH_SEMAPHORE(state.sem);
|
2022-12-22 17:16:10 -04:00
|
|
|
if (use_uncomp) {
|
|
|
|
state.accel = Vector3f{pkt.uncompAccel[0], pkt.uncompAccel[1], pkt.uncompAccel[2]};
|
|
|
|
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]};
|
|
|
|
}
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
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.temperature = pkt.temp;
|
|
|
|
|
|
|
|
AP::baro().handle_external(baro);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2023-02-19 21:05:20 -04:00
|
|
|
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
2022-12-19 12:27:51 -04:00
|
|
|
{
|
|
|
|
AP_ExternalAHRS::mag_data_message_t mag;
|
2022-12-22 17:16:10 -04:00
|
|
|
if (use_uncomp) {
|
|
|
|
mag.field = Vector3f{pkt.uncompMag[0], pkt.uncompMag[1], pkt.uncompMag[2]};
|
|
|
|
} else {
|
|
|
|
mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]};
|
|
|
|
}
|
2022-12-19 12:27:51 -04:00
|
|
|
mag.field *= 1000; // to mGauss
|
|
|
|
|
|
|
|
AP::compass().handle_external(mag);
|
|
|
|
}
|
2023-02-19 21:05:20 -04:00
|
|
|
#endif
|
2022-12-19 12:27:51 -04:00
|
|
|
|
|
|
|
{
|
|
|
|
AP_ExternalAHRS::ins_data_message_t ins;
|
|
|
|
|
|
|
|
ins.accel = state.accel;
|
|
|
|
ins.gyro = state.gyro;
|
|
|
|
ins.temperature = pkt.temp;
|
|
|
|
|
|
|
|
AP::ins().handle_external(ins);
|
|
|
|
}
|
|
|
|
|
2023-07-13 21:58:05 -03:00
|
|
|
#if HAL_LOGGING_ENABLED
|
2022-12-19 12:27:51 -04:00
|
|
|
// @LoggerMessage: EAH3
|
|
|
|
// @Description: External AHRS data
|
|
|
|
// @Field: TimeUS: Time since system startup
|
|
|
|
// @Field: Temp: Temprature
|
|
|
|
// @Field: Pres: Pressure
|
|
|
|
// @Field: MX: Magnetic feild X-axis
|
|
|
|
// @Field: MY: Magnetic feild Y-axis
|
|
|
|
// @Field: MZ: Magnetic feild Z-axis
|
|
|
|
// @Field: AX: Acceleration X-axis
|
|
|
|
// @Field: AY: Acceleration Y-axis
|
|
|
|
// @Field: AZ: Acceleration Z-axis
|
|
|
|
// @Field: GX: Rotation rate X-axis
|
|
|
|
// @Field: GY: Rotation rate Y-axis
|
|
|
|
// @Field: GZ: Rotation rate Z-axis
|
|
|
|
// @Field: Q1: Attitude quaternion 1
|
|
|
|
// @Field: Q2: Attitude quaternion 2
|
|
|
|
// @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,
|
2022-12-22 17:16:10 -04:00
|
|
|
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]);
|
2023-07-13 21:58:05 -03:00
|
|
|
#endif // HAL_LOGGING_ENABLED
|
2022-12-19 12:27:51 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2021-03-24 03:24:01 -03:00
|
|
|
// get serial port number for the uart
|
|
|
|
int8_t AP_ExternalAHRS_VectorNav::get_port(void) const
|
|
|
|
{
|
|
|
|
if (!uart) {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
return port_num;
|
|
|
|
};
|
|
|
|
|
|
|
|
// accessors for AP_AHRS
|
|
|
|
bool AP_ExternalAHRS_VectorNav::healthy(void) const
|
|
|
|
{
|
2022-12-19 12:27:51 -04:00
|
|
|
const uint32_t now = AP_HAL::millis();
|
2024-07-03 00:07:10 -03:00
|
|
|
if (type == TYPE::VN_AHRS) {
|
2022-12-19 12:27:51 -04:00
|
|
|
return (now - last_pkt1_ms < 40);
|
|
|
|
}
|
2021-03-24 03:24:01 -03:00
|
|
|
return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500);
|
|
|
|
}
|
|
|
|
|
|
|
|
bool AP_ExternalAHRS_VectorNav::initialised(void) const
|
|
|
|
{
|
2022-12-19 12:27:51 -04:00
|
|
|
if (!setup_complete) {
|
|
|
|
return false;
|
|
|
|
}
|
2024-07-03 00:07:10 -03:00
|
|
|
if (type == TYPE::VN_AHRS) {
|
2022-12-19 12:27:51 -04:00
|
|
|
return last_pkt1_ms != 0;
|
|
|
|
}
|
2021-03-24 03:24:01 -03:00
|
|
|
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
|
|
|
|
{
|
2022-12-19 12:27:51 -04:00
|
|
|
if (!setup_complete) {
|
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav setup failed");
|
2021-03-24 03:24:01 -03:00
|
|
|
return false;
|
|
|
|
}
|
2022-12-19 12:27:51 -04:00
|
|
|
if (!healthy()) {
|
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav unhealthy");
|
2021-03-24 03:24:01 -03:00
|
|
|
return false;
|
|
|
|
}
|
2024-07-03 00:07:10 -03:00
|
|
|
if (type == TYPE::VN_INS) {
|
|
|
|
if (last_ins_pkt2->GPS1Fix < 3) {
|
2022-12-19 12:27:51 -04:00
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock");
|
|
|
|
return false;
|
|
|
|
}
|
2024-07-03 00:07:10 -03:00
|
|
|
if (has_dual_gnss && (last_ins_pkt2->GPS2Fix < 3)) {
|
2022-12-19 12:27:51 -04:00
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock");
|
|
|
|
return false;
|
|
|
|
}
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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
|
|
|
|
{
|
|
|
|
memset(&status, 0, sizeof(status));
|
2024-07-03 00:07:10 -03:00
|
|
|
if (type == TYPE::VN_INS) {
|
|
|
|
if (last_ins_pkt1 && last_ins_pkt2) {
|
2023-09-24 06:00:11 -03:00
|
|
|
status.flags.initalized = true;
|
2022-12-19 12:27:51 -04:00
|
|
|
}
|
2024-07-03 00:07:10 -03:00
|
|
|
if (healthy() && last_ins_pkt2) {
|
2023-09-24 06:00:11 -03:00
|
|
|
status.flags.attitude = true;
|
|
|
|
status.flags.vert_vel = true;
|
|
|
|
status.flags.vert_pos = true;
|
2022-12-19 12:27:51 -04:00
|
|
|
|
2024-07-03 00:07:10 -03:00
|
|
|
const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2;
|
2022-12-19 12:27:51 -04:00
|
|
|
if (pkt2.GPS1Fix >= 3) {
|
2023-09-24 06:00:11 -03:00
|
|
|
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;
|
2022-12-19 12:27:51 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
status.flags.initalized = initialised();
|
|
|
|
if (healthy()) {
|
|
|
|
status.flags.attitude = true;
|
2021-03-24 03:24:01 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// send an EKF_STATUS message to GCS
|
2022-07-08 01:15:35 -03:00
|
|
|
void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const
|
2021-03-24 03:24:01 -03:00
|
|
|
{
|
2024-07-03 00:07:10 -03:00
|
|
|
if (!last_ins_pkt1) {
|
2021-03-24 03:24:01 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
// prepare flags
|
|
|
|
uint16_t flags = 0;
|
|
|
|
nav_filter_status filterStatus;
|
|
|
|
get_filter_status(filterStatus);
|
|
|
|
if (filterStatus.flags.attitude) {
|
|
|
|
flags |= EKF_ATTITUDE;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.horiz_vel) {
|
|
|
|
flags |= EKF_VELOCITY_HORIZ;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.vert_vel) {
|
|
|
|
flags |= EKF_VELOCITY_VERT;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.horiz_pos_rel) {
|
|
|
|
flags |= EKF_POS_HORIZ_REL;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.horiz_pos_abs) {
|
|
|
|
flags |= EKF_POS_HORIZ_ABS;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.vert_pos) {
|
|
|
|
flags |= EKF_POS_VERT_ABS;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.terrain_alt) {
|
|
|
|
flags |= EKF_POS_VERT_AGL;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.const_pos_mode) {
|
|
|
|
flags |= EKF_CONST_POS_MODE;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.pred_horiz_pos_rel) {
|
|
|
|
flags |= EKF_PRED_POS_HORIZ_REL;
|
|
|
|
}
|
|
|
|
if (filterStatus.flags.pred_horiz_pos_abs) {
|
|
|
|
flags |= EKF_PRED_POS_HORIZ_ABS;
|
|
|
|
}
|
|
|
|
if (!filterStatus.flags.initalized) {
|
|
|
|
flags |= EKF_UNINITIALIZED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// send message
|
2024-07-03 00:07:10 -03:00
|
|
|
const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)last_ins_pkt1;
|
2021-03-24 03:24:01 -03:00
|
|
|
const float vel_gate = 5;
|
|
|
|
const float pos_gate = 5;
|
|
|
|
const float hgt_gate = 5;
|
|
|
|
const float mag_var = 0;
|
2022-07-08 01:15:35 -03:00
|
|
|
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
|
2021-03-24 03:24:01 -03:00
|
|
|
pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate,
|
|
|
|
mag_var, 0, 0);
|
|
|
|
}
|
|
|
|
|
2023-04-24 06:54:51 -03:00
|
|
|
#endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|