mirror of https://github.com/ArduPilot/ardupilot
AP_Logger: Update PSC logging to include desired
This commit is contained in:
Leonard Hall
Randy Mackay
parent
0a4ba0482a
commit
cbce08bace
|
|
@ -321,8 +321,9 @@ public:
|
|||
#endif
|
||||
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_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_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
|
||||
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 *units, const char *mults, const char *fmt, ...);
|
||||
|
|
|
|||
|
|
@ -553,41 +553,53 @@ void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clu
|
|||
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{
|
||||
LOG_PACKET_HEADER_INIT(LOG_PSC_MSG),
|
||||
const struct log_PSCN pkt{
|
||||
LOG_PACKET_HEADER_INIT(LOG_PSCN_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
pos_target_x : pos_target.x * 0.01f,
|
||||
pos_target_Y : pos_target.y * 0.01f,
|
||||
position_x : position.x * 0.01f,
|
||||
position_y : position.y * 0.01f,
|
||||
vel_target_x : vel_target.x * 0.01f,
|
||||
vel_target_y : vel_target.y * 0.01f,
|
||||
velocity_x : velocity.x * 0.01f,
|
||||
velocity_y : velocity.y * 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
|
||||
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));
|
||||
}
|
||||
|
||||
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{
|
||||
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
|
||||
const struct log_PSCE pkt{
|
||||
LOG_PACKET_HEADER_INIT(LOG_PSCE_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));
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -680,36 +680,46 @@ struct PACKED log_Winch {
|
|||
int8_t temp;
|
||||
};
|
||||
|
||||
struct PACKED log_PSC {
|
||||
// position controller North axis logging
|
||||
struct PACKED log_PSCN {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
float pos_target_x;
|
||||
float pos_target_Y;
|
||||
float position_x;
|
||||
float position_y;
|
||||
float vel_target_x;
|
||||
float vel_target_y;
|
||||
float velocity_x;
|
||||
float velocity_y;
|
||||
float accel_target_x;
|
||||
float accel_target_y;
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float pos_target;
|
||||
float pos;
|
||||
float vel_desired;
|
||||
float vel_target;
|
||||
float vel;
|
||||
float accel_desired;
|
||||
float accel_target;
|
||||
float accel;
|
||||
};
|
||||
|
||||
// position controller z-axis logging
|
||||
struct PACKED log_PSCZ {
|
||||
// position controller East axis logging
|
||||
struct PACKED log_PSCE {
|
||||
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;
|
||||
float pos_target;
|
||||
float pos;
|
||||
float vel_desired;
|
||||
float vel_target;
|
||||
float vel;
|
||||
float accel_desired;
|
||||
float accel_target;
|
||||
float accel;
|
||||
};
|
||||
|
||||
// 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
|
||||
|
|
@ -1178,34 +1188,41 @@ struct PACKED log_PSCZ {
|
|||
// @Field: Vcc: Voltage to Motor
|
||||
// @Field: Temp: Motor temperature
|
||||
|
||||
// @LoggerMessage: PSC
|
||||
// @Description: Position Control data
|
||||
// @LoggerMessage: PSCN
|
||||
// @Description: Position Control North
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: TPX: Target position relative to origin, North
|
||||
// @Field: TPY: Target position relative to origin, East
|
||||
// @Field: PX: Position relative to origin, North
|
||||
// @Field: PY: Position relative to origin, East
|
||||
// @Field: TVX: Target velocity, North
|
||||
// @Field: TVY: Target velocity, East
|
||||
// @Field: VX: Velocity, North
|
||||
// @Field: VY: Velocity, East
|
||||
// @Field: TAX: Target acceleration, X-axis
|
||||
// @Field: TAY: Target acceleration, Y-axis
|
||||
// @Field: AX: Acceleration, X-axis
|
||||
// @Field: AY: Acceleration, Y-axis
|
||||
// @Field: TPN: Target position relative to EKF origin
|
||||
// @Field: PN: Position relative to EKF origin
|
||||
// @Field: DVN: Desired velocity North
|
||||
// @Field: TVN: Target velocity North
|
||||
// @Field: VN: Velocity North
|
||||
// @Field: DAN: Desired acceleration North
|
||||
// @Field: TAN: Target acceleration North
|
||||
// @Field: AN: Acceleration North
|
||||
|
||||
// @LoggerMessage: PSCZ
|
||||
// @Description: Position Control Z-axis
|
||||
// @LoggerMessage: PSCE
|
||||
// @Description: Position Control East
|
||||
// @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
|
||||
// @Field: TPE: Target position relative to EKF origin
|
||||
// @Field: PE: Position relative to EKF origin
|
||||
// @Field: DVE: Desired velocity East
|
||||
// @Field: TVE: Target velocity East
|
||||
// @Field: VE: Velocity East
|
||||
// @Field: DAE: Desired acceleration East
|
||||
// @Field: TAE: Target acceleration East
|
||||
// @Field: AE: Acceleration East
|
||||
|
||||
// @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
|
||||
#define LOG_BASE_STRUCTURES \
|
||||
|
|
@ -1314,10 +1331,12 @@ LOG_STRUCTURE_FROM_VISUALODOM \
|
|||
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" }, \
|
||||
{ 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" }, \
|
||||
{ LOG_PSCZ_MSG, sizeof(log_PSCZ), \
|
||||
"PSCZ", "Qfffffffff", "TimeUS,TPZ,PZ,DVZ,TVZ,VZ,DAZ,TAZ,AZ,ThO", "smmnnnooo%", "F000000002" }
|
||||
{ LOG_PSCN_MSG, sizeof(log_PSCN), \
|
||||
"PSCN", "Qffffffff", "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", "smmnnnooo", "F00000000" }, \
|
||||
{ LOG_PSCE_MSG, sizeof(log_PSCE), \
|
||||
"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
|
||||
// @Description: Swift Health Data
|
||||
|
|
@ -1427,8 +1446,9 @@ enum LogMessages : uint8_t {
|
|||
LOG_ARM_DISARM_MSG,
|
||||
LOG_IDS_FROM_AVOIDANCE,
|
||||
LOG_WINCH_MSG,
|
||||
LOG_PSC_MSG,
|
||||
LOG_PSCZ_MSG,
|
||||
LOG_PSCN_MSG,
|
||||
LOG_PSCE_MSG,
|
||||
LOG_PSCD_MSG,
|
||||
LOG_RAW_PROXIMITY_MSG,
|
||||
LOG_IDS_FROM_PRECLAND,
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue