From 202eb3af35e70e98a3a40d325608ab063b50db25 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 May 2016 12:26:30 +1000 Subject: [PATCH] DataFlash: added imu_mask to Log_Write_IMUDT --- libraries/DataFlash/DataFlash.cpp | 2 +- libraries/DataFlash/DataFlash.h | 2 +- libraries/DataFlash/LogFile.cpp | 14 ++++++++++---- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 500693e1fe..4ac5aec860 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -28,7 +28,7 @@ const AP_Param::GroupInfo DataFlash_Class::var_info[] = { // @Param: _REPLAY // @DisplayName: Enable logging of information needed for Replay // @Description: If LOG_REPLAY is set to 1 then the EKF2 state estimator will log detailed information needed for diagnosing problems with the Kalman filter. It is suggested that you also raise LOG_FILE_BUFSIZE to give more buffer space for logging and use a high quality microSD card to ensure no sensor data is lost - // @Values:0:Disabled,1:EnabledIMU1,3:EnabledIMU1andIMU2 + // @Values:0:Disabled,1:Enabled // @User: Standard AP_GROUPINFO("_REPLAY", 3, DataFlash_Class, _params.log_replay, 0), diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index af248f4739..c332501e53 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -108,7 +108,7 @@ public: void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, uint64_t time_us=0); void Log_Write_RFND(const RangeFinder &rangefinder); void Log_Write_IMU(const AP_InertialSensor &ins); - void Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us); + void Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us, uint8_t imu_mask); void Log_Write_Vibration(const AP_InertialSensor &ins); void Log_Write_RCIN(void); void Log_Write_RCOUT(void); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 7b7c26e96a..41a66dcc6d 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -932,7 +932,7 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) } // Write an accel/gyro delta time data packet -void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us) +void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us, uint8_t imu_mask) { float delta_t = ins.get_delta_time(); float delta_vel_t = ins.get_delta_velocity_dt(0); @@ -954,7 +954,9 @@ void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t tim delta_vel_y : delta_velocity.y, delta_vel_z : delta_velocity.z }; - WriteBlock(&pkt, sizeof(pkt)); + if (imu_mask & 1) { + WriteBlock(&pkt, sizeof(pkt)); + } if ((ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) || !ins.use_gyro(1)) { return; } @@ -980,7 +982,9 @@ void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t tim delta_vel_y : delta_velocity.y, delta_vel_z : delta_velocity.z }; - WriteBlock(&pkt2, sizeof(pkt2)); + if (imu_mask & 2) { + WriteBlock(&pkt2, sizeof(pkt2)); + } if ((ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) || !ins.use_gyro(2)) { return; @@ -1006,7 +1010,9 @@ void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t tim delta_vel_y : delta_velocity.y, delta_vel_z : delta_velocity.z }; - WriteBlock(&pkt3, sizeof(pkt3)); + if (imu_mask & 4) { + WriteBlock(&pkt3, sizeof(pkt3)); + } } void DataFlash_Class::Log_Write_Vibration(const AP_InertialSensor &ins)