Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: Convert to m except IMAX

Browse Source
This commit is contained in:
Leonard Hall
2025-07-11 13:50:33 +09:00
committed by Randy Mackay
parent 5492740b4e
commit a353ce05ba
3 changed files with 798 additions and 600 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp
+41 -41
View File
@@ -8,83 +8,83 @@
#include "LogStructure.h"
// a convenience function for writing out the position controller PIDs
void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss)
void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
{
const struct log_PSCx pkt{
LOG_PACKET_HEADER_INIT(id),
time_us : AP_HAL::micros64(),
pos_desired : pos_desired_cm * 0.01f,
pos_target : pos_target_cm * 0.01f,
pos : pos_cm * 0.01f,
vel_desired : vel_desired_cms * 0.01f,
vel_target : vel_target_cms * 0.01f,
vel : vel_cms * 0.01f,
accel_desired : accel_desired_cmss * 0.01f,
accel_target : accel_target_cmss * 0.01f,
accel : accel_cmss * 0.01f
pos_desired : pos_desired_m,
pos_target : pos_target_m,
pos : pos_m,
vel_desired : vel_desired_ms,
vel_target : vel_target_ms,
vel : vel_ms,
accel_desired : accel_desired_mss,
accel_target : accel_target_mss,
accel : accel_mss
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void AC_PosControl::Write_PSCN(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss)
void AC_PosControl::Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
{
Write_PSCx(LOG_PSCN_MSG, pos_desired_cm, pos_target_cm, pos_cm, vel_desired_cms, vel_target_cms, vel_cms, accel_desired_cmss, accel_target_cmss, accel_cmss);
Write_PSCx(LOG_PSCN_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss);
}
void AC_PosControl::Write_PSCE(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss)
void AC_PosControl::Write_PSCE(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
{
Write_PSCx(LOG_PSCE_MSG, pos_desired_cm, pos_target_cm, pos_cm, vel_desired_cms, vel_target_cms, vel_cms, accel_desired_cmss, accel_target_cmss, accel_cmss);
Write_PSCx(LOG_PSCE_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss);
}
void AC_PosControl::Write_PSCD(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss)
void AC_PosControl::Write_PSCD(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
{
Write_PSCx(LOG_PSCD_MSG, pos_desired_cm, pos_target_cm, pos_cm, vel_desired_cms, vel_target_cms, vel_cms, accel_desired_cmss, accel_target_cmss, accel_cmss);
Write_PSCx(LOG_PSCD_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss);
}
// a convenience function for writing out the position controller offsets
void AC_PosControl::Write_PSOx(LogMessages id, float pos_target_offset_cm, float pos_offset_cm,
float vel_target_offset_cms, float vel_offset_cms,
float accel_target_offset_cmss, float accel_offset_cmss)
void AC_PosControl::Write_PSOx(LogMessages id, float pos_target_offset_m, float pos_offset_m,
float vel_target_offset_ms, float vel_offset_ms,
float accel_target_offset_mss, float accel_offset_mss)
{
const struct log_PSOx pkt{
LOG_PACKET_HEADER_INIT(id),
time_us : AP_HAL::micros64(),
pos_target_offset : pos_target_offset_cm * 0.01f,
pos_offset : pos_offset_cm * 0.01f,
vel_target_offset : vel_target_offset_cms * 0.01f,
vel_offset : vel_offset_cms * 0.01f,
accel_target_offset : accel_target_offset_cmss * 0.01f,
accel_offset : accel_offset_cmss * 0.01f,
pos_target_offset : pos_target_offset_m,
pos_offset : pos_offset_m,
vel_target_offset : vel_target_offset_ms,
vel_offset : vel_offset_ms,
accel_target_offset : accel_target_offset_mss,
accel_offset : accel_offset_mss,
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void AC_PosControl::Write_PSON(float pos_target_offset_cm, float pos_offset_cm,
float vel_target_offset_cms, float vel_offset_cms,
float accel_target_offset_cmss, float accel_offset_cmss)
void AC_PosControl::Write_PSON(float pos_target_offset_m, float pos_offset_m,
float vel_target_offset_ms, float vel_offset_ms,
float accel_target_offset_mss, float accel_offset_mss)
{
Write_PSOx(LOG_PSON_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);
Write_PSOx(LOG_PSON_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
}
void AC_PosControl::Write_PSOE(float pos_target_offset_cm, float pos_offset_cm,
float vel_target_offset_cms, float vel_offset_cms,
float accel_target_offset_cmss, float accel_offset_cmss)
void AC_PosControl::Write_PSOE(float pos_target_offset_m, float pos_offset_m,
float vel_target_offset_ms, float vel_offset_ms,
float accel_target_offset_mss, float accel_offset_mss)
{
Write_PSOx(LOG_PSOE_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);
Write_PSOx(LOG_PSOE_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
}
void AC_PosControl::Write_PSOD(float pos_target_offset_cm, float pos_offset_cm,
float vel_target_offset_cms, float vel_offset_cms,
float accel_target_offset_cmss, float accel_offset_cmss)
void AC_PosControl::Write_PSOD(float pos_target_offset_m, float pos_offset_m,
float vel_target_offset_ms, float vel_offset_ms,
float accel_target_offset_mss, float accel_offset_mss)
{
Write_PSOx(LOG_PSOD_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);
Write_PSOx(LOG_PSOD_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
}
void AC_PosControl::Write_PSOT(float pos_target_offset_cm, float pos_offset_cm,
float vel_target_offset_cms, float vel_offset_cms,
float accel_target_offset_cmss, float accel_offset_cmss)
void AC_PosControl::Write_PSOT(float pos_target_offset_m, float pos_offset_m,
float vel_target_offset_ms, float vel_offset_ms,
float accel_target_offset_mss, float accel_offset_mss)
{
Write_PSOx(LOG_PSOT_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);
Write_PSOx(LOG_PSOT_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
}
#endif // HAL_LOGGING_ENABLED