Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ExternalAHRS: Correct VN100 pre-arm handling

Browse Source
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"); hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin");
return false; 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 // 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,75,3,%u,01,0721", unsigned(800 / get_rate()));
run_command("VNWRG,76,3,16,11,0001,0106"); 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 { } else {
// Default to setup for sensors other than VN-100 or VN-110 // 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
@ -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,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,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)); 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 // Resume asynchronous communications
@ -760,6 +771,11 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure
return true; 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, get filter status. We don't know the meaning of the status bits yet,
so assume all OK if we have GPS lock 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: protected:
uint8_t num_gps_sensors(void) const override { uint8_t num_gps_sensors(void) const override;
return 1;
}
private: private:
AP_HAL::UARTDriver *uart; AP_HAL::UARTDriver *uart;
int8_t port_num; int8_t port_num;