Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: add PSCZ logging

Browse Source 
Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>

includes const log_PSCZ declaration
also changed PSZ ThO to percentage
This commit is contained in:
Randy Mackay 2021-01-29 13:42:26 +09:00
parent 8135d9e60a
commit 3ae8af6b21
3 changed files with 51 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Logger/AP_Logger.h
View File

@ -335,6 +335,7 @@ public:
void Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up);
void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp);
void Write_PSC(const Vector3f &pos_target, const Vector3f &position, const Vector3f &vel_target, const Vector3f &velocity, const Vector3f &accel_target, const float &accel_x, const float &accel_y);
void Write_PSCZ(float pos_target_z, float pos_z, float vel_desired_z, float vel_target_z, float vel_z, float accel_desired_z, float accel_target_z, float accel_z, float throttle_out);
void Write(const char *name, const char *labels, const char *fmt, ...);
void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

18
libraries/AP_Logger/LogFile.cpp
View File

@ -747,3 +747,21 @@ void AP_Logger::Write_PSC(const Vector3f &pos_target, const Vector3f &position,
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_PSCZ(float pos_target_z, float pos_z, float vel_desired_z, float vel_target_z, float vel_z, float accel_desired_z, float accel_target_z, float accel_z, float throttle_out)
{
const struct log_PSCZ pkt{
LOG_PACKET_HEADER_INIT(LOG_PSCZ_MSG),
time_us : AP_HAL::micros64(),
pos_target_z : pos_target_z * 0.01f,
pos_z : pos_z * 0.01f,
vel_desired_z : vel_desired_z * 0.01f,
vel_target_z : vel_target_z * 0.01f,
vel_z : vel_z * 0.01f,
accel_desired_z : accel_desired_z * 0.01f,
accel_target_z : accel_target_z * 0.01f,
accel_z : accel_z * 0.01f,
throttle_out : throttle_out
};
WriteBlock(&pkt, sizeof(pkt));
}

33
libraries/AP_Logger/LogStructure.h
View File

@ -898,6 +898,21 @@ struct PACKED log_PSC {
float accel_y;
};
// position controller z-axis logging
struct PACKED log_PSCZ {
LOG_PACKET_HEADER;
uint64_t time_us;
float pos_target_z;
float pos_z;
float vel_desired_z;
float vel_target_z;
float vel_z;
float accel_desired_z;
float accel_target_z;
float accel_z;
float throttle_out;
};
// FMT messages define all message formats other than FMT
// UNIT messages define units which can be referenced by FMTU messages
// FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields
@ -1571,6 +1586,19 @@ struct PACKED log_PSC {
// @Field: AX: Acceleration, X-axis
// @Field: AY: Acceleration, Y-axis
// @LoggerMessage: PSCZ
// @Description: Position Control Z-axis
// @Field: TimeUS: Time since system startup
// @Field: TPZ: Target position above EKF origin
// @Field: PZ: Position above EKF origin
// @Field: DVZ: Desired velocity Z-axis
// @Field: TVZ: Target velocity Z-axis
// @Field: VZ: Velocity Z-axis
// @Field: DAZ: Desired acceleration Z-axis
// @Field: TAZ: Target acceleration Z-axis
// @Field: AZ: Acceleration Z-axis
// @Field: ThO: Throttle output
// messages for all boards
#define LOG_BASE_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
@ -1703,7 +1731,9 @@ LOG_STRUCTURE_FROM_VISUALODOM \
{ LOG_WINCH_MSG, sizeof(log_Winch), \
"WINC", "QBBBBBfffHfb", "TimeUS,Heal,ThEnd,Mov,Clut,Mode,DLen,Len,DRate,Tens,Vcc,Temp", "s-----mmn?vO", "F-----000000" }, \
{ LOG_PSC_MSG, sizeof(log_PSC), \
"PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000" }
"PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000" }, \
{ LOG_PSCZ_MSG, sizeof(log_PSCZ), \
"PSCZ", "Qfffffffff", "TimeUS,TPZ,PZ,DVZ,TVZ,VZ,DAZ,TAZ,AZ,ThO", "smmnnnooo%", "F000000002" }
// @LoggerMessage: SBPH
// @Description: Swift Health Data
@ -1825,6 +1855,7 @@ enum LogMessages : uint8_t {
LOG_SIMPLE_AVOID_MSG,
LOG_WINCH_MSG,
LOG_PSC_MSG,
LOG_PSCZ_MSG,
_LOG_LAST_MSG_
};