diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 0033b83d01..c87ee06621 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #if HAL_EXTERNAL_AHRS_ENABLED @@ -34,6 +35,15 @@ extern const AP_HAL::HAL &hal; HAL_Semaphore AP_ExternalAHRS::sem; +/* + send requested config to the VN + */ +void AP_ExternalAHRS::send_config(void) const +{ + nmea_printf(uart, "$VNWRG,75,3,%u,35,0003,0F2C,0147,0613", unsigned(400/rate.get())); + nmea_printf(uart, "$VNWRG,76,3,80,4E,0002,0010,20B8,2018"); +} + /* header for pre-configured 50Hz data assumes the following config for VN-300: @@ -98,6 +108,10 @@ AP_ExternalAHRS::AP_ExternalAHRS() { AP_Param::setup_object_defaults(this, var_info); _singleton = this; + if (rate.get() < 50) { + // min 50Hz + rate.set(50); + } } #ifndef HAL_EXTERNAL_AHRS_DEFAULT @@ -115,6 +129,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE), + // @Param: _RATE + // @DisplayName: AHRS data rate + // @Description: Requested rate for AHRS device + // @Units: Hz + // @User: Standard + AP_GROUPINFO("_RATE", 2, AP_ExternalAHRS, rate, 50), + AP_GROUPEND }; @@ -224,6 +245,7 @@ void AP_ExternalAHRS::update_thread() // open port in the thread port_opened = true; uart->begin(baudrate, 1024, 512); + send_config(); } while (true) { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index fe4936e992..83d8cba5bf 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -51,7 +51,7 @@ public: // fixed IMU rate for now float get_IMU_rate(void) const { - return 50; + return rate.get(); } // get serial port number, -1 for not enabled @@ -122,8 +122,10 @@ private: void process_packet1(const uint8_t *b); void process_packet2(const uint8_t *b); + void send_config(void) const; AP_Enum devtype; + AP_Int16 rate; static AP_ExternalAHRS *_singleton;