Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ExternalAHRS: Correct VN100 pre-arm handling

This commit is contained in:
rishabsingh3003 2024-09-28 22:34:06 -04:00
parent 637aec0085
commit 7ca6910bf6
3 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -271,7 +271,16 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
}
}
if (!state.have_origin) {
bool check_origin = true;
// don't run this check for VN100 Series since its IMU only
if (DevType(devtype) == DevType::VecNav) {
const char* name = get_name();
if (name && !strncmp(name, "VN-1", 4)) {
check_origin = false;
}
}
if (check_origin && !state.have_origin) {
hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin");
return false;
}

16
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View File

@ -446,6 +446,11 @@ 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,0106");
WITH_SEMAPHORE(state.sem);
set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::IMU) |
uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
} 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
@ -459,6 +464,12 @@ void AP_ExternalAHRS_VectorNav::initialize() {
run_command("VNWRG,75,3,%u,01,0721", unsigned(imu_rate / get_rate()));
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));
WITH_SEMAPHORE(state.sem);
set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::IMU) |
uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS) |
uint16_t(AP_ExternalAHRS::AvailableSensor::GPS));
}
// Resume asynchronous communications
@ -760,6 +771,11 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure
return true;
}
uint8_t AP_ExternalAHRS_VectorNav::num_gps_sensors(void) const
{
return type == TYPE::VN_INS ? 1 + has_dual_gnss : 0;
}
/*
get filter status. We don't know the meaning of the status bits yet,
so assume all OK if we have GPS lock

5
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
View File

@ -49,9 +49,8 @@ public:
protected:
uint8_t num_gps_sensors(void) const override {
return 1;
}
uint8_t num_gps_sensors(void) const override;
private:
AP_HAL::UARTDriver *uart;
int8_t port_num;