Skip to content

Commit

Permalink
AP_ExternalAHRS_VectorNav: Fix build
Browse files Browse the repository at this point in the history 
Minor changes to fix build and use due to bench testing
  • Loading branch information
@lashVN
lashVN committed Jul 17, 2024
1 parent c9b994c commit 35a71f6
Showing 1 changed file with 24 additions and 34 deletions.
58 changes: 24 additions & 34 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ extern const AP_HAL::HAL &hal;


/*
Type::AHRS configures 2 packets: high-rate IMU and mid-rate EKF
TYPE::VN_AHRS configures 2 packets: high-rate IMU and mid-rate EKF
Header for IMU packet
$VNWRG,75,3,16,01,0721*D415
Common group (Group 1)
Expand Down Expand Up @@ -85,7 +85,7 @@ constexpr uint8_t VN_AHRS_ekf_packet::header[];
constexpr uint8_t VN_AHRS_EKF_LENGTH = sizeof(VN_AHRS_ekf_packet) + sizeof(VN_AHRS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC

/*
Type::INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS
TYPE::VN_INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS
Header for IMU packet
$VNWRG,75,3,16,01,0721*D415
Common group (Group 1)
Expand All @@ -111,7 +111,7 @@ Header for EKF packet
Header for GNSS packet
$VNWRG,77,1,160,49,0003,26B8,0018*4FD9
Common group (Group 1)
timeStartup
TimeStartup
TimeGps
Gnss1 group (Group 3)
NumSats
Expand Down Expand Up @@ -231,22 +231,20 @@ bool AP_ExternalAHRS_VectorNav::check_uart()
bool match_header2 = false;
bool match_header3 = false;
bool match_header4 = false;
bool match_header5 = false;

if (pktbuf[0] != SYNC_BYTE) {
goto reset;
}


match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1))));
if (type == TYPE::VN_AHRS) {
match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1))));
match_header2 = (0 == memcmp(&pktbuf[1], VN_AHRS_ekf_packet::header, MIN(sizeof(VN_AHRS_ekf_packet::header), unsigned(pktoffset - 1))));
} else {
match_header3 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1))));
match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1))));
match_header5 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1))));
match_header3 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1))));
match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1))));
}
if (!match_header1 && !match_header2 && !match_header3 && !match_header4 && !match_header5) {
if (!match_header1 && !match_header2 && !match_header3 && !match_header4) {
goto reset;
}

Expand All @@ -268,17 +266,7 @@ bool AP_ExternalAHRS_VectorNav::check_uart()
} else {
goto reset;
}
} else if (match_header3 && pktoffset >= VN_IMU_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_IMU_LENGTH - 1, 0);
if (crc == 0) {
// got pkt1
process_imu_packet(&pktbuf[sizeof(VN_IMU_packet::header) + 1]);
memmove(&pktbuf[0], &pktbuf[VN_IMU_LENGTH], pktoffset - VN_IMU_LENGTH);
pktoffset -= VN_IMU_LENGTH;
} else {
goto reset;
}
} else if (match_header4 && pktoffset >= VN_INS_EKF_LENGTH) {
} else if (match_header3 && pktoffset >= VN_INS_EKF_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_EKF_LENGTH - 1, 0);
if (crc == 0) {
process_ins_ekf_packet(&pktbuf[sizeof(VN_INS_ekf_packet::header) + 1]);
Expand All @@ -287,7 +275,7 @@ bool AP_ExternalAHRS_VectorNav::check_uart()
} else {
goto reset;
}
} else if (match_header5 && pktoffset >= VN_INS_GNSS_LENGTH) {
} else if (match_header4 && pktoffset >= VN_INS_GNSS_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_GNSS_LENGTH - 1, 0);
if (crc == 0) {
process_ins_gnss_packet(&pktbuf[sizeof(VN_INS_gnss_packet::header) + 1]);
Expand Down Expand Up @@ -420,8 +408,8 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term()
case 1:
if (nmea.error_response) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
} else if (strncmp(nmea.term, message_to_send + 6,
nmea.term_offset) != 0) { // Start after "VNXXX,"
} else if (strlen(message_to_send) > 6 &&
strncmp(nmea.term, &message_to_send[6], nmea.term_offset != 0)) { // Start after "VNXXX,"
return false;
}
return true;
Expand Down Expand Up @@ -458,7 +446,7 @@ void AP_ExternalAHRS_VectorNav::initialize() {

// These assumes unit is still configured at its default rate of 800hz
run_command("VNWRG,75,3,%u,01,0721", unsigned(800 / get_rate()));
run_command("VNWRG,76,3,16,11,0001,0104");
run_command("VNWRG,76,3,16,11,0001,0106");
} 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
Expand All @@ -470,7 +458,7 @@ void AP_ExternalAHRS_VectorNav::initialize() {
has_dual_gnss = true;
}
run_command("VNWRG,75,3,%u,01,0721", unsigned(imu_rate / get_rate()));
run_command("VNWRG,76,3,%u,31,0001,0104,0613", unsigned(imu_rate / 50));
run_command("VNWRG,76,3,%u,31,0001,0106,0613", unsigned(imu_rate / 50));
run_command("VNWRG,77,3,%u,49,0003,26B8,0018", unsigned(imu_rate / 5));
}

Expand Down Expand Up @@ -548,7 +536,7 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b)
}

#if HAL_LOGGING_ENABLED
// @LoggerMessage: EAHIM
// @LoggerMessage: EAHI
// @Description: External AHRS IMU data
// @Field: TimeUS: Time since system startup
// @Field: Temp: Temprature
Expand Down Expand Up @@ -585,7 +573,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) {
state.have_quaternion = true;

#if HAL_LOGGING_ENABLED
// @LoggerMessage: EAHAT
// @LoggerMessage: EAHA
// @Description: External AHRS Attitude data
// @Field: TimeUS: Time since system startup
// @Field: Q1: Attitude quaternion 1
Expand Down Expand Up @@ -629,8 +617,8 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
#if HAL_LOGGING_ENABLED
auto now = AP_HAL::micros64();

// @LoggerMessage: EAHAT
// @Description: External AHRS Kalman data
// @LoggerMessage: EAHA
// @Description: External AHRS Attitude data
// @Field: TimeUS: Time since system startup
// @Field: Q1: Attitude quaternion 1
// @Field: Q2: Attitude quaternion 2
Expand All @@ -651,7 +639,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
pkt.ypr[0], pkt.ypr[1], pkt.ypr[2],
pkt.yprU[0], pkt.yprU[1], pkt.yprU[2]);

// @LoggerMessage: EAHKF
// @LoggerMessage: EAHK
// @Description: External AHRS INS Kalman Filter data
// @Field: TimeUS: Time since system startup
// @Field: InsStatus: VectorNav INS health status
Expand All @@ -665,11 +653,11 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
// @Field: VelU: Filter estimated Velocity uncertainty

AP::logger().WriteStreaming("EAHKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
"s-ddmnnndd", "F00000000",
"s-ddmnnndn", "F00000000",
"QHdddfffff",
now,
pkt.insStatus,
pkt.lla[0], pkt.lla[1], pkt.lla[2],
pkt.posLla[0], pkt.posLla[1], pkt.posLla[2],
pkt.velNed[0], pkt.velNed[1], pkt.velNed[2],
pkt.posU, pkt.velU);
#endif // HAL_LOGGING_ENABLED
Expand All @@ -680,7 +668,9 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) {
const struct VN_INS_gnss_packet &pkt = *(struct VN_INS_gnss_packet *)b;
AP_ExternalAHRS::gps_data_message_t gps;


last_pkt3_ms = AP_HAL::millis();
latest_ins_gnss_packet = &pkt;

// get ToW in milliseconds
gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL);
Expand Down Expand Up @@ -736,7 +726,7 @@ bool AP_ExternalAHRS_VectorNav::initialised(void) const
if (!setup_complete) {
return false;
}
return last_pkt1_ms != 0 && last_pkt2_ms != 0 && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != 0);
return last_pkt1_ms != UINT32_MAX && last_pkt2_ms != UINT32_MAX && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != UINT32_MAX);
}

bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
Expand All @@ -750,7 +740,7 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure
return false;
}
if (type == TYPE::VN_INS) {
if (latest_ins_gnss_packet->fix2 < 3) {
if (latest_ins_gnss_packet->fix1 < 3) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock");
return false;
}
Expand Down

0 comments on commit 35a71f6

Please sign in to comment.