From 6c31a6982bb6ebfd00c22187d3aa3346af186a5c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 31 Mar 2017 10:37:35 +1100 Subject: [PATCH] DataFlash: added Log_Write_AttitudeView --- libraries/DataFlash/DataFlash.h | 1 + libraries/DataFlash/LogFile.cpp | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 733a381b74..f6a1dcde39 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -134,6 +134,7 @@ public: void Log_Write_ESC(void); void Log_Write_Airspeed(AP_Airspeed &airspeed); void Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets); + void Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets); void Log_Write_Current(const AP_BattMonitor &battery); void Log_Write_Compass(const Compass &compass, uint64_t time_us=0); void Log_Write_Mode(uint8_t mode, uint8_t reason = 0); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 682c1ee505..5610d0831f 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1832,6 +1832,24 @@ void DataFlash_Class::Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets) WriteBlock(&pkt, sizeof(pkt)); } +// Write an attitude packet +void DataFlash_Class::Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets) +{ + struct log_Attitude pkt = { + LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), + time_us : AP_HAL::micros64(), + control_roll : (int16_t)targets.x, + roll : (int16_t)ahrs.roll_sensor, + control_pitch : (int16_t)targets.y, + pitch : (int16_t)ahrs.pitch_sensor, + control_yaw : (uint16_t)targets.z, + yaw : (uint16_t)ahrs.yaw_sensor, + error_rp : (uint16_t)(ahrs.get_error_rp() * 100), + error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100) + }; + WriteBlock(&pkt, sizeof(pkt)); +} + // Write an Current data packet void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery) {