You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
171 lines
6.4 KiB
171 lines
6.4 KiB
#include "AP_InertialSensor.h" |
|
#include "AP_InertialSensor_Backend.h" |
|
|
|
#include <AP_Logger/AP_Logger.h> |
|
|
|
// Write ACC data packet: raw accel data |
|
void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const |
|
{ |
|
const uint64_t now = AP_HAL::micros64(); |
|
const struct log_ACC pkt { |
|
LOG_PACKET_HEADER_INIT(LOG_ACC_MSG), |
|
time_us : now, |
|
instance : instance, |
|
sample_us : sample_us?sample_us:now, |
|
AccX : accel.x, |
|
AccY : accel.y, |
|
AccZ : accel.z |
|
}; |
|
AP::logger().WriteBlock(&pkt, sizeof(pkt)); |
|
} |
|
|
|
// Write GYR data packet: raw gyro data |
|
void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const |
|
{ |
|
const uint64_t now = AP_HAL::micros64(); |
|
const struct log_GYR pkt{ |
|
LOG_PACKET_HEADER_INIT(LOG_GYR_MSG), |
|
time_us : now, |
|
instance : instance, |
|
sample_us : sample_us?sample_us:now, |
|
GyrX : gyro.x, |
|
GyrY : gyro.y, |
|
GyrZ : gyro.z |
|
}; |
|
AP::logger().WriteBlock(&pkt, sizeof(pkt)); |
|
} |
|
|
|
// Write IMU data packet: raw accel/gyro data |
|
void AP_InertialSensor::Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const |
|
{ |
|
const Vector3f &gyro = get_gyro(imu_instance); |
|
const Vector3f &accel = get_accel(imu_instance); |
|
const struct log_IMU pkt{ |
|
LOG_PACKET_HEADER_INIT(LOG_IMU_MSG), |
|
time_us : time_us, |
|
instance: imu_instance, |
|
gyro_x : gyro.x, |
|
gyro_y : gyro.y, |
|
gyro_z : gyro.z, |
|
accel_x : accel.x, |
|
accel_y : accel.y, |
|
accel_z : accel.z, |
|
gyro_error : _gyro_error_count[imu_instance], |
|
accel_error : _accel_error_count[imu_instance], |
|
temperature : get_temperature(imu_instance), |
|
gyro_health : (uint8_t)get_gyro_health(imu_instance), |
|
accel_health : (uint8_t)get_accel_health(imu_instance), |
|
gyro_rate : get_gyro_rate_hz(imu_instance), |
|
accel_rate : get_accel_rate_hz(imu_instance), |
|
}; |
|
AP::logger().WriteBlock(&pkt, sizeof(pkt)); |
|
} |
|
|
|
// Write IMU data packet for all instances |
|
void AP_InertialSensor::Write_IMU() const |
|
{ |
|
const uint64_t time_us = AP_HAL::micros64(); |
|
|
|
uint8_t n = MAX(get_accel_count(), get_gyro_count()); |
|
for (uint8_t i=0; i<n; i++) { |
|
Write_IMU_instance(time_us, i); |
|
} |
|
} |
|
|
|
// Write VIBE data packet for all instances |
|
void AP_InertialSensor::Write_Vibration() const |
|
{ |
|
const uint64_t time_us = AP_HAL::micros64(); |
|
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { |
|
if (!use_accel(i)) { |
|
continue; |
|
} |
|
|
|
const Vector3f vibration = get_vibration_levels(i); |
|
const struct log_Vibe pkt{ |
|
LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG), |
|
time_us : time_us, |
|
imu : i, |
|
vibe_x : vibration.x, |
|
vibe_y : vibration.y, |
|
vibe_z : vibration.z, |
|
clipping : get_accel_clip_count(i) |
|
}; |
|
AP::logger().WriteBlock(&pkt, sizeof(pkt)); |
|
} |
|
} |
|
|
|
// Write information about a series of IMU readings to log: |
|
bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const |
|
{ |
|
uint8_t instance_to_write = instance; |
|
if (post_filter && (_doing_pre_post_filter_logging |
|
|| (_doing_post_filter_logging && _doing_sensor_rate_logging))) { |
|
instance_to_write += (type == IMU_SENSOR_TYPE_ACCEL ? _imu._accel_count : _imu._gyro_count); |
|
} |
|
const struct log_ISBH pkt{ |
|
LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG), |
|
time_us : AP_HAL::micros64(), |
|
seqno : isb_seqnum, |
|
sensor_type : (uint8_t)type, |
|
instance : instance_to_write, |
|
multiplier : multiplier, |
|
sample_count : (uint16_t)_required_count, |
|
sample_us : measurement_started_us, |
|
sample_rate_hz : sample_rate_hz, |
|
}; |
|
|
|
return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt)); |
|
} |
|
|
|
// Write a series of IMU readings to log: |
|
bool AP_InertialSensor::BatchSampler::Write_ISBD() const |
|
{ |
|
struct log_ISBD pkt = { |
|
LOG_PACKET_HEADER_INIT(LOG_ISBD_MSG), |
|
time_us : AP_HAL::micros64(), |
|
isb_seqno : isb_seqnum, |
|
seqno : (uint16_t) (data_read_offset/samples_per_msg) |
|
}; |
|
memcpy(pkt.x, &data_x[data_read_offset], sizeof(pkt.x)); |
|
memcpy(pkt.y, &data_y[data_read_offset], sizeof(pkt.y)); |
|
memcpy(pkt.z, &data_z[data_read_offset], sizeof(pkt.z)); |
|
|
|
return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt)); |
|
} |
|
|
|
// @LoggerMessage: FTN |
|
// @Description: Filter Tuning Messages |
|
// @Field: TimeUS: microseconds since system startup |
|
// @Field: I: instance |
|
// @Field: NDn: number of active dynamic harmonic notches |
|
// @Field: NF1: dynamic harmonic notch centre frequency for motor 1 |
|
// @Field: NF2: dynamic harmonic notch centre frequency for motor 2 |
|
// @Field: NF3: dynamic harmonic notch centre frequency for motor 3 |
|
// @Field: NF4: dynamic harmonic notch centre frequency for motor 4 |
|
// @Field: NF5: dynamic harmonic notch centre frequency for motor 5 |
|
// @Field: NF6: dynamic harmonic notch centre frequency for motor 6 |
|
// @Field: NF7: dynamic harmonic notch centre frequency for motor 7 |
|
// @Field: NF8: dynamic harmonic notch centre frequency for motor 8 |
|
// @Field: NF9: dynamic harmonic notch centre frequency for motor 9 |
|
// @Field: NF10: dynamic harmonic notch centre frequency for motor 10 |
|
// @Field: NF11: dynamic harmonic notch centre frequency for motor 11 |
|
// @Field: NF12: dynamic harmonic notch centre frequency for motor 12 |
|
void AP_InertialSensor::write_notch_log_messages() const |
|
{ |
|
for (auto ¬ch : harmonic_notches) { |
|
const uint8_t i = ¬ch - &harmonic_notches[0]; |
|
if (!notch.params.enabled()) { |
|
continue; |
|
} |
|
const float* notches = notch.calculated_notch_freq_hz; |
|
AP::logger().WriteStreaming( |
|
"FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff", |
|
AP_HAL::micros64(), |
|
i, |
|
notch.num_calculated_notch_frequencies, |
|
notches[0], notches[1], notches[2], notches[3], |
|
notches[4], notches[5], notches[6], notches[7], |
|
notches[8], notches[9], notches[10], notches[11]); |
|
} |
|
}
|
|
|