2016-01-19 21:26:31 -04:00
# pragma once
2011-11-12 23:20:25 -04:00
2023-01-03 01:09:46 -04:00
# include "AP_InertialSensor_config.h"
2012-11-05 00:27:03 -04:00
// Gyro and Accelerometer calibration criteria
2013-01-10 14:42:24 -04:00
# define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f
# define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f
2015-06-11 04:15:57 -03:00
# define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ 5.0f // accel vibration floor filter hz
# define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz
2015-12-29 13:18:07 -04:00
# define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout
2012-11-05 00:27:03 -04:00
2022-02-28 21:19:10 -04:00
# include <AP_HAL/AP_HAL_Boards.h>
2020-10-04 00:18:30 -03:00
2012-10-11 21:27:19 -03:00
# include <stdint.h>
2016-01-19 21:26:31 -04:00
# include <AP_AccelCal/AP_AccelCal.h>
2020-03-27 19:08:30 -03:00
# include <AP_HAL/utility/RingBuffer.h>
2015-08-11 03:28:43 -03:00
# include <AP_Math/AP_Math.h>
2020-12-28 22:29:40 -04:00
# include <AP_ExternalAHRS/AP_ExternalAHRS.h>
2016-01-19 21:26:31 -04:00
# include <Filter/LowPassFilter.h>
2019-06-17 05:44:12 -03:00
# include <Filter/HarmonicNotchFilter.h>
2023-11-27 23:20:19 -04:00
# include <AP_SerialManager/AP_SerialManager_config.h>
2023-02-17 00:26:27 -04:00
# include "AP_InertialSensor_Params.h"
# include "AP_InertialSensor_tempcal.h"
2014-10-14 01:48:33 -03:00
2021-10-29 22:15:49 -03:00
# ifndef AP_SIM_INS_ENABLED
2021-10-29 22:15:49 -03:00
# define AP_SIM_INS_ENABLED AP_SIM_ENABLED
2021-10-29 22:15:49 -03:00
# endif
2022-10-13 09:43:34 -03:00
# ifndef AP_SIM_INS_FILE_ENABLED
# define AP_SIM_INS_FILE_ENABLED AP_SIM_ENABLED
# endif
2014-10-14 01:48:33 -03:00
class AP_InertialSensor_Backend ;
2015-08-05 13:36:14 -03:00
class AuxiliaryBus ;
2017-06-05 00:03:28 -03:00
class AP_AHRS ;
2024-02-11 01:19:56 -04:00
class FastRateBuffer ;
2014-10-14 01:48:33 -03:00
2015-05-06 23:08:30 -03:00
/*
2019-01-18 00:23:42 -04:00
forward declare AP_Logger class . We can ' t include logger . h
2015-05-06 23:08:30 -03:00
because of mutual dependencies
*/
2019-01-18 00:23:42 -04:00
class AP_Logger ;
2015-05-06 23:08:30 -03:00
2011-11-12 23:20:25 -04:00
/* AP_InertialSensor is an abstraction for gyro and accel measurements
* which are correctly aligned to the body axes and scaled to SI units .
2012-12-04 20:20:31 -04:00
*
* Gauss - Newton accel calibration routines borrowed from Rolfe Schmidt
* blog post describing the method : http : //chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
* original sketch available at http : //rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
2011-11-12 23:20:25 -04:00
*/
2015-07-20 17:25:40 -03:00
class AP_InertialSensor : AP_AccelCal_Client
2011-11-12 23:20:25 -04:00
{
2014-10-14 01:48:33 -03:00
friend class AP_InertialSensor_Backend ;
2024-02-11 01:19:56 -04:00
friend class FastRateBuffer ;
2014-10-14 01:48:33 -03:00
2012-08-17 03:19:56 -03:00
public :
2017-12-12 21:06:12 -04:00
AP_InertialSensor ( ) ;
2017-08-08 12:17:57 -03:00
/* Do not allow copies */
2022-09-30 06:50:43 -03:00
CLASS_NO_COPY ( AP_InertialSensor ) ;
2015-07-20 17:25:40 -03:00
2019-02-10 14:29:24 -04:00
static AP_InertialSensor * get_singleton ( ) ;
2012-11-05 00:27:03 -04:00
2015-09-17 03:42:41 -03:00
enum Gyro_Calibration_Timing {
GYRO_CAL_NEVER = 0 ,
2015-10-15 03:06:34 -03:00
GYRO_CAL_STARTUP_ONLY = 1
2015-09-17 03:42:41 -03:00
} ;
2012-11-05 00:27:03 -04:00
/// Perform startup initialisation.
///
/// Called to initialise the state of the IMU.
///
2015-09-17 03:42:41 -03:00
/// Gyros will be calibrated unless INS_GYRO_CAL is zero
2012-11-05 00:27:03 -04:00
///
/// @param style The initialisation startup style.
///
2015-12-25 23:11:33 -04:00
void init ( uint16_t sample_rate_hz ) ;
2012-11-05 00:27:03 -04:00
2023-08-27 22:33:40 -03:00
// get accel/gyro instance numbers that a backend will get when they register
bool get_accel_instance ( uint8_t & instance ) const ;
bool get_gyro_instance ( uint8_t & instance ) const ;
2014-10-14 01:48:33 -03:00
/// Register a new gyro/accel driver, allocating an instance
/// number
2021-03-17 22:36:41 -03:00
bool register_gyro ( uint8_t & instance , uint16_t raw_sample_rate_hz , uint32_t id ) ;
bool register_accel ( uint8_t & instance , uint16_t raw_sample_rate_hz , uint32_t id ) ;
2012-11-05 00:27:03 -04:00
2017-10-03 20:44:07 -03:00
// a function called by the main thread at the main loop rate:
void periodic ( ) ;
2015-03-09 03:31:55 -03:00
/// calibrating - returns true if the gyros or accels are currently being calibrated
2021-01-07 20:46:35 -04:00
bool calibrating ( ) const ;
2015-03-09 03:31:55 -03:00
2021-01-10 16:09:44 -04:00
/// calibrating - returns true if a temperature calibration is running
bool temperature_cal_running ( ) const ;
2012-11-05 00:27:03 -04:00
/// Perform cold-start initialisation for just the gyros.
///
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work
///
2014-10-14 01:48:33 -03:00
void init_gyro ( void ) ;
2012-11-05 00:27:03 -04:00
2020-05-21 15:29:28 -03:00
// get startup messages to output to the GCS
bool get_output_banner ( uint8_t instance_id , char * banner , uint8_t banner_len ) ;
2012-11-05 00:27:03 -04:00
/// Fetch the current gyro values
///
/// @returns vector of rotational rates in radians/sec
///
2013-12-08 18:50:12 -04:00
const Vector3f & get_gyro ( uint8_t i ) const { return _gyro [ i ] ; }
2024-06-25 06:05:26 -03:00
const Vector3f & get_gyro ( void ) const { return get_gyro ( _first_usable_gyro ) ; }
2012-11-05 00:27:03 -04:00
// set gyro offsets in radians/sec
2023-02-17 00:26:27 -04:00
const Vector3f & get_gyro_offsets ( uint8_t i ) const { return _gyro_offset ( i ) ; }
2024-06-25 06:05:26 -03:00
const Vector3f & get_gyro_offsets ( void ) const { return get_gyro_offsets ( _first_usable_gyro ) ; }
2012-11-05 00:27:03 -04:00
2015-02-17 02:54:17 -04:00
//get delta angle if available
2021-02-11 17:34:37 -04:00
bool get_delta_angle ( uint8_t i , Vector3f & delta_angle , float & delta_angle_dt ) const ;
bool get_delta_angle ( Vector3f & delta_angle , float & delta_angle_dt ) const {
2024-06-25 06:05:26 -03:00
return get_delta_angle ( _first_usable_gyro , delta_angle , delta_angle_dt ) ;
2021-02-11 17:34:37 -04:00
}
2016-01-19 21:26:31 -04:00
2015-02-17 02:54:17 -04:00
//get delta velocity if available
2021-02-11 17:34:37 -04:00
bool get_delta_velocity ( uint8_t i , Vector3f & delta_velocity , float & delta_velocity_dt ) const ;
bool get_delta_velocity ( Vector3f & delta_velocity , float & delta_velocity_dt ) const {
2024-06-25 06:05:26 -03:00
return get_delta_velocity ( _first_usable_accel , delta_velocity , delta_velocity_dt ) ;
2021-02-11 17:34:37 -04:00
}
2015-03-21 17:18:58 -03:00
2012-11-05 00:27:03 -04:00
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
///
2013-12-08 18:50:12 -04:00
const Vector3f & get_accel ( uint8_t i ) const { return _accel [ i ] ; }
2024-06-25 06:05:26 -03:00
const Vector3f & get_accel ( void ) const { return get_accel ( _first_usable_accel ) ; }
2014-12-29 06:19:35 -04:00
2013-12-08 05:43:53 -04:00
// multi-device interface
2015-04-17 00:34:33 -03:00
bool get_gyro_health ( uint8_t instance ) const { return ( instance < _gyro_count ) ? _gyro_healthy [ instance ] : false ; }
2024-06-25 06:05:26 -03:00
bool get_gyro_health ( void ) const { return get_gyro_health ( _first_usable_gyro ) ; }
2014-09-01 08:20:27 -03:00
bool get_gyro_health_all ( void ) const ;
2023-11-08 22:45:13 -04:00
bool gyros_consistent ( uint8_t threshold ) const ;
2022-08-09 14:58:53 -03:00
uint8_t get_gyro_count ( void ) const { return MIN ( INS_MAX_INSTANCES , _gyro_count ) ; }
2014-10-08 08:17:31 -03:00
bool gyro_calibrated_ok ( uint8_t instance ) const { return _gyro_cal_ok [ instance ] ; }
bool gyro_calibrated_ok_all ( ) const ;
2015-08-01 03:40:40 -03:00
bool use_gyro ( uint8_t instance ) const ;
2019-04-11 06:51:25 -03:00
Gyro_Calibration_Timing gyro_calibration_timing ( ) ;
2013-12-08 05:43:53 -04:00
2015-04-17 00:34:33 -03:00
bool get_accel_health ( uint8_t instance ) const { return ( instance < _accel_count ) ? _accel_healthy [ instance ] : false ; }
2024-06-25 06:05:26 -03:00
bool get_accel_health ( void ) const { return get_accel_health ( _first_usable_accel ) ; }
2014-09-01 08:20:27 -03:00
bool get_accel_health_all ( void ) const ;
2023-11-08 22:45:13 -04:00
bool accels_consistent ( float accel_error_threshold ) const ;
2022-01-13 11:49:05 -04:00
uint8_t get_accel_count ( void ) const { return MIN ( INS_MAX_INSTANCES , _accel_count ) ; }
2015-05-12 03:18:25 -03:00
bool accel_calibrated_ok_all ( ) const ;
2015-08-01 03:40:40 -03:00
bool use_accel ( uint8_t instance ) const ;
2013-12-08 05:43:53 -04:00
2017-05-01 00:01:43 -03:00
// get observed sensor rates, including any internal sampling multiplier
uint16_t get_gyro_rate_hz ( uint8_t instance ) const { return uint16_t ( _gyro_raw_sample_rates [ instance ] * _gyro_over_sampling [ instance ] ) ; }
uint16_t get_accel_rate_hz ( uint8_t instance ) const { return uint16_t ( _accel_raw_sample_rates [ instance ] * _accel_over_sampling [ instance ] ) ; }
2017-08-08 12:17:57 -03:00
2019-07-23 05:43:18 -03:00
// FFT support access
2022-11-28 05:33:50 -04:00
# if HAL_GYROFFT_ENABLED
2024-06-25 06:05:26 -03:00
const Vector3f & get_gyro_for_fft ( void ) const { return _gyro_for_fft [ _first_usable_gyro ] ; }
2020-03-27 19:08:30 -03:00
FloatBuffer & get_raw_gyro_window ( uint8_t instance , uint8_t axis ) { return _gyro_window [ instance ] [ axis ] ; }
2024-06-25 06:05:26 -03:00
FloatBuffer & get_raw_gyro_window ( uint8_t axis ) { return get_raw_gyro_window ( _first_usable_gyro , axis ) ; }
2024-02-12 20:26:07 -04:00
# if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
2022-09-22 11:35:48 -03:00
bool has_fft_notch ( ) const ;
2024-02-12 20:26:07 -04:00
# endif
2020-01-03 15:52:33 -04:00
# endif
2024-02-11 01:19:56 -04:00
uint16_t get_raw_gyro_rate_hz ( uint8_t instance ) const { return _gyro_raw_sample_rates [ _first_usable_gyro ] ; }
uint16_t get_raw_gyro_rate_hz ( ) const { return get_raw_gyro_rate_hz ( _first_usable_gyro ) ; }
2019-07-23 05:43:18 -03:00
bool set_gyro_window_size ( uint16_t size ) ;
2012-11-05 00:27:03 -04:00
// get accel offsets in m/s/s
2023-02-17 00:26:27 -04:00
const Vector3f & get_accel_offsets ( uint8_t i ) const { return _accel_offset ( i ) ; }
2024-06-25 06:05:26 -03:00
const Vector3f & get_accel_offsets ( void ) const { return get_accel_offsets ( _first_usable_accel ) ; }
2012-11-05 00:27:03 -04:00
// get accel scale
2023-02-17 00:26:27 -04:00
const Vector3f & get_accel_scale ( uint8_t i ) const { return _accel_scale ( i ) ; }
2024-06-25 06:05:26 -03:00
const Vector3f & get_accel_scale ( void ) const { return get_accel_scale ( _first_usable_accel ) ; }
2012-08-17 03:19:56 -03:00
2016-10-07 19:11:10 -03:00
// return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
2016-10-27 01:06:11 -03:00
const Vector3f & get_imu_pos_offset ( uint8_t instance ) const {
2023-02-17 00:26:27 -04:00
return _accel_pos ( instance ) ;
2016-10-07 19:11:10 -03:00
}
2016-10-27 01:06:11 -03:00
const Vector3f & get_imu_pos_offset ( void ) const {
2024-06-25 06:05:26 -03:00
return _accel_pos ( _first_usable_accel ) ;
2016-10-07 19:11:10 -03:00
}
2015-03-16 23:32:54 -03:00
// return the temperature if supported. Zero is returned if no
// temperature is available
float get_temperature ( uint8_t instance ) const { return _temperature [ instance ] ; }
2012-11-05 00:27:03 -04:00
/* get_delta_time returns the time period in seconds
2012-08-30 04:48:36 -03:00
* overwhich the sensor data was collected
2012-08-17 03:19:56 -03:00
*/
2017-12-04 01:33:36 -04:00
float get_delta_time ( ) const { return MIN ( _delta_time , _loop_delta_t_max ) ; }
2012-08-17 03:19:56 -03:00
// return the maximum gyro drift rate in radians/s/s. This
// depends on what gyro chips are being used
2014-10-14 01:48:33 -03:00
float get_gyro_drift_rate ( void ) const { return ToRad ( 0.5f / 60 ) ; }
2012-03-08 03:10:27 -04:00
2014-10-14 01:48:33 -03:00
// update gyro and accel values from accumulated samples
2022-04-01 16:59:10 -03:00
void update ( void ) __RAMFUNC__ ;
2014-10-14 01:48:33 -03:00
// wait for a sample to be available
2022-04-01 16:59:10 -03:00
void wait_for_sample ( void ) __RAMFUNC__ ;
2013-10-08 03:28:39 -03:00
2012-11-05 00:27:03 -04:00
// class level parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
2023-02-17 00:26:27 -04:00
# if INS_AUX_INSTANCES
AP_InertialSensor_Params params [ INS_AUX_INSTANCES ] ;
# endif
2012-11-05 00:27:03 -04:00
2013-01-13 01:03:13 -04:00
// set overall board orientation
2021-11-05 13:11:46 -03:00
void set_board_orientation ( enum Rotation orientation ) {
2013-01-13 01:03:13 -04:00
_board_orientation = orientation ;
}
2020-05-21 15:29:28 -03:00
// return the selected loop rate at which samples are made avilable
uint16_t get_loop_rate_hz ( void ) const { return _loop_rate ; }
2014-10-16 17:52:21 -03:00
2015-12-25 22:34:34 -04:00
// return the main loop delta_t in seconds
float get_loop_delta_t ( void ) const { return _loop_delta_t ; }
2016-01-19 21:26:31 -04:00
2014-10-14 01:48:33 -03:00
bool healthy ( void ) const { return get_gyro_health ( ) & & get_accel_health ( ) ; }
2012-11-05 00:27:03 -04:00
2024-06-25 06:05:26 -03:00
uint8_t get_first_usable_accel ( void ) const { return _first_usable_accel ; }
uint8_t get_first_usable_gyro ( void ) const { return _first_usable_gyro ; }
2013-12-09 05:02:04 -04:00
2015-03-11 22:19:31 -03:00
// get the gyro filter rate in Hz
2019-05-25 07:59:57 -03:00
uint16_t get_gyro_filter_hz ( void ) const { return _gyro_filter_cutoff ; }
2015-03-11 22:19:31 -03:00
// get the accel filter rate in Hz
2019-05-25 07:59:57 -03:00
uint16_t get_accel_filter_hz ( void ) const { return _accel_filter_cutoff ; }
2022-03-06 11:00:30 -04:00
2024-02-12 20:26:07 -04:00
# if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
2022-03-06 11:00:30 -04:00
// setup the notch for throttle based tracking
2023-01-18 18:58:44 -04:00
bool setup_throttle_gyro_harmonic_notch ( float center_freq_hz , float lower_freq_hz , float ref , uint8_t harmonics ) ;
2015-03-11 22:19:31 -03:00
2021-12-30 20:32:17 -04:00
// write out harmonic notch log messages
void write_notch_log_messages ( ) const ;
2024-02-12 20:26:07 -04:00
# endif
2021-12-30 20:32:17 -04:00
2017-06-27 01:42:45 -03:00
// indicate which bit in LOG_BITMASK indicates raw logging enabled
void set_log_raw_bit ( uint32_t log_raw_bit ) { _log_raw_bit = log_raw_bit ; }
2015-05-06 23:08:30 -03:00
2021-01-22 01:31:09 -04:00
// Logging Functions
void Write_IMU ( ) const ;
void Write_Vibration ( ) const ;
2015-06-11 04:15:57 -03:00
// calculate vibration levels and check for accelerometer clipping (called by a backends)
void calc_vibration_and_clipping ( uint8_t instance , const Vector3f & accel , float dt ) ;
// retrieve latest calculated vibration levels
2024-06-25 06:05:26 -03:00
Vector3f get_vibration_levels ( ) const { return get_vibration_levels ( _first_usable_accel ) ; }
2015-07-30 04:58:06 -03:00
Vector3f get_vibration_levels ( uint8_t instance ) const ;
2015-06-11 04:15:57 -03:00
// retrieve and clear accelerometer clipping count
uint32_t get_accel_clip_count ( uint8_t instance ) const ;
2015-07-29 16:34:48 -03:00
// check for vibration movement. True when all axis show nearly zero movement
bool is_still ( ) ;
2015-10-02 15:45:21 -03:00
AuxiliaryBus * get_auxiliary_bus ( int16_t backend_id ) { return get_auxiliary_bus ( backend_id , 0 ) ; }
AuxiliaryBus * get_auxiliary_bus ( int16_t backend_id , uint8_t instance ) ;
2015-08-05 13:36:14 -03:00
2015-10-13 15:33:20 -03:00
void detect_backends ( void ) ;
2024-02-11 01:19:56 -04:00
void update_backends ( ) ;
2015-10-13 15:33:20 -03:00
2015-12-29 13:18:07 -04:00
// accel peak hold detector
void set_accel_peak_hold ( uint8_t instance , const Vector3f & accel ) ;
2015-12-30 23:23:04 -04:00
float get_accel_peak_hold_neg_x ( ) const { return _peak_hold_state . accel_peak_hold_neg_x ; }
2015-12-29 13:18:07 -04:00
2015-07-20 17:25:40 -03:00
//Returns accel calibrator interface object pointer
AP_AccelCal * get_acal ( ) const { return _acal ; }
// Returns body fixed accelerometer level data averaged during accel calibration's first step
bool get_fixed_mount_accel_cal_sample ( uint8_t sample_num , Vector3f & ret ) const ;
// Returns primary accelerometer level data averaged during accel calibration's first step
2024-06-25 06:05:26 -03:00
bool get_first_usable_accel_cal_sample_avg ( uint8_t sample_num , Vector3f & ret ) const ;
2015-07-20 17:25:40 -03:00
// Returns newly calculated trim values if calculated
2021-08-12 19:57:43 -03:00
bool get_new_trim ( Vector3f & trim_rad ) ;
2015-07-20 17:25:40 -03:00
2022-03-18 04:02:30 -03:00
# if HAL_INS_ACCELCAL_ENABLED
2015-07-20 17:25:40 -03:00
// initialise and register accel calibrator
// called during the startup of accel cal
void acal_init ( ) ;
// update accel calibrator
void acal_update ( ) ;
2022-03-18 04:02:30 -03:00
# endif
2015-12-29 00:32:06 -04:00
2021-09-17 10:06:16 -03:00
# if HAL_GCS_ENABLED
2022-10-04 17:42:03 -03:00
bool calibrate_gyros ( ) ;
MAV_RESULT calibrate_trim ( ) ;
// simple accel calibration
2018-03-17 09:03:13 -03:00
MAV_RESULT simple_accel_cal ( ) ;
2022-10-04 17:42:03 -03:00
private :
uint32_t last_accel_cal_ms ;
public :
2021-09-17 10:06:16 -03:00
# endif
2017-11-27 01:54:55 -04:00
2015-12-29 00:32:06 -04:00
bool accel_cal_requires_reboot ( ) const { return _accel_cal_requires_reboot ; }
2017-04-28 21:26:58 -03:00
// return time in microseconds of last update() call
uint32_t get_last_update_usec ( void ) const { return _last_update_usec ; }
2017-08-08 12:17:57 -03:00
2019-04-18 01:24:01 -03:00
// for killing an IMU for testing purposes
void kill_imu ( uint8_t imu_idx , bool kill_it ) ;
2023-11-27 23:20:19 -04:00
# 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
2017-10-03 20:44:07 -03:00
enum IMU_SENSOR_TYPE {
IMU_SENSOR_TYPE_ACCEL = 0 ,
IMU_SENSOR_TYPE_GYRO = 1 ,
} ;
2023-01-03 01:51:51 -04:00
# if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
2017-10-03 20:44:07 -03:00
class BatchSampler {
public :
BatchSampler ( const AP_InertialSensor & imu ) :
type ( IMU_SENSOR_TYPE_ACCEL ) ,
_imu ( imu ) {
AP_Param : : setup_object_defaults ( this , var_info ) ;
} ;
void init ( ) ;
2021-09-14 17:28:20 -03:00
void sample ( uint8_t instance , IMU_SENSOR_TYPE _type , uint64_t sample_us , const Vector3f & sample ) __RAMFUNC__ ;
2017-10-03 20:44:07 -03:00
// a function called by the main thread at the main loop rate:
void periodic ( ) ;
2018-03-18 20:28:33 -03:00
bool doing_sensor_rate_logging ( ) const { return _doing_sensor_rate_logging ; }
2022-06-25 17:16:55 -03:00
bool doing_post_filter_logging ( ) const {
return ( _doing_post_filter_logging & & ( post_filter | | ! _doing_sensor_rate_logging ) )
| | ( _doing_pre_post_filter_logging & & post_filter ) ;
}
2018-03-18 20:28:33 -03:00
2023-03-02 18:20:11 -04:00
// Getters for arming check
bool is_initialised ( ) const { return initialised ; }
bool enabled ( ) const { return _sensor_mask > 0 ; }
2017-10-03 20:44:07 -03:00
// class level parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
2023-02-17 00:26:27 -04:00
2017-10-03 20:44:07 -03:00
// Parameters
AP_Int16 _required_count ;
2023-04-04 05:09:18 -03:00
uint16_t _real_required_count ;
2017-10-03 20:44:07 -03:00
AP_Int8 _sensor_mask ;
2018-03-18 20:28:33 -03:00
AP_Int8 _batch_options_mask ;
2019-01-18 00:23:42 -04:00
// Parameters controlling pushing data to AP_Logger:
2018-03-18 20:28:33 -03:00
// Each DF message is ~ 108 bytes in size, so we use about 1kB/s of
// logging bandwidth with a 100ms interval. If we are taking
// 1024 samples then we need to send 32 packets, so it will
// take ~3 seconds to push a complete batch to the log. If
// you are running a on an FMU with three IMUs then you
// will loop back around to the first sensor after about
// twenty seconds.
AP_Int16 samples_per_msg ;
AP_Int8 push_interval_ms ;
2017-10-03 20:44:07 -03:00
// end Parameters
private :
2018-03-18 20:28:33 -03:00
enum batch_opt_t {
BATCH_OPT_SENSOR_RATE = ( 1 < < 0 ) ,
2019-05-17 12:57:43 -03:00
BATCH_OPT_POST_FILTER = ( 1 < < 1 ) ,
2022-06-25 17:16:55 -03:00
BATCH_OPT_PRE_POST_FILTER = ( 1 < < 2 ) ,
2018-03-18 20:28:33 -03:00
} ;
2017-10-03 20:44:07 -03:00
void rotate_to_next_sensor ( ) ;
2018-03-18 20:28:33 -03:00
void update_doing_sensor_rate_logging ( ) ;
2017-10-03 20:44:07 -03:00
2021-09-14 17:28:20 -03:00
bool should_log ( uint8_t instance , IMU_SENSOR_TYPE type ) __RAMFUNC__ ;
2017-10-03 20:44:07 -03:00
void push_data_to_log ( ) ;
2021-01-22 01:31:09 -04:00
// Logging functions
bool Write_ISBH ( const float sample_rate_hz ) const ;
bool Write_ISBD ( ) const ;
2022-06-25 17:16:55 -03:00
bool has_option ( batch_opt_t option ) const { return _batch_options_mask & uint16_t ( option ) ; }
2017-10-03 20:44:07 -03:00
uint64_t measurement_started_us ;
2022-06-25 17:16:55 -03:00
bool initialised ;
bool isbh_sent ;
bool _doing_sensor_rate_logging ;
bool _doing_post_filter_logging ;
bool _doing_pre_post_filter_logging ;
uint8_t instance ; // instance we are sending data for
bool post_filter ; // whether we are sending post-filter data
AP_InertialSensor : : IMU_SENSOR_TYPE type ;
2017-10-03 20:44:07 -03:00
uint16_t isb_seqnum ;
int16_t * data_x ;
int16_t * data_y ;
int16_t * data_z ;
uint16_t data_write_offset ; // units: samples
uint16_t data_read_offset ; // units: samples
uint32_t last_sent_ms ;
// all samples are multiplied by this
2018-03-18 20:28:33 -03:00
uint16_t multiplier ; // initialised as part of init()
2017-10-03 20:44:07 -03:00
const AP_InertialSensor & _imu ;
} ;
BatchSampler batchsampler { * this } ;
2023-01-03 01:51:51 -04:00
# endif
2017-10-03 20:44:07 -03:00
2020-12-28 22:29:40 -04:00
# if HAL_EXTERNAL_AHRS_ENABLED
// handle external AHRS data
void handle_external ( const AP_ExternalAHRS : : ins_data_message_t & pkt ) ;
# endif
2021-01-16 00:59:35 -04:00
# if HAL_INS_TEMPERATURE_CAL_ENABLE
/*
get a string representation of parameters that should be made
persistent across changes of firmware type
*/
void get_persistent_params ( ExpandingString & str ) const ;
# endif
2021-02-10 20:49:08 -04:00
// force save of current calibration as valid
void force_save_calibration ( void ) ;
2024-02-12 20:26:07 -04:00
# if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
2022-04-15 04:38:56 -03:00
// structure per harmonic notch filter. This is public to allow for
// easy iteration
class HarmonicNotch {
public :
HarmonicNotchFilterParams params ;
HarmonicNotchFilterVector3f filter [ INS_MAX_INSTANCES ] ;
uint8_t num_dynamic_notches ;
// the current center frequency for the notch
float calculated_notch_freq_hz [ INS_MAX_NOTCHES ] ;
uint8_t num_calculated_notch_frequencies ;
// runtime update of notch parameters
void update_params ( uint8_t instance , bool converging , float gyro_rate ) ;
// Update the harmonic notch frequencies
void update_freq_hz ( float scaled_freq ) ;
void update_frequencies_hz ( uint8_t num_freqs , const float scaled_freq [ ] ) ;
2022-06-11 23:32:46 -03:00
// enable/disable the notch
void set_inactive ( bool _inactive ) {
inactive = _inactive ;
}
bool is_inactive ( void ) const {
return inactive ;
}
2022-04-15 04:38:56 -03:00
private :
// support for updating harmonic filter at runtime
2022-04-17 04:03:46 -03:00
float last_center_freq_hz [ INS_MAX_INSTANCES ] ;
float last_bandwidth_hz [ INS_MAX_INSTANCES ] ;
float last_attenuation_dB [ INS_MAX_INSTANCES ] ;
2022-06-11 23:32:46 -03:00
bool inactive ;
2022-04-15 04:38:56 -03:00
} harmonic_notches [ HAL_INS_NUM_HARMONIC_NOTCH_FILTERS ] ;
2024-02-12 20:26:07 -04:00
# endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
2022-04-15 04:38:56 -03:00
2014-10-14 01:48:33 -03:00
private :
// load backend drivers
2016-11-10 00:30:48 -04:00
bool _add_backend ( AP_InertialSensor_Backend * backend ) ;
2015-08-05 10:01:52 -03:00
void _start_backends ( ) ;
2015-10-02 15:45:21 -03:00
AP_InertialSensor_Backend * _find_backend ( int16_t backend_id , uint8_t instance ) ;
2012-10-11 21:27:19 -03:00
2015-03-10 20:16:04 -03:00
// gyro initialisation
2014-10-14 01:48:33 -03:00
void _init_gyro ( ) ;
2012-11-05 00:27:03 -04:00
2012-12-04 20:20:31 -04:00
// Calibration routines borrowed from Rolfe Schmidt
// blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
2021-08-12 19:57:43 -03:00
bool _calculate_trim ( const Vector3f & accel_sample , Vector3f & trim_rad ) ;
2012-11-05 00:27:03 -04:00
2016-09-03 21:18:09 -03:00
// save gyro calibration values to eeprom
void _save_gyro_calibration ( ) ;
2012-11-05 00:27:03 -04:00
2021-01-22 01:31:09 -04:00
// Logging function
void Write_IMU_instance ( const uint64_t time_us , const uint8_t imu_instance ) const ;
2014-10-14 01:48:33 -03:00
// backend objects
2014-10-15 20:32:40 -03:00
AP_InertialSensor_Backend * _backends [ INS_MAX_BACKENDS ] ;
2012-11-05 00:27:03 -04:00
2014-10-14 01:48:33 -03:00
// number of gyros and accel drivers. Note that most backends
// provide both accel and gyro data, so will increment both
// counters on initialisation
uint8_t _gyro_count ;
uint8_t _accel_count ;
2014-10-16 17:27:01 -03:00
uint8_t _backend_count ;
2013-11-06 22:53:59 -04:00
2020-05-21 15:29:28 -03:00
// the selected loop rate at which samples are made available
uint16_t _loop_rate ;
2015-12-25 22:34:34 -04:00
float _loop_delta_t ;
2017-12-04 01:33:36 -04:00
float _loop_delta_t_max ;
2016-01-19 21:26:31 -04:00
2014-10-14 01:48:33 -03:00
// Most recent accelerometer reading
Vector3f _accel [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
Vector3f _delta_velocity [ INS_MAX_INSTANCES ] ;
2015-03-21 17:18:58 -03:00
float _delta_velocity_dt [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
bool _delta_velocity_valid [ INS_MAX_INSTANCES ] ;
2015-09-08 11:42:28 -03:00
// delta velocity accumulator
Vector3f _delta_velocity_acc [ INS_MAX_INSTANCES ] ;
// time accumulator for delta velocity accumulator
float _delta_velocity_acc_dt [ INS_MAX_INSTANCES ] ;
2014-10-14 01:48:33 -03:00
2015-11-15 20:05:20 -04:00
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter [ INS_MAX_INSTANCES ] ;
LowPassFilter2pVector3f _gyro_filter [ INS_MAX_INSTANCES ] ;
Vector3f _accel_filtered [ INS_MAX_INSTANCES ] ;
Vector3f _gyro_filtered [ INS_MAX_INSTANCES ] ;
2022-11-28 05:33:50 -04:00
# if HAL_GYROFFT_ENABLED
2019-07-23 05:43:18 -03:00
// Thread-safe public version of _last_raw_gyro
2022-09-22 11:35:48 -03:00
Vector3f _gyro_for_fft [ INS_MAX_INSTANCES ] ;
Vector3f _last_gyro_for_fft [ INS_MAX_INSTANCES ] ;
2020-03-27 19:08:30 -03:00
FloatBuffer _gyro_window [ INS_MAX_INSTANCES ] [ XYZ_AXIS_COUNT ] ;
2019-07-23 05:43:18 -03:00
uint16_t _gyro_window_size ;
2022-09-22 11:35:48 -03:00
// capture a gyro window after the filters
LowPassFilter2pVector3f _post_filter_gyro_filter [ INS_MAX_INSTANCES ] ;
bool _post_filter_fft ;
uint8_t _fft_window_phase ;
2020-01-03 15:52:33 -04:00
# endif
2015-11-15 20:05:20 -04:00
bool _new_accel_data [ INS_MAX_INSTANCES ] ;
bool _new_gyro_data [ INS_MAX_INSTANCES ] ;
2016-01-19 21:26:31 -04:00
2014-10-14 01:48:33 -03:00
// Most recent gyro reading
2013-12-08 18:50:12 -04:00
Vector3f _gyro [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
Vector3f _delta_angle [ INS_MAX_INSTANCES ] ;
2016-01-16 00:41:19 -04:00
float _delta_angle_dt [ INS_MAX_INSTANCES ] ;
2015-02-17 02:54:17 -04:00
bool _delta_angle_valid [ INS_MAX_INSTANCES ] ;
2016-01-16 00:41:19 -04:00
// time accumulator for delta angle accumulator
float _delta_angle_acc_dt [ INS_MAX_INSTANCES ] ;
2015-09-10 09:34:01 -03:00
Vector3f _delta_angle_acc [ INS_MAX_INSTANCES ] ;
Vector3f _last_delta_angle [ INS_MAX_INSTANCES ] ;
Vector3f _last_raw_gyro [ INS_MAX_INSTANCES ] ;
2012-11-05 00:27:03 -04:00
2018-03-18 20:28:33 -03:00
// bitmask indicating if a sensor is doing sensor-rate sampling:
uint8_t _accel_sensor_rate_sampling_enabled ;
uint8_t _gyro_sensor_rate_sampling_enabled ;
// multipliers for data supplied via sensor-rate logging:
uint16_t _accel_raw_sampling_multiplier [ INS_MAX_INSTANCES ] ;
uint16_t _gyro_raw_sampling_multiplier [ INS_MAX_INSTANCES ] ;
2016-09-03 21:51:37 -03:00
// IDs to uniquely identify each sensor: shall remain
// the same across reboots
2023-02-17 00:26:27 -04:00
AP_Int32 _accel_id_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
AP_Int32 _gyro_id_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
2016-09-03 21:51:37 -03:00
2012-11-05 00:27:03 -04:00
// accelerometer scaling and offsets
2023-02-17 00:26:27 -04:00
AP_Vector3f _accel_scale_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
AP_Vector3f _accel_offset_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
AP_Vector3f _gyro_offset_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
2012-11-29 16:15:12 -04:00
2016-10-07 19:11:10 -03:00
// accelerometer position offset in body frame
2023-02-17 00:26:27 -04:00
AP_Vector3f _accel_pos_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
// Use Accessor methods to access above variables
# if INS_AUX_INSTANCES
# define INS_PARAM_WRAPPER(var) \
inline decltype ( var # # _old_param [ 0 ] ) & var ( uint8_t i ) { \
if ( i < ( INS_MAX_INSTANCES - INS_AUX_INSTANCES ) ) { \
return var # # _old_param [ i ] ; \
} else { \
return params [ i - ( INS_MAX_INSTANCES - INS_AUX_INSTANCES ) ] . var ; \
} \
} \
inline decltype ( var # # _old_param [ 0 ] ) & var ( uint8_t i ) const { \
return const_cast < AP_InertialSensor * > ( this ) - > var ( i ) ; \
}
# else
# define INS_PARAM_WRAPPER(var) \
inline decltype ( var # # _old_param [ 0 ] ) & var ( uint8_t i ) { \
return var # # _old_param [ i ] ; \
} \
inline decltype ( var # # _old_param [ 0 ] ) & var ( uint8_t i ) const { \
return const_cast < AP_InertialSensor * > ( this ) - > var ( i ) ; \
}
# endif
// Accessor methods for old parameters
INS_PARAM_WRAPPER ( _accel_id ) ;
INS_PARAM_WRAPPER ( _gyro_id ) ;
INS_PARAM_WRAPPER ( _accel_scale ) ;
INS_PARAM_WRAPPER ( _accel_offset ) ;
INS_PARAM_WRAPPER ( _gyro_offset ) ;
INS_PARAM_WRAPPER ( _accel_pos ) ;
2016-10-07 19:11:10 -03:00
2015-06-09 17:47:16 -03:00
// accelerometer max absolute offsets to be used for calibration
2015-06-29 21:51:43 -03:00
float _accel_max_abs_offsets [ INS_MAX_INSTANCES ] ;
2015-06-09 17:47:16 -03:00
2015-09-10 08:56:34 -03:00
// accelerometer and gyro raw sample rate in units of Hz
2017-04-30 21:53:41 -03:00
float _accel_raw_sample_rates [ INS_MAX_INSTANCES ] ;
float _gyro_raw_sample_rates [ INS_MAX_INSTANCES ] ;
2017-05-01 00:01:43 -03:00
// how many sensors samples per notify to the backend
uint8_t _accel_over_sampling [ INS_MAX_INSTANCES ] ;
uint8_t _gyro_over_sampling [ INS_MAX_INSTANCES ] ;
2017-04-30 21:53:41 -03:00
// last sample time in microseconds. Use for deltaT calculations
// on non-FIFO sensors
uint64_t _accel_last_sample_us [ INS_MAX_INSTANCES ] ;
uint64_t _gyro_last_sample_us [ INS_MAX_INSTANCES ] ;
// sample times for checking real sensor rate for FIFO sensors
uint16_t _sample_accel_count [ INS_MAX_INSTANCES ] ;
uint32_t _sample_accel_start_us [ INS_MAX_INSTANCES ] ;
uint16_t _sample_gyro_count [ INS_MAX_INSTANCES ] ;
uint32_t _sample_gyro_start_us [ INS_MAX_INSTANCES ] ;
2015-03-16 23:32:54 -03:00
// temperatures for an instance if available
float _temperature [ INS_MAX_INSTANCES ] ;
2012-11-29 16:15:12 -04:00
// filtering frequency (0 means default)
2019-05-25 07:59:57 -03:00
AP_Int16 _accel_filter_cutoff ;
AP_Int16 _gyro_filter_cutoff ;
2015-09-17 03:42:41 -03:00
AP_Int8 _gyro_cal_timing ;
2013-01-13 01:03:13 -04:00
2015-08-01 03:40:40 -03:00
// use for attitude, velocity, position estimates
2023-02-17 00:26:27 -04:00
AP_Int8 _use_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
INS_PARAM_WRAPPER ( _use ) ;
2015-08-01 03:40:40 -03:00
2016-11-15 01:51:18 -04:00
// control enable of fast sampling
AP_Int8 _fast_sampling_mask ;
2020-05-21 15:29:28 -03:00
// control enable of fast sampling
AP_Int8 _fast_sampling_rate ;
2018-01-17 03:16:48 -04:00
// control enable of detected sensors
AP_Int8 _enable_mask ;
2013-01-13 01:03:13 -04:00
// board orientation from AHRS
2014-10-14 01:48:33 -03:00
enum Rotation _board_orientation ;
2014-10-08 08:17:31 -03:00
2016-11-03 06:19:04 -03:00
// per-sensor orientation to allow for board type defaults at runtime
enum Rotation _gyro_orientation [ INS_MAX_INSTANCES ] ;
enum Rotation _accel_orientation [ INS_MAX_INSTANCES ] ;
2016-09-03 21:51:37 -03:00
// calibrated_ok/id_ok flags
2014-10-14 01:48:33 -03:00
bool _gyro_cal_ok [ INS_MAX_INSTANCES ] ;
2016-09-03 21:51:37 -03:00
bool _accel_id_ok [ INS_MAX_INSTANCES ] ;
2014-10-14 01:48:33 -03:00
// primary accel and gyro
2024-06-25 06:05:26 -03:00
uint8_t _first_usable_gyro ;
uint8_t _first_usable_accel ;
2014-10-14 01:48:33 -03:00
2019-07-04 07:18:14 -03:00
// mask of accels and gyros which we will be actively using
// and this should wait for in wait_for_sample()
uint8_t _gyro_wait_mask ;
uint8_t _accel_wait_mask ;
2017-06-27 01:42:45 -03:00
// bitmask bit which indicates if we should log raw accel and gyro data
uint32_t _log_raw_bit ;
2014-10-15 20:32:40 -03:00
// has wait_for_sample() found a sample?
bool _have_sample : 1 ;
2015-08-05 10:01:52 -03:00
bool _backends_detected : 1 ;
2021-01-07 20:46:35 -04:00
// are gyros or accels currently being calibrated
bool _calibrating_accel ;
bool _calibrating_gyro ;
2022-10-04 17:42:03 -03:00
bool _trimming_accel ;
2021-01-07 20:46:35 -04:00
2014-10-15 20:32:40 -03:00
// the delta time in seconds for the last sample
2014-10-14 01:48:33 -03:00
float _delta_time ;
2014-10-15 05:54:30 -03:00
// last time a wait_for_sample() returned a sample
uint32_t _last_sample_usec ;
2014-10-19 20:46:02 -03:00
// target time for next wait_for_sample() return
uint32_t _next_sample_usec ;
2016-01-19 21:26:31 -04:00
2014-10-15 05:54:30 -03:00
// time between samples in microseconds
uint32_t _sample_period_usec ;
2014-10-15 23:27:22 -03:00
2017-04-28 21:26:58 -03:00
// last time update() completed
uint32_t _last_update_usec ;
2014-10-15 23:27:22 -03:00
// health of gyros and accels
bool _gyro_healthy [ INS_MAX_INSTANCES ] ;
bool _accel_healthy [ INS_MAX_INSTANCES ] ;
2014-12-29 06:19:35 -04:00
uint32_t _accel_error_count [ INS_MAX_INSTANCES ] ;
uint32_t _gyro_error_count [ INS_MAX_INSTANCES ] ;
2015-05-06 23:08:30 -03:00
2015-06-11 04:15:57 -03:00
// vibration and clipping
uint32_t _accel_clip_count [ INS_MAX_INSTANCES ] ;
2015-07-30 04:58:06 -03:00
LowPassFilterVector3f _accel_vibe_floor_filter [ INS_VIBRATION_CHECK_INSTANCES ] ;
LowPassFilterVector3f _accel_vibe_filter [ INS_VIBRATION_CHECK_INSTANCES ] ;
2015-07-29 16:34:48 -03:00
2015-12-30 23:23:04 -04:00
// peak hold detector state for primary accel
struct PeakHoldState {
float accel_peak_hold_neg_x ;
uint32_t accel_peak_hold_neg_x_age ;
} _peak_hold_state ;
2015-12-29 13:18:07 -04:00
2015-07-29 16:34:48 -03:00
// threshold for detecting stillness
AP_Float _still_threshold ;
2015-06-11 04:15:57 -03:00
2015-07-20 17:25:40 -03:00
// Trim options
AP_Int8 _acc_body_aligned ;
AP_Int8 _trim_option ;
2019-02-10 14:29:24 -04:00
static AP_InertialSensor * _singleton ;
2015-07-20 17:25:40 -03:00
AP_AccelCal * _acal ;
AccelCalibrator * _accel_calibrator ;
//save accelerometer bias and scale factors
2018-11-07 07:26:38 -04:00
void _acal_save_calibrations ( ) override ;
void _acal_event_failure ( ) override ;
2015-07-20 17:25:40 -03:00
// Returns AccelCalibrator objects pointer for specified acceleromter
2018-11-07 07:26:38 -04:00
AccelCalibrator * _acal_get_calibrator ( uint8_t i ) override { return i < get_accel_count ( ) ? & ( _accel_calibrator [ i ] ) : nullptr ; }
2015-07-20 17:25:40 -03:00
2021-08-12 19:57:43 -03:00
Vector3f _trim_rad ;
2015-07-20 17:25:40 -03:00
bool _new_trim ;
2015-12-29 00:32:06 -04:00
bool _accel_cal_requires_reboot ;
2015-04-30 06:15:36 -03:00
// sensor error count at startup (used to ignore errors within 2 seconds of startup)
uint32_t _accel_startup_error_count [ INS_MAX_INSTANCES ] ;
uint32_t _gyro_startup_error_count [ INS_MAX_INSTANCES ] ;
bool _startup_error_counts_set ;
uint32_t _startup_ms ;
2019-04-18 01:24:01 -03:00
2024-07-09 08:30:23 -03:00
# if AP_INERTIALSENSOR_KILL_IMU_ENABLED
2019-04-18 01:24:01 -03:00
uint8_t imu_kill_mask ;
2024-07-09 08:30:23 -03:00
# endif
2021-01-07 20:46:35 -04:00
# if HAL_INS_TEMPERATURE_CAL_ENABLE
2021-01-08 06:50:24 -04:00
public :
2021-01-09 01:23:18 -04:00
// instance number for logging
2023-02-17 00:26:27 -04:00
# if INS_AUX_INSTANCES
uint8_t tcal_instance ( const AP_InertialSensor_TCal & tc ) const {
2023-03-27 19:58:56 -03:00
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES - INS_AUX_INSTANCES ; i + + ) {
2023-02-17 00:26:27 -04:00
if ( & tc = = & tcal_old_param [ i ] ) {
return i ;
}
}
for ( uint8_t i = 0 ; i < INS_AUX_INSTANCES ; i + + ) {
if ( & tc = = & params [ i ] . tcal ) {
return i + INS_MAX_INSTANCES ;
}
}
return 0 ;
2021-01-09 01:23:18 -04:00
}
2023-02-17 00:26:27 -04:00
# else
uint8_t tcal_instance ( const AP_InertialSensor_TCal & tc ) const {
return & tc - & tcal ( 0 ) ;
}
# endif
2021-01-08 06:50:24 -04:00
private :
2023-02-17 00:26:27 -04:00
AP_InertialSensor_TCal tcal_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
2021-01-07 20:46:35 -04:00
2021-01-16 01:40:00 -04:00
enum class TCalOptions : uint8_t {
PERSIST_TEMP_CAL = ( 1U < < 0 ) ,
PERSIST_ACCEL_CAL = ( 1U < < 1 ) ,
} ;
2021-01-07 20:46:35 -04:00
// temperature that last calibration was run at
2023-02-17 00:26:27 -04:00
AP_Float caltemp_accel_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
AP_Float caltemp_gyro_old_param [ INS_MAX_INSTANCES - INS_AUX_INSTANCES ] ;
INS_PARAM_WRAPPER ( caltemp_accel ) ;
INS_PARAM_WRAPPER ( caltemp_gyro ) ;
INS_PARAM_WRAPPER ( tcal ) ;
2021-01-16 01:40:00 -04:00
AP_Int32 tcal_options ;
2021-01-15 21:23:17 -04:00
bool tcal_learning ;
2021-01-07 20:46:35 -04:00
# endif
2023-09-29 18:26:31 -03:00
// Raw logging options bitmask and parameter
enum class RAW_LOGGING_OPTION {
PRIMARY_GYRO_ONLY = ( 1U < < 0 ) ,
ALL_GYROS = ( 1U < < 1 ) ,
POST_FILTER = ( 1U < < 2 ) ,
PRE_AND_POST_FILTER = ( 1U < < 3 ) ,
} ;
AP_Int16 raw_logging_options ;
bool raw_logging_option_set ( RAW_LOGGING_OPTION option ) const {
return ( raw_logging_options . get ( ) & int32_t ( option ) ) ! = 0 ;
}
2024-02-11 01:19:56 -04:00
// if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
// Support for the fast rate thread in copter
FastRateBuffer * fast_rate_buffer ;
2024-10-25 01:04:27 -03:00
bool fast_rate_buffer_enabled ;
2024-02-11 01:19:56 -04:00
public :
// enable the fast rate buffer and start pushing samples to it
void enable_fast_rate_buffer ( ) ;
// disable the fast rate buffer and stop pushing samples to it
void disable_fast_rate_buffer ( ) ;
// get the next available gyro sample from the fast rate buffer
bool get_next_gyro_sample ( Vector3f & gyro ) ;
// get the number of available gyro samples in the fast rate buffer
uint32_t get_num_gyro_samples ( ) ;
// set the rate at which samples are collected, unused samples are dropped
void set_rate_decimation ( uint8_t rdec ) ;
// push a new gyro sample into the fast rate buffer
bool push_next_gyro_sample ( const Vector3f & gyro ) ;
// run the filter parmeter update code.
void update_backend_filters ( ) ;
// are rate loop samples enabled for this instance?
bool is_rate_loop_gyro_enabled ( uint8_t instance ) const ;
// endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
2011-11-12 23:20:25 -04:00
} ;
2017-11-13 03:09:43 -04:00
namespace AP {
AP_InertialSensor & ins ( ) ;
} ;