Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: add structure for PSC message

Browse Source
This commit is contained in:
Iampete1 2020-09-09 08:16:39 +01:00 committed by Peter Barker
parent 1565c69943
commit 02c225ee93
3 changed files with 59 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

@ -289,6 +289,7 @@ public:
void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
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(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, ...);

21
libraries/AP_Logger/LogFile.cpp
View File

@ -1134,3 +1134,24 @@ 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)
{
struct log_PSC pkt{
LOG_PACKET_HEADER_INIT(LOG_PSC_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
};
WriteBlock(&pkt, sizeof(pkt));
}

38
libraries/AP_Logger/LogStructure.h
View File

@ -1271,6 +1271,23 @@ struct PACKED log_Winch {
int8_t temp;
};
struct PACKED log_PSC {
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;
};
// 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
@ -2451,6 +2468,22 @@ struct PACKED log_Winch {
// @Field: Vcc: Voltage to Motor
// @Field: Temp: Motor temperature
// @LoggerMessage: PSC
// @Description: Position Control data
// @Field: TimeUS: Time since system startup
// @Field: TPX: Target position relative to origin, X-axis
// @Field: TPY: Target position relative to origin, Y-axis
// @Field: PX: Position relative to origin, X-axis
// @Field: PY: Position relative to origin, Y-axis
// @Field: TVX: Target velocity, X-axis
// @Field: TVY: Target velocity, Y-axis
// @Field: VX: Velocity, X-axis
// @Field: VY: Velocity, Y-axis
// @Field: TAX: Target acceleration, X-axis
// @Field: TAY: Target acceleration, Y-axis
// @Field: AX: Acceleration, X-axis
// @Field: AY: Acceleration, Y-axis
// messages for all boards
#define LOG_BASE_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
@ -2668,7 +2701,9 @@ struct PACKED log_Winch {
{ LOG_ERROR_MSG, sizeof(log_Error), \
"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" }
"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" }
// @LoggerMessage: SBPH
// @Description: Swift Health Data
@ -2830,6 +2865,7 @@ enum LogMessages : uint8_t {
LOG_VISUALVEL_MSG,
LOG_SIMPLE_AVOID_MSG,
LOG_WINCH_MSG,
LOG_PSC_MSG,
_LOG_LAST_MSG_
};