Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added support for writing raw IMU data to a UART 

this is for supporting external visual odomotry systems which need the
IMU data to correctly process image data

# Conflicts:
#	libraries/AP_InertialSensor/AP_InertialSensor.cpp
This commit is contained in:
Andrew Tridgell 2023-11-28 14:20:19 +11:00
parent e04d1bba9e
commit aba5ec6854
2 changed files with 61 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1882,6 +1882,11 @@ void AP_InertialSensor::update(void)
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL finished all IMUs");
}
#endif
#if AP_SERIALMANAGER_IMUOUT_ENABLED
if (uart.imu_out_uart) {
send_uart_data();
}
#endif
}
/*
@ -2614,6 +2619,50 @@ void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it)
}
#endif // AP_INERTIALSENSOR_KILL_IMU_ENABLED
#if AP_SERIALMANAGER_IMUOUT_ENABLED
/*
setup a UART for sending external data
*/
void AP_InertialSensor::set_imu_out_uart(AP_HAL::UARTDriver *_uart)
{
uart.imu_out_uart = _uart;
uart.counter = 0;
}
/*
send IMU delta-angle and delta-velocity to a UART
*/
void AP_InertialSensor::send_uart_data(void)
{
struct {
uint16_t magic = 0x29c4;
uint16_t length;
uint32_t timestamp_us;
Vector3f delta_velocity;
Vector3f delta_angle;
float delta_velocity_dt;
float delta_angle_dt;
uint16_t counter;
uint16_t crc;
} data;
if (uart.imu_out_uart->txspace() < sizeof(data)) {
// not enough space
return;
}
data.length = sizeof(data);
data.timestamp_us = AP_HAL::micros();
get_delta_angle(get_primary_gyro(), data.delta_angle, data.delta_angle_dt);
get_delta_velocity(get_primary_accel(), data.delta_velocity, data.delta_velocity_dt);
data.counter = uart.counter++;
data.crc = crc_xmodem((const uint8_t *)&data, sizeof(data)-sizeof(uint16_t));
uart.imu_out_uart->write((const uint8_t *)&data, sizeof(data));
}
#endif // AP_SERIALMANAGER_IMUOUT_ENABLED
#if HAL_EXTERNAL_AHRS_ENABLED
void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)

12
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -19,6 +19,7 @@
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include <Filter/LowPassFilter.h>
#include <Filter/HarmonicNotchFilter.h>
#include <AP_SerialManager/AP_SerialManager_config.h>
#include "AP_InertialSensor_Params.h"
#include "AP_InertialSensor_tempcal.h"
@ -302,6 +303,17 @@ public:
// for killing an IMU for testing purposes
void kill_imu(uint8_t imu_idx, bool kill_it);
#if AP_SERIALMANAGER_IMUOUT_ENABLED
// optional UART for sending IMU data to an external process
void set_imu_out_uart(AP_HAL::UARTDriver *uart);
void send_uart_data(void);
struct {
uint16_t counter;
AP_HAL::UARTDriver *imu_out_uart;
} uart;
#endif // AP_SERIALMANAGER_IMUOUT_ENABLED
enum IMU_SENSOR_TYPE {
IMU_SENSOR_TYPE_ACCEL = 0,
IMU_SENSOR_TYPE_GYRO = 1,