diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index d0beac4a9a..683918f8b7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -23,7 +23,7 @@ #include "AP_ExternalAHRS.h" #include "AP_ExternalAHRS_backend.h" #include "AP_ExternalAHRS_VectorNav.h" -#include "AP_ExternalAHRS_LORD.h" +#include "AP_ExternalAHRS_MicroStrain.h" #include @@ -53,7 +53,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Param: _TYPE // @DisplayName: AHRS type // @Description: Type of AHRS device - // @Values: 0:None,1:VectorNav,2:LORD + // @Values: 0:None,1:VectorNav,2:MicroStrain // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE), @@ -98,9 +98,9 @@ void AP_ExternalAHRS::init(void) backend = new AP_ExternalAHRS_VectorNav(this, state); break; #endif -#if AP_EXTERNAL_AHRS_LORD_ENABLED - case DevType::LORD: - backend = new AP_ExternalAHRS_LORD(this, state); +#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED + case DevType::MicroStrain: + backend = new AP_ExternalAHRS_MicroStrain(this, state); break; default: #endif diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 5e69f54bb9..992a49373d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -46,8 +46,8 @@ public: #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED VecNav = 1, #endif -#if AP_EXTERNAL_AHRS_LORD_ENABLED - LORD = 2, +#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED + MicroStrain = 2, #endif }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_LORD.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.cpp similarity index 88% rename from libraries/AP_ExternalAHRS/AP_ExternalAHRS_LORD.cpp rename to libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.cpp index d43f74eb25..14371f3a6f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_LORD.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.cpp @@ -11,16 +11,16 @@ along with this program. If not, see . */ /* - suppport for LORD Microstrain CX5/GX5-45 serially connected AHRS Systems + suppport for MicroStrain CX5/GX5-45 serially connected AHRS Systems */ #define ALLOW_DOUBLE_MATH_FUNCTIONS #include "AP_ExternalAHRS_config.h" -#if AP_EXTERNAL_AHRS_LORD_ENABLED +#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED -#include "AP_ExternalAHRS_LORD.h" +#include "AP_ExternalAHRS_MicroStrain.h" #include #include #include @@ -74,7 +74,7 @@ enum class FilterPacketField { extern const AP_HAL::HAL &hal; -AP_ExternalAHRS_LORD::AP_ExternalAHRS_LORD(AP_ExternalAHRS *_frontend, +AP_ExternalAHRS_MicroStrain::AP_ExternalAHRS_MicroStrain(AP_ExternalAHRS *_frontend, AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state) { auto &sm = AP::serialmanager(); @@ -88,15 +88,15 @@ AP_ExternalAHRS_LORD::AP_ExternalAHRS_LORD(AP_ExternalAHRS *_frontend, return; } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_LORD::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { AP_BoardConfig::allocation_error("Failed to allocate ExternalAHRS update thread"); } hal.scheduler->delay(5000); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LORD ExternalAHRS initialised"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain ExternalAHRS initialised"); } -void AP_ExternalAHRS_LORD::update_thread(void) +void AP_ExternalAHRS_MicroStrain::update_thread(void) { if (!port_open) { port_open = true; @@ -110,7 +110,7 @@ void AP_ExternalAHRS_LORD::update_thread(void) } // Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet. -void AP_ExternalAHRS_LORD::build_packet() +void AP_ExternalAHRS_MicroStrain::build_packet() { WITH_SEMAPHORE(sem); uint32_t nbytes = MIN(uart->available(), 2048u); @@ -167,7 +167,7 @@ void AP_ExternalAHRS_LORD::build_packet() } // returns true if the fletcher checksum for the packet is valid, else false. -bool AP_ExternalAHRS_LORD::valid_packet(const LORD_Packet & packet) const +bool AP_ExternalAHRS_MicroStrain::valid_packet(const MicroStrain_Packet & packet) const { uint8_t checksum_one = 0; uint8_t checksum_two = 0; @@ -186,7 +186,7 @@ bool AP_ExternalAHRS_LORD::valid_packet(const LORD_Packet & packet) const } // Calls the correct functions based on the packet descriptor of the packet -void AP_ExternalAHRS_LORD::handle_packet(const LORD_Packet& packet) +void AP_ExternalAHRS_MicroStrain::handle_packet(const MicroStrain_Packet& packet) { switch ((DescriptorSet) packet.header[2]) { case DescriptorSet::IMUData: @@ -208,7 +208,7 @@ void AP_ExternalAHRS_LORD::handle_packet(const LORD_Packet& packet) } // Collects data from an imu packet into `imu_data` -void AP_ExternalAHRS_LORD::handle_imu(const LORD_Packet& packet) +void AP_ExternalAHRS_MicroStrain::handle_imu(const MicroStrain_Packet& packet) { last_ins_pkt = AP_HAL::millis(); @@ -245,7 +245,7 @@ void AP_ExternalAHRS_LORD::handle_imu(const LORD_Packet& packet) } // Posts data from an imu packet to `state` and `handle_external` methods -void AP_ExternalAHRS_LORD::post_imu() const +void AP_ExternalAHRS_MicroStrain::post_imu() const { { WITH_SEMAPHORE(state.sem); @@ -279,7 +279,7 @@ void AP_ExternalAHRS_LORD::post_imu() const const AP_ExternalAHRS::baro_data_message_t baro { instance: 0, pressure_pa: imu_data.pressure, - // setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by lord + // setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by MicroStrain temperature: 25, }; AP::baro().handle_external(baro); @@ -288,7 +288,7 @@ void AP_ExternalAHRS_LORD::post_imu() const } // Collects data from a gnss packet into `gnss_data` -void AP_ExternalAHRS_LORD::handle_gnss(const LORD_Packet &packet) +void AP_ExternalAHRS_MicroStrain::handle_gnss(const MicroStrain_Packet &packet) { last_gps_pkt = AP_HAL::millis(); @@ -354,7 +354,7 @@ void AP_ExternalAHRS_LORD::handle_gnss(const LORD_Packet &packet) } } -void AP_ExternalAHRS_LORD::handle_filter(const LORD_Packet &packet) +void AP_ExternalAHRS_MicroStrain::handle_filter(const MicroStrain_Packet &packet) { last_filter_pkt = AP_HAL::millis(); @@ -392,7 +392,7 @@ void AP_ExternalAHRS_LORD::handle_filter(const LORD_Packet &packet) } } -void AP_ExternalAHRS_LORD::post_filter() const +void AP_ExternalAHRS_MicroStrain::post_filter() const { { WITH_SEMAPHORE(state.sem); @@ -437,7 +437,7 @@ void AP_ExternalAHRS_LORD::post_filter() const AP::gps().handle_external(gps); } -int8_t AP_ExternalAHRS_LORD::get_port(void) const +int8_t AP_ExternalAHRS_MicroStrain::get_port(void) const { if (!uart) { return -1; @@ -446,41 +446,41 @@ int8_t AP_ExternalAHRS_LORD::get_port(void) const }; // Get model/type name -const char* AP_ExternalAHRS_LORD::get_name() const +const char* AP_ExternalAHRS_MicroStrain::get_name() const { - return "LORD"; + return "MicroStrain"; } -bool AP_ExternalAHRS_LORD::healthy(void) const +bool AP_ExternalAHRS_MicroStrain::healthy(void) const { uint32_t now = AP_HAL::millis(); return (now - last_ins_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500); } -bool AP_ExternalAHRS_LORD::initialised(void) const +bool AP_ExternalAHRS_MicroStrain::initialised(void) const { return last_ins_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; } -bool AP_ExternalAHRS_LORD::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +bool AP_ExternalAHRS_MicroStrain::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { if (!healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, "LORD unhealthy"); + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain unhealthy"); return false; } if (gnss_data.fix_type < 3) { - hal.util->snprintf(failure_msg, failure_msg_len, "LORD no GPS lock"); + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain no GPS lock"); return false; } if (filter_status.state != 0x02) { - hal.util->snprintf(failure_msg, failure_msg_len, "LORD filter not running"); + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain filter not running"); return false; } return true; } -void AP_ExternalAHRS_LORD::get_filter_status(nav_filter_status &status) const +void AP_ExternalAHRS_MicroStrain::get_filter_status(nav_filter_status &status) const { memset(&status, 0, sizeof(status)); if (last_ins_pkt != 0 && last_gps_pkt != 0) { @@ -502,7 +502,7 @@ void AP_ExternalAHRS_LORD::get_filter_status(nav_filter_status &status) const } } -void AP_ExternalAHRS_LORD::send_status_report(GCS_MAVLINK &link) const +void AP_ExternalAHRS_MicroStrain::send_status_report(GCS_MAVLINK &link) const { // prepare flags uint16_t flags = 0; @@ -553,7 +553,7 @@ void AP_ExternalAHRS_LORD::send_status_report(GCS_MAVLINK &link) const } -Vector3f AP_ExternalAHRS_LORD::populate_vector3f(const uint8_t *data, uint8_t offset) const +Vector3f AP_ExternalAHRS_MicroStrain::populate_vector3f(const uint8_t *data, uint8_t offset) const { return Vector3f { be32tofloat_ptr(data, offset), @@ -562,7 +562,7 @@ Vector3f AP_ExternalAHRS_LORD::populate_vector3f(const uint8_t *data, uint8_t of }; } -Quaternion AP_ExternalAHRS_LORD::populate_quaternion(const uint8_t *data, uint8_t offset) const +Quaternion AP_ExternalAHRS_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offset) const { return Quaternion { be32tofloat_ptr(data, offset), @@ -572,4 +572,4 @@ Quaternion AP_ExternalAHRS_LORD::populate_quaternion(const uint8_t *data, uint8_ }; } -#endif // AP_EXTERNAL_AHRS_LORD_ENABLE +#endif // AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_LORD.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.h similarity index 84% rename from libraries/AP_ExternalAHRS/AP_ExternalAHRS_LORD.h rename to libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.h index 38fab6ebbb..39401afc7b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_LORD.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.h @@ -18,16 +18,16 @@ #include "AP_ExternalAHRS_config.h" -#if AP_EXTERNAL_AHRS_LORD_ENABLED +#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED #include "AP_ExternalAHRS_backend.h" #include -class AP_ExternalAHRS_LORD: public AP_ExternalAHRS_backend +class AP_ExternalAHRS_MicroStrain: public AP_ExternalAHRS_backend { public: - AP_ExternalAHRS_LORD(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + AP_ExternalAHRS_MicroStrain(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); // get serial port number, -1 for not enabled int8_t get_port(void) const override; @@ -74,15 +74,15 @@ private: uint32_t last_gps_pkt; uint32_t last_filter_pkt; - // A LORD packet can be a maximum of 261 bytes - struct LORD_Packet { + // A MicroStrain packet can be a maximum of 261 bytes + struct MicroStrain_Packet { uint8_t header[4]; uint8_t payload[255]; uint8_t checksum[2]; }; struct { - LORD_Packet packet; + MicroStrain_Packet packet; ParseState state; uint8_t index; } message_in; @@ -134,11 +134,11 @@ private: } filter_data; void build_packet(); - bool valid_packet(const LORD_Packet &packet) const; - void handle_packet(const LORD_Packet &packet); - void handle_imu(const LORD_Packet &packet); - void handle_gnss(const LORD_Packet &packet); - void handle_filter(const LORD_Packet &packet); + bool valid_packet(const MicroStrain_Packet &packet) const; + void handle_packet(const MicroStrain_Packet &packet); + void handle_imu(const MicroStrain_Packet &packet); + void handle_gnss(const MicroStrain_Packet &packet); + void handle_filter(const MicroStrain_Packet &packet); void post_imu() const; void post_gnss() const; void post_filter() const; @@ -148,4 +148,4 @@ private: }; -#endif // AP_EXTERNAL_AHRS_LORD_ENABLED +#endif // AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 0e167aad76..bd35d7b438 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -10,8 +10,8 @@ #define AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED HAL_EXTERNAL_AHRS_ENABLED #endif -#ifndef AP_EXTERNAL_AHRS_LORD_ENABLED -#define AP_EXTERNAL_AHRS_LORD_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED +#define AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif #ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED