Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ExternalAHRS: added EAHRS_RATE for IMU rate

Browse Source 
this allows setting of required data rate from the external AHRS
This commit is contained in:
Andrew Tridgell 2021-02-06 17:24:39 +11:00
parent 266b425115
commit ae088f792d
2 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -27,6 +27,7 @@
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Common/NMEA.h>
#include <stdio.h>
#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) {

4
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -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> devtype;
AP_Int16 rate;
static AP_ExternalAHRS *_singleton;