AC_PosControl: dataflash logging of PSC message

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -1,6 +1,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AC_PosControl.h"
 #include <AP_Math/AP_Math.h>
+#include <DataFlash/DataFlash.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -836,6 +837,34 @@ float AC_PosControl::time_since_last_xy_update() const
     return (now - _last_update_xy_ms)*0.001f;
 }
 
+// write log to dataflash
+void AC_PosControl::write_log()
+{
+    const Vector3f &pos_target = get_pos_target();
+    const Vector3f &vel_target = get_vel_target();
+    const Vector3f &accel_target = get_accel_target();
+    const Vector3f &position = _inav.get_position();
+    const Vector3f &velocity = _inav.get_velocity();
+    float accel_x, accel_y;
+    lean_angles_to_accel(accel_x, accel_y);
+
+    DataFlash_Class::instance()->Log_Write("PSC", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
+                                           "smmmmnnnnoooo", "FBBBBBBBBBBBB", "Qffffffffffff",
+                                           AP_HAL::micros64(),
+                                           (double)pos_target.x,
+                                           (double)pos_target.y,
+                                           (double)position.x,
+                                           (double)position.y,
+                                           (double)vel_target.x,
+                                           (double)vel_target.y,
+                                           (double)velocity.x,
+                                           (double)velocity.y,
+                                           (double)accel_target.x,
+                                           (double)accel_target.y,
+                                           (double)accel_x,
+                                           (double)accel_y);
+}
+
 /// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
 void AC_PosControl::init_vel_controller_xyz()
 {

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -296,6 +296,9 @@ public:
     // time_since_last_xy_update - returns time in seconds since the horizontal position controller was last run
     float time_since_last_xy_update() const;
 
+    // write log to dataflash
+    void write_log();
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected: