Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: Update PSC logging to include desired

This commit is contained in:
Leonard Hall 2021-09-22 12:11:15 +09:30 committed by Randy Mackay
parent 0a4ba0482a
commit cbce08bace
3 changed files with 118 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Logger/AP_Logger.h
View File

@ -321,8 +321,9 @@ public:
#endif #endif
void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
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_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_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
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_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
void Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
void Write(const char *name, const char *labels, const char *fmt, ...); 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, ...); void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

68
libraries/AP_Logger/LogFile.cpp
View File

@ -553,41 +553,53 @@ void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clu
WriteBlock(&pkt, sizeof(pkt)); WriteBlock(&pkt, sizeof(pkt));
} }
void AP_Logger::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 AP_Logger::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{ {
struct log_PSC pkt{ const struct log_PSCN pkt{
LOG_PACKET_HEADER_INIT(LOG_PSC_MSG), LOG_PACKET_HEADER_INIT(LOG_PSCN_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
pos_target_x : pos_target.x * 0.01f, pos_target : pos_target * 0.01f,
pos_target_Y : pos_target.y * 0.01f, pos : pos * 0.01f,
position_x : position.x * 0.01f, vel_desired : vel_desired * 0.01f,
position_y : position.y * 0.01f, vel_target : vel_target * 0.01f,
vel_target_x : vel_target.x * 0.01f, vel : vel * 0.01f,
vel_target_y : vel_target.y * 0.01f, accel_desired : accel_desired * 0.01f,
velocity_x : velocity.x * 0.01f, accel_target : accel_target * 0.01f,
velocity_y : velocity.y * 0.01f, accel : accel * 0.01f
accel_target_x : accel_target.x * 0.01f,
accel_target_y : accel_target.y * 0.01f,
accel_x : accel_x * 0.01f,
accel_y : accel_y * 0.01f
}; };
WriteBlock(&pkt, sizeof(pkt)); 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) void AP_Logger::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{ {
const struct log_PSCZ pkt{ const struct log_PSCE pkt{
LOG_PACKET_HEADER_INIT(LOG_PSCZ_MSG), LOG_PACKET_HEADER_INIT(LOG_PSCE_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
pos_target_z : pos_target_z * 0.01f, pos_target : pos_target * 0.01f,
pos_z : pos_z * 0.01f, pos : pos * 0.01f,
vel_desired_z : vel_desired_z * 0.01f, vel_desired : vel_desired * 0.01f,
vel_target_z : vel_target_z * 0.01f, vel_target : vel_target * 0.01f,
vel_z : vel_z * 0.01f, vel : vel * 0.01f,
accel_desired_z : accel_desired_z * 0.01f, accel_desired : accel_desired * 0.01f,
accel_target_z : accel_target_z * 0.01f, accel_target : accel_target * 0.01f,
accel_z : accel_z * 0.01f, accel : accel * 0.01f
throttle_out : throttle_out };
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{
const struct log_PSCD pkt{
LOG_PACKET_HEADER_INIT(LOG_PSCD_MSG),
time_us : AP_HAL::micros64(),
pos_target : pos_target * 0.01f,
pos : pos * 0.01f,
vel_desired : vel_desired * 0.01f,
vel_target : vel_target * 0.01f,
vel : vel * 0.01f,
accel_desired : accel_desired * 0.01f,
accel_target : accel_target * 0.01f,
accel : accel * 0.01f
}; };
WriteBlock(&pkt, sizeof(pkt)); WriteBlock(&pkt, sizeof(pkt));
} }

130
libraries/AP_Logger/LogStructure.h
View File

@ -680,36 +680,46 @@ struct PACKED log_Winch {
int8_t temp; int8_t temp;
}; };
struct PACKED log_PSC { // position controller North axis logging
struct PACKED log_PSCN {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
float pos_target_x; float pos_target;
float pos_target_Y; float pos;
float position_x; float vel_desired;
float position_y; float vel_target;
float vel_target_x; float vel;
float vel_target_y; float accel_desired;
float velocity_x; float accel_target;
float velocity_y; float accel;
float accel_target_x;
float accel_target_y;
float accel_x;
float accel_y;
}; };
// position controller z-axis logging // position controller East axis logging
struct PACKED log_PSCZ { struct PACKED log_PSCE {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
float pos_target_z; float pos_target;
float pos_z; float pos;
float vel_desired_z; float vel_desired;
float vel_target_z; float vel_target;
float vel_z; float vel;
float accel_desired_z; float accel_desired;
float accel_target_z; float accel_target;
float accel_z; float accel;
float throttle_out; };
// position controller Down axis logging
struct PACKED log_PSCD {
LOG_PACKET_HEADER;
uint64_t time_us;
float pos_target;
float pos;
float vel_desired;
float vel_target;
float vel;
float accel_desired;
float accel_target;
float accel;
}; };
// FMT messages define all message formats other than FMT // FMT messages define all message formats other than FMT
@ -1178,34 +1188,41 @@ struct PACKED log_PSCZ {
// @Field: Vcc: Voltage to Motor // @Field: Vcc: Voltage to Motor
// @Field: Temp: Motor temperature // @Field: Temp: Motor temperature
// @LoggerMessage: PSC // @LoggerMessage: PSCN
// @Description: Position Control data // @Description: Position Control North
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: TPX: Target position relative to origin, North // @Field: TPN: Target position relative to EKF origin
// @Field: TPY: Target position relative to origin, East // @Field: PN: Position relative to EKF origin
// @Field: PX: Position relative to origin, North // @Field: DVN: Desired velocity North
// @Field: PY: Position relative to origin, East // @Field: TVN: Target velocity North
// @Field: TVX: Target velocity, North // @Field: VN: Velocity North
// @Field: TVY: Target velocity, East // @Field: DAN: Desired acceleration North
// @Field: VX: Velocity, North // @Field: TAN: Target acceleration North
// @Field: VY: Velocity, East // @Field: AN: Acceleration North
// @Field: TAX: Target acceleration, X-axis
// @Field: TAY: Target acceleration, Y-axis
// @Field: AX: Acceleration, X-axis
// @Field: AY: Acceleration, Y-axis
// @LoggerMessage: PSCZ // @LoggerMessage: PSCE
// @Description: Position Control Z-axis // @Description: Position Control East
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: TPZ: Target position above EKF origin // @Field: TPE: Target position relative to EKF origin
// @Field: PZ: Position above EKF origin // @Field: PE: Position relative to EKF origin
// @Field: DVZ: Desired velocity Z-axis // @Field: DVE: Desired velocity East
// @Field: TVZ: Target velocity Z-axis // @Field: TVE: Target velocity East
// @Field: VZ: Velocity Z-axis // @Field: VE: Velocity East
// @Field: DAZ: Desired acceleration Z-axis // @Field: DAE: Desired acceleration East
// @Field: TAZ: Target acceleration Z-axis // @Field: TAE: Target acceleration East
// @Field: AZ: Acceleration Z-axis // @Field: AE: Acceleration East
// @Field: ThO: Throttle output
// @LoggerMessage: PSCD
// @Description: Position Control Down
// @Field: TimeUS: Time since system startup
// @Field: TPD: Target position relative to EKF origin
// @Field: PD: Position relative to EKF origin
// @Field: DVD: Desired velocity Down
// @Field: TVD: Target velocity Down
// @Field: VD: Velocity Down
// @Field: DAD: Desired acceleration Down
// @Field: TAD: Target acceleration Down
// @Field: AD: Acceleration Down
// messages for all boards // messages for all boards
#define LOG_BASE_STRUCTURES \ #define LOG_BASE_STRUCTURES \
@ -1314,10 +1331,12 @@ LOG_STRUCTURE_FROM_VISUALODOM \
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" }, \ "ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" }, \
{ LOG_WINCH_MSG, sizeof(log_Winch), \ { 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" }, \ "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), \ { LOG_PSCN_MSG, sizeof(log_PSCN), \
"PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000" }, \ "PSCN", "Qffffffff", "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", "smmnnnooo", "F00000000" }, \
{ LOG_PSCZ_MSG, sizeof(log_PSCZ), \ { LOG_PSCE_MSG, sizeof(log_PSCE), \
"PSCZ", "Qfffffffff", "TimeUS,TPZ,PZ,DVZ,TVZ,VZ,DAZ,TAZ,AZ,ThO", "smmnnnooo%", "F000000002" } "PSCE", "Qffffffff", "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", "smmnnnooo", "F00000000" }, \
{ LOG_PSCD_MSG, sizeof(log_PSCD), \
"PSCD", "Qffffffff", "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", "smmnnnooo", "F00000000" }
// @LoggerMessage: SBPH // @LoggerMessage: SBPH
// @Description: Swift Health Data // @Description: Swift Health Data
@ -1427,8 +1446,9 @@ enum LogMessages : uint8_t {
LOG_ARM_DISARM_MSG, LOG_ARM_DISARM_MSG,
LOG_IDS_FROM_AVOIDANCE, LOG_IDS_FROM_AVOIDANCE,
LOG_WINCH_MSG, LOG_WINCH_MSG,
LOG_PSC_MSG, LOG_PSCN_MSG,
LOG_PSCZ_MSG, LOG_PSCE_MSG,
LOG_PSCD_MSG,
LOG_RAW_PROXIMITY_MSG, LOG_RAW_PROXIMITY_MSG,
LOG_IDS_FROM_PRECLAND, LOG_IDS_FROM_PRECLAND,