aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: added Log_Write_Rate()

Browse Source 
needed for quadplane as well as copter so should be in common code
This commit is contained in:
Andrew Tridgell 2016-03-25 12:11:11 +11:00
parent 1fa075e7a8
commit 075dd49afe
3 changed files with 60 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/DataFlash/DataFlash.h
View File

@ -20,6 +20,7 @@
#include <AP_RPM/AP_RPM.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <DataFlash/LogStructure.h>
#include <AP_Motors/AP_Motors.h>
#include <stdint.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -37,6 +38,10 @@ enum DataFlash_Backend_Type {
DATAFLASH_BACKEND_BOTH = 3,
};
// fwd declarations to avoid include errors
class AC_AttitudeControl;
class AC_PosControl;
class DataFlash_Class
{
friend class DataFlash_Backend; // for _num_types
@ -123,6 +128,10 @@ public:
const AP_Mission::Mission_Command &cmd);
void Log_Write_Origin(uint8_t origin_type, const Location &loc);
void Log_Write_RPM(const AP_RPM &rpm_sensor);
void Log_Write_Rate(const AP_AHRS &ahrs,
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control);
// This structure provides information on the internal member data of a PID for logging purposes
struct PID_Info {

30
libraries/DataFlash/LogFile.cpp
View File

@ -9,6 +9,9 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <AP_Motors/AP_Motors.h>
#include <AC_AttitudeControl/AC_AttitudeControl.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include "DataFlash.h"
#include "DataFlash_SITL.h"
@ -1795,3 +1798,30 @@ void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor)
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write an rate packet
void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs,
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control)
{
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target();
struct log_Rate pkt_rate = {
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : AP_HAL::micros64(),
control_roll : (float)rate_targets.x,
roll : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
roll_out : motors.get_roll(),
control_pitch : (float)rate_targets.y,
pitch : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
pitch_out : motors.get_pitch(),
control_yaw : (float)rate_targets.z,
yaw : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
yaw_out : motors.get_yaw(),
control_accel : (float)accel_target.z,
accel : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
accel_out : motors.get_throttle()
};
WriteBlock(&pkt_rate, sizeof(pkt_rate));
}

25
libraries/DataFlash/LogStructure.h
View File

@ -626,6 +626,23 @@ struct PACKED log_RPM {
float rpm2;
};
struct PACKED log_Rate {
LOG_PACKET_HEADER;
uint64_t time_us;
float control_roll;
float roll;
float roll_out;
float control_pitch;
float pitch;
float pitch_out;
float control_yaw;
float yaw;
float yaw_out;
float control_accel;
float accel;
float accel_out;
};
// #if SBP_HW_LOGGING
struct PACKED log_SbpLLH {
@ -860,7 +877,9 @@ Format characters in the format string for binary log messages
{ LOG_GIMBAL2_MSG, sizeof(log_Gimbal2), \
"GMB2", "IBfffffffff", "TimeMS,es,ex,ey,ez,rx,ry,rz,tx,ty,tz" }, \
{ LOG_GIMBAL3_MSG, sizeof(log_Gimbal3), \
"GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }
"GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }
// #if SBP_HW_LOGGING
#define LOG_SBP_STRUCTURES \
@ -973,9 +992,7 @@ enum LogMessages {
LOG_GIMBAL1_MSG,
LOG_GIMBAL2_MSG,
LOG_GIMBAL3_MSG,
// message types 211 to 220 reversed for autotune use
LOG_RATE_MSG,
};
enum LogOriginType {