aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Logger: move AP_AHRS functions out

Browse Source
This commit is contained in:
Josh Henderson 2021-01-08 22:45:38 -05:00 committed by Peter Barker
parent 62ff9a4d4b
commit 0ebbf0f74e
3 changed files with 4 additions and 302 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Logger/AP_Logger.h
View File

@ -311,8 +311,6 @@ public:
void Write_Rally();
void Write_Baro();
void Write_Power(void);
void Write_AHRS2();
void Write_POS();
void Write_Radio(const mavlink_radio_t &packet);
void Write_Message(const char *message);
void Write_MessageF(const char *fmt, ...);
@ -322,8 +320,6 @@ public:
void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t esc_temp, uint16_t current_tot, int16_t motor_temp, float error_rate = 0.0f);
void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct);
void Write_ESCStatus(uint64_t time_us, uint8_t id, uint32_t error_count, float voltage, float current, float temperature, int32_t rpm, uint8_t power_pct);
void Write_Attitude(const Vector3f &targets);
void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets);
void Write_Current();
void Write_Compass();
void Write_Mode(uint8_t mode, const ModeReason reason);
@ -332,19 +328,13 @@ public:
void Write_Command(const mavlink_command_int_t &packet, MAV_RESULT result, bool was_command_long=false);
void Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd);
void Write_Origin(uint8_t origin_type, const Location &loc);
void Write_RPM(const AP_RPM &rpm_sensor);
void Write_Rate(const AP_AHRS_View *ahrs,
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control);
void Write_RallyPoint(uint8_t total,
uint8_t sequence,
const RallyLocation &rally_point);
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored);
void Write_AOA_SSA(AP_AHRS &ahrs);
void Write_Beacon(AP_Beacon &beacon);
void Write_Proximity(AP_Proximity &proximity);
void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);

145
libraries/AP_Logger/LogFile.cpp
View File

@ -7,9 +7,6 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <AP_Motors/AP_Motors.h>
#include <AC_AttitudeControl/AC_AttitudeControl.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AP_RSSI/AP_RSSI.h>
#include <AP_GPS/AP_GPS.h>
@ -447,56 +444,6 @@ void AP_Logger::Write_Power(void)
#endif
}
// Write an AHRS2 packet
void AP_Logger::Write_AHRS2()
{
const AP_AHRS &ahrs = AP::ahrs();
Vector3f euler;
struct Location loc;
Quaternion quat;
if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc) || !ahrs.get_secondary_quaternion(quat)) {
return;
}
const struct log_AHRS pkt{
LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
time_us : AP_HAL::micros64(),
roll : (int16_t)(degrees(euler.x)*100),
pitch : (int16_t)(degrees(euler.y)*100),
yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)),
alt : loc.alt*1.0e-2f,
lat : loc.lat,
lng : loc.lng,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4,
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write a POS packet
void AP_Logger::Write_POS()
{
const AP_AHRS &ahrs = AP::ahrs();
Location loc;
if (!ahrs.get_position(loc)) {
return;
}
float home, origin;
ahrs.get_relative_position_D_home(home);
const struct log_POS pkt{
LOG_PACKET_HEADER_INIT(LOG_POS_MSG),
time_us : AP_HAL::micros64(),
lat : loc.lat,
lng : loc.lng,
alt : loc.alt*1.0e-2f,
rel_home_alt : -home,
rel_origin_alt : ahrs.get_relative_position_D_origin(origin) ? -origin : quiet_nanf(),
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_Radio(const mavlink_radio_t &packet)
{
const struct log_Radio pkt{
@ -562,45 +509,6 @@ void AP_Logger::Write_Trigger(const Location &current_loc)
Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, 0);
}
// Write an attitude packet
void AP_Logger::Write_Attitude(const Vector3f &targets)
{
const AP_AHRS &ahrs = AP::ahrs();
const struct log_Attitude pkt{
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
time_us : AP_HAL::micros64(),
control_roll : (int16_t)targets.x,
roll : (int16_t)ahrs.roll_sensor,
control_pitch : (int16_t)targets.y,
pitch : (int16_t)ahrs.pitch_sensor,
control_yaw : (uint16_t)wrap_360_cd(targets.z),
yaw : (uint16_t)wrap_360_cd(ahrs.yaw_sensor),
error_rp : (uint16_t)(ahrs.get_error_rp() * 100),
error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100),
active : ahrs.get_active_AHRS_type(),
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write an attitude packet
void AP_Logger::Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets)
{
const struct log_Attitude pkt{
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
time_us : AP_HAL::micros64(),
control_roll : (int16_t)targets.x,
roll : (int16_t)ahrs.roll_sensor,
control_pitch : (int16_t)targets.y,
pitch : (int16_t)ahrs.pitch_sensor,
control_yaw : (uint16_t)wrap_360_cd(targets.z),
yaw : (uint16_t)wrap_360_cd(ahrs.yaw_sensor),
error_rp : (uint16_t)(ahrs.get_error_rp() * 100),
error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100)
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_Current_instance(const uint64_t time_us,
const uint8_t battery_instance)
{
@ -795,19 +703,6 @@ void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info)
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_Origin(uint8_t origin_type, const Location &loc)
{
const struct log_ORGN pkt{
LOG_PACKET_HEADER_INIT(LOG_ORGN_MSG),
time_us : AP_HAL::micros64(),
origin_type : origin_type,
latitude : loc.lat,
longitude : loc.lng,
altitude : loc.alt
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor)
{
float rpm1 = -1, rpm2 = -1;
@ -824,33 +719,6 @@ void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor)
WriteBlock(&pkt, sizeof(pkt));
}
// Write a rate packet
void AP_Logger::Write_Rate(const AP_AHRS_View *ahrs,
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control)
{
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target();
const struct log_Rate pkt_rate{
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : AP_HAL::micros64(),
control_roll : degrees(rate_targets.x),
roll : degrees(ahrs->get_gyro().x),
roll_out : motors.get_roll(),
control_pitch : degrees(rate_targets.y),
pitch : degrees(ahrs->get_gyro().y),
pitch_out : motors.get_pitch(),
control_yaw : degrees(rate_targets.z),
yaw : degrees(ahrs->get_gyro().z),
yaw_out : motors.get_yaw(),
control_accel : (float)accel_target.z,
accel : (float)(-(ahrs->get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
accel_out : motors.get_throttle()
};
WriteBlock(&pkt_rate, sizeof(pkt_rate));
}
// Write visual odometry sensor data
void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
{
@ -909,19 +777,6 @@ void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms,
WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
}
// Write AOA and SSA
void AP_Logger::Write_AOA_SSA(AP_AHRS &ahrs)
{
const struct log_AOA_SSA aoa_ssa{
LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),
time_us : AP_HAL::micros64(),
AOA : ahrs.getAOA(),
SSA : ahrs.getSSA()
};
WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
}
// Write beacon sensor (position) data
void AP_Logger::Write_Beacon(AP_Beacon &beacon)
{

151
libraries/AP_Logger/LogStructure.h
View File

@ -121,6 +121,8 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AP_NavEKF2/LogStructure.h>
#include <AP_NavEKF3/LogStructure.h>
#include <AP_AHRS/LogStructure.h>
// structure used to define logging format
struct LogStructure {
uint8_t msg_type;
@ -374,28 +376,6 @@ struct PACKED log_Optflow {
float body_y;
};
struct PACKED log_AHRS {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t roll;
int16_t pitch;
uint16_t yaw;
float alt;
int32_t lat;
int32_t lng;
float q1, q2, q3, q4;
};
struct PACKED log_POS {
LOG_PACKET_HEADER;
uint64_t time_us;
int32_t lat;
int32_t lng;
float alt;
float rel_home_alt;
float rel_origin_alt;
};
struct PACKED log_POWR {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -514,20 +494,6 @@ struct PACKED log_Camera {
uint16_t yaw;
};
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t control_roll;
int16_t roll;
int16_t control_pitch;
int16_t pitch;
uint16_t control_yaw;
uint16_t yaw;
uint16_t error_rp;
uint16_t error_yaw;
uint8_t active;
};
struct PACKED log_PID {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -791,15 +757,6 @@ struct PACKED log_MAV_Stats {
// uint8_t state_retry_max;
};
struct PACKED log_ORGN {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t origin_type;
int32_t latitude;
int32_t longitude;
int32_t altitude;
};
struct PACKED log_RPM {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -807,23 +764,6 @@ struct PACKED log_RPM {
float rpm2;
};
struct PACKED log_Rate {
LOG_PACKET_HEADER;
uint64_t time_us;
float control_roll;
float roll;
float roll_out;
float control_pitch;
float pitch;
float pitch_out;
float control_yaw;
float yaw;
float yaw_out;
float control_accel;
float accel;
float accel_out;
};
struct PACKED log_SbpLLH {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -887,13 +827,6 @@ struct PACKED log_Rally {
int16_t altitude;
};
struct PACKED log_AOA_SSA {
LOG_PACKET_HEADER;
uint64_t time_us;
float AOA;
float SSA;
};
struct PACKED log_Beacon {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1097,20 +1030,6 @@ struct PACKED log_PSC {
// @Field: Ver_vel: Vehicle vertical velocity
// @Field: Squark: Transponder squawk code
// @LoggerMessage: AHR2
// @Description: Backup AHRS data
// @Field: TimeUS: Time since system startup
// @Field: Roll: Estimated roll
// @Field: Pitch: Estimated pitch
// @Field: Yaw: Estimated yaw
// @Field: Alt: Estimated altitude
// @Field: Lat: Estimated latitude
// @Field: Lng: Estimated longitude
// @Field: Q1: Estimated attitude quaternion component 1
// @Field: Q2: Estimated attitude quaternion component 2
// @Field: Q3: Estimated attitude quaternion component 3
// @Field: Q4: Estimated attitude quaternion component 4
// @LoggerMessage: ARM
// @Description: Arming status changes
// @Field: TimeUS: Time since system startup
@ -1133,19 +1052,6 @@ struct PACKED log_PSC {
// @Field: Hfp: Probability sensor has failed
// @Field: Pri: True if sensor is the primary sensor
// @LoggerMessage: ATT
// @Description: Canonical vehicle attitude
// @Field: TimeUS: Time since system startup
// @Field: DesRoll: vehicle desired roll
// @Field: Roll: achieved vehicle roll
// @Field: DesPitch: vehicle desired pitch
// @Field: Pitch: achieved vehicle pitch
// @Field: DesYaw: vehicle desired yaw
// @Field: Yaw: achieved vehicle yaw
// @Field: ErrRP: lowest estimated gyro drift error
// @Field: ErrYaw: difference between measured yaw and DCM yaw estimate
// @Field: AEKF: active EKF type
// @LoggerMessage: BARO
// @Description: Gathered Barometer data
// @Field: TimeUS: Time since system startup
@ -1553,14 +1459,6 @@ struct PACKED log_PSC {
// @Field: bodyX: derived velocity, X-axis
// @Field: bodyY: derived velocity, Y-axis
// @LoggerMessage: ORGN
// @Description: Vehicle navigation origin or other notable position
// @Field: TimeUS: Time since system startup
// @Field: Type: Position type
// @Field: Lat: Position latitude
// @Field: Lng: Position longitude
// @Field: Alt: Position altitude
// @LoggerMessage: PARM
// @Description: parameter value
// @Field: TimeUS: Time since system startup
@ -1596,15 +1494,6 @@ struct PACKED log_PSC {
// @Field: I2CI: Number of i2c interrupts serviced
// @Field: Ex: number of microseconds being added to each loop to address scheduler overruns
// @LoggerMessage: POS
// @Description: Canonical vehicle position
// @Field: TimeUS: Time since system startup
// @Field: Lat: Canonical vehicle latitude
// @Field: Lng: Canonical vehicle longitude
// @Field: Alt: Canonical vehicle altitude
// @Field: RelHomeAlt: Canonical vehicle altitude relative to home
// @Field: RelOriginAlt: Canonical vehicle altitude relative to navigation origin
// @LoggerMessage: POWR
// @Description: System power information
// @Field: TimeUS: Time since system startup
@ -1650,24 +1539,6 @@ struct PACKED log_PSC {
// @Field: Lng: longitude of rally point
// @Field: Alt: altitude of rally point
// @LoggerMessage: RATE
// @Description: Desired and achieved vehicle attitude rates
// @Field: TimeUS: Time since system startup
// @Field: RDes: vehicle desired roll rate
// @Field: R: achieved vehicle roll rate
// @Field: ROut: normalized output for Roll
// @Field: PDes: vehicle desired pitch rate
// @Field: P: vehicle pitch rate
// @Field: POut: normalized output for Pitch
// @Field: YDes: vehicle desired yaw rate
// @Field: Y: achieved vehicle yaw rate
// @Field: YOut: normalized output for Yaw
// @Field: YDes: vehicle desired yaw rate
// @Field: Y: achieved vehicle yaw rate
// @Field: ADes: desired vehicle vertical acceleration
// @Field: A: achieved vehicle vertical acceleration
// @Field: AOut: percentage of vertical thrust output current being used
// @LoggerMessage: RCIN
// @Description: RC input channels to vehicle
// @Field: TimeUS: Time since system startup
@ -1936,8 +1807,6 @@ struct PACKED log_PSC {
"BAT", "QBfffffcf", "TimeUS,Instance,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res", "s#vvAiJOw", "F-000!/?0" }, \
{ LOG_CURRENT_CELLS_MSG, sizeof(log_Current_Cells), \
"BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" }, \
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
"ATT", "QccccCCCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw,AEKF", "sddddhhdh-", "FBBBBBBBB-" }, \
{ LOG_MAG_MSG, sizeof(log_MAG), \
"MAG", "QBhhhhhhhhhBI", "TimeUS,I,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOX,MOY,MOZ,Health,S", "s#GGGGGGGGG-s", "F-CCCCCCCCC-F" }, \
{ LOG_MODE_MSG, sizeof(log_Mode), \
@ -1960,10 +1829,6 @@ struct PACKED log_PSC {
"OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
{ LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \
"SA", "QBffffB","TimeUS,State,DVelX,DVelY,MVelX,MVelY,Back", "sbnnnnb", "F------"}, \
{ LOG_AHR2_MSG, sizeof(log_AHRS), \
"AHR2","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4","sddhmDU????", "FBBB0GG????" }, \
{ LOG_POS_MSG, sizeof(log_POS), \
"POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" }, \
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
@ -2006,17 +1871,14 @@ struct PACKED log_PSC {
"ISBH",ISBH_FMT,ISBH_LABELS,ISBH_UNITS,ISBH_MULTS }, \
{ LOG_ISBD_MSG, sizeof(log_ISBD), \
"ISBD",ISBD_FMT,ISBD_LABELS, ISBD_UNITS, ISBD_MULTS }, \
{ LOG_ORGN_MSG, sizeof(log_ORGN), \
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s-DUm", "F-GGB" }, \
LOG_STRUCTURE_FROM_DAL \
LOG_STRUCTURE_FROM_NAVEKF2 \
LOG_STRUCTURE_FROM_NAVEKF3 \
LOG_STRUCTURE_FROM_AHRS \
{ LOG_DF_FILE_STATS, sizeof(log_DSF), \
"DSF", "QIHIIII", "TimeUS,Dp,Blk,Bytes,FMn,FMx,FAv", "s--b---", "F--0---" }, \
{ LOG_RPM_MSG, sizeof(log_RPM), \
"RPM", "Qff", "TimeUS,rpm1,rpm2", "sqq", "F00" }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut", "skk-kk-kk-oo-", "F?????????BB-" }, \
{ LOG_RALLY_MSG, sizeof(log_Rally), \
"RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" }, \
{ LOG_MAV_MSG, sizeof(log_MAV), \
@ -2090,7 +1952,7 @@ enum LogMessages : uint8_t {
LOG_RSSI_MSG,
LOG_BARO_MSG,
LOG_POWR_MSG,
LOG_AHR2_MSG,
LOG_IDS_FROM_AHRS,
LOG_SIMSTATE_MSG,
LOG_CMD_MSG,
LOG_MAVLINK_COMMAND_MSG,
@ -2104,7 +1966,6 @@ enum LogMessages : uint8_t {
LOG_CSRV_MSG,
LOG_CESC_MSG,
LOG_ARSP_MSG,
LOG_ATTITUDE_MSG,
LOG_CURRENT_MSG,
LOG_CURRENT_CELLS_MSG,
LOG_MAG_MSG,
@ -2121,7 +1982,6 @@ enum LogMessages : uint8_t {
LOG_GPS_RAWS_MSG,
LOG_ACC_MSG,
LOG_GYR_MSG,
LOG_POS_MSG,
LOG_PIDR_MSG,
LOG_PIDP_MSG,
LOG_PIDY_MSG,
@ -2129,7 +1989,6 @@ enum LogMessages : uint8_t {
LOG_PIDS_MSG,
LOG_DSTL_MSG,
LOG_VIBE_MSG,
LOG_ORGN_MSG,
LOG_RPM_MSG,
LOG_GPA_MSG,
LOG_RFND_MSG,
@ -2148,11 +2007,9 @@ enum LogMessages : uint8_t {
LOG_MSG_SBPEVENT,
LOG_TRIGGER_MSG,
LOG_RATE_MSG,
LOG_RALLY_MSG,
LOG_VISUALODOM_MSG,
LOG_VISUALPOS_MSG,
LOG_AOA_SSA_MSG,
LOG_BEACON_MSG,
LOG_PROXIMITY_MSG,
LOG_DF_FILE_STATS,