Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: dataflash logging of PSC message

Browse Source
This commit is contained in:
Randy Mackay 2018-03-01 14:42:52 +09:00
parent c5d838125b
commit 13007e2a07
2 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1,6 +1,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AC_PosControl.h" #include "AC_PosControl.h"
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <DataFlash/DataFlash.h>
extern const AP_HAL::HAL& hal; 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; 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 /// 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() void AC_PosControl::init_vel_controller_xyz()
{ {

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

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