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

AP_InertialSensor: added ExternalAHRS backend

Browse Source
This commit is contained in:
Andrew Tridgell 2020-12-29 13:29:40 +11:00 committed by Peter Barker
parent 4f176c25de
commit b74e5c0e2f
6 changed files with 116 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -9,6 +9,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_BMI160.h"
@ -24,6 +25,7 @@
#include "AP_InertialSensor_BMI088.h"
#include "AP_InertialSensor_Invensensev2.h"
#include "AP_InertialSensor_ADIS1647x.h"
#include "AP_InertialSensor_ExternalAHRS.h"
/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
* Output is on the debug console. */
@ -850,6 +852,15 @@ AP_InertialSensor::detect_backends(void)
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this));
return;
}
#if HAL_EXTERNAL_AHRS_ENABLED
// if enabled, make the first IMU the external AHRS
if (int8_t serial_port = AP::externalAHRS().get_port() >= 0) {
ADD_BACKEND(new AP_InertialSensor_ExternalAHRS(*this, serial_port));
}
#endif
#if defined(HAL_INS_PROBE_LIST)
// IMUs defined by IMU lines in hwdef.dat
HAL_INS_PROBE_LIST;
@ -2214,6 +2225,15 @@ void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it)
#endif // HAL_MINIMIZE_FEATURES
#if HAL_EXTERNAL_AHRS_ENABLED
void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
{
for (uint8_t i = 0; i < _backend_count; i++) {
_backends[i]->handle_external(pkt);
}
}
#endif // HAL_EXTERNAL_AHRS_ENABLED
namespace AP {
AP_InertialSensor &ins()

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -37,6 +37,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Math/AP_Math.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include <Filter/LowPassFilter2p.h>
#include <Filter/LowPassFilter.h>
#include <Filter/NotchFilter.h>
@ -418,6 +419,11 @@ public:
};
BatchSampler batchsampler{*this};
#if HAL_EXTERNAL_AHRS_ENABLED
// handle external AHRS data
void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt);
#endif
private:
// load backend drivers
bool _add_backend(AP_InertialSensor_Backend *backend);

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -170,8 +170,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f;
_imu._gyro_last_sample_us[instance] = sample_us;
} else {
// don't accept below 100Hz
if (_imu._gyro_raw_sample_rates[instance] < 100) {
// don't accept below 40Hz
if (_imu._gyro_raw_sample_rates[instance] < 40) {
return;
}
@ -358,8 +358,8 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f;
_imu._accel_last_sample_us[instance] = sample_us;
} else {
// don't accept below 100Hz
if (_imu._accel_raw_sample_rates[instance] < 100) {
// don't accept below 40Hz
if (_imu._accel_raw_sample_rates[instance] < 40) {
return;
}

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -25,6 +25,7 @@
#include <inttypes.h>
#include <AP_Math/AP_Math.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include "AP_InertialSensor.h"
@ -79,6 +80,10 @@ public:
// get a startup banner to output to the GCS
virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }
#if HAL_EXTERNAL_AHRS_ENABLED
virtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {}
#endif
/*
device driver IDs. These are used to fill in the devtype field
of the device ID, which shows up as INS*ID* parameters to
@ -112,6 +117,7 @@ public:
DEVTYPE_INS_ICM20602 = 0x2F,
DEVTYPE_INS_ICM20601 = 0x30,
DEVTYPE_INS_ADIS1647X = 0x31,
DEVTYPE_SERIAL = 0x32,
};
protected:

52
libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp Normal file
View File

@ -0,0 +1,52 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_ExternalAHRS.h"
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#if HAL_EXTERNAL_AHRS_ENABLED
const extern AP_HAL::HAL& hal;
AP_InertialSensor_ExternalAHRS::AP_InertialSensor_ExternalAHRS(AP_InertialSensor &imu, uint8_t _serial_port) :
AP_InertialSensor_Backend(imu),
serial_port(_serial_port)
{
}
void AP_InertialSensor_ExternalAHRS::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
{
Vector3f accel = pkt.accel;
Vector3f gyro = pkt.gyro;
_rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel, AP_HAL::micros64());
_publish_temperature(accel_instance, pkt.temperature);
_notify_new_gyro_sensor_rate_sample(gyro_instance, gyro);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro, AP_HAL::micros64());
}
bool AP_InertialSensor_ExternalAHRS::update(void)
{
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
}
void AP_InertialSensor_ExternalAHRS::start()
{
const float rate = AP::externalAHRS().get_IMU_rate();
gyro_instance = _imu.register_gyro(rate,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 1, DEVTYPE_SERIAL));
accel_instance = _imu.register_accel(rate,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 2, DEVTYPE_SERIAL));
}
void AP_InertialSensor_ExternalAHRS::accumulate()
{
AP::externalAHRS().update();
}
#endif // HAL_EXTERNAL_AHRS_ENABLED

28
libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#if HAL_EXTERNAL_AHRS_ENABLED
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
class AP_InertialSensor_ExternalAHRS : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_ExternalAHRS(AP_InertialSensor &imu, uint8_t serial_port);
/* update accel and gyro state */
bool update() override;
void start() override;
void accumulate() override;
void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) override;
private:
uint8_t gyro_instance;
uint8_t accel_instance;
const uint8_t serial_port;
};
#endif // HAL_EXTERNAL_AHRS_ENABLED