mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
AP_AHRS: move logging functions to AHRS
This commit is contained in:
Josh Henderson
Peter Barker
parent
0052e4d528
commit
62ff9a4d4b
@ -475,12 +475,12 @@ void AP_AHRS::Log_Write_Home_And_Origin()
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
Location ekf_orig;
|
||||
if (get_origin(ekf_orig)) {
|
||||
logger->Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
||||
Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (home_is_set()) {
|
||||
logger->Write_Origin(LogOriginType::ahrs_home, _home);
|
||||
Write_Origin(LogOriginType::ahrs_home, _home);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -610,6 +610,13 @@ public:
|
||||
// for holding parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// Logging to disk functions
|
||||
void Write_AHRS2(void) const;
|
||||
void Write_AOA_SSA(void); // should be const? but it calls update functions
|
||||
void Write_Attitude(const Vector3f &targets) const;
|
||||
void Write_Origin(uint8_t origin_type, const Location &loc) const;
|
||||
void Write_POS(void) const;
|
||||
|
||||
protected:
|
||||
void update_nmea_out();
|
||||
|
||||
|
||||
141
libraries/AP_AHRS/AP_AHRS_Logging.cpp
Normal file
141
libraries/AP_AHRS/AP_AHRS_Logging.cpp
Normal file
@ -0,0 +1,141 @@
|
||||
#include "AP_AHRS.h"
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl.h>
|
||||
#include <AC_AttitudeControl/AC_PosControl.h>
|
||||
|
||||
|
||||
// Write an AHRS2 packet
|
||||
void AP_AHRS::Write_AHRS2() const
|
||||
{
|
||||
Vector3f euler;
|
||||
struct Location loc;
|
||||
Quaternion quat;
|
||||
if (!get_secondary_attitude(euler) || !get_secondary_position(loc) || !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,
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write AOA and SSA
|
||||
void AP_AHRS::Write_AOA_SSA(void)
|
||||
{
|
||||
const struct log_AOA_SSA aoa_ssa{
|
||||
LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
AOA : getAOA(),
|
||||
SSA : getSSA()
|
||||
};
|
||||
|
||||
AP::logger().WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
|
||||
}
|
||||
|
||||
// Write an attitude packet
|
||||
void AP_AHRS::Write_Attitude(const Vector3f &targets) const
|
||||
{
|
||||
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)roll_sensor,
|
||||
control_pitch : (int16_t)targets.y,
|
||||
pitch : (int16_t)pitch_sensor,
|
||||
control_yaw : (uint16_t)wrap_360_cd(targets.z),
|
||||
yaw : (uint16_t)wrap_360_cd(yaw_sensor),
|
||||
error_rp : (uint16_t)(get_error_rp() * 100),
|
||||
error_yaw : (uint16_t)(get_error_yaw() * 100),
|
||||
active : get_active_AHRS_type(),
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void AP_AHRS::Write_Origin(uint8_t origin_type, const Location &loc) const
|
||||
{
|
||||
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
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write a POS packet
|
||||
void AP_AHRS::Write_POS() const
|
||||
{
|
||||
Location loc;
|
||||
if (!get_position(loc)) {
|
||||
return;
|
||||
}
|
||||
float home, origin;
|
||||
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 : get_relative_position_D_origin(origin) ? -origin : AP::logger().quiet_nanf(),
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write an attitude view packet
|
||||
void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
|
||||
{
|
||||
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)roll_sensor,
|
||||
control_pitch : (int16_t)targets.y,
|
||||
pitch : (int16_t)pitch_sensor,
|
||||
control_yaw : (uint16_t)wrap_360_cd(targets.z),
|
||||
yaw : (uint16_t)wrap_360_cd(yaw_sensor),
|
||||
error_rp : (uint16_t)(get_error_rp() * 100),
|
||||
error_yaw : (uint16_t)(get_error_yaw() * 100)
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write a rate packet
|
||||
void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
|
||||
const AC_PosControl &pos_control) const
|
||||
{
|
||||
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(get_gyro().x),
|
||||
roll_out : motors.get_roll(),
|
||||
control_pitch : degrees(rate_targets.y),
|
||||
pitch : degrees(get_gyro().y),
|
||||
pitch_out : motors.get_pitch(),
|
||||
control_yaw : degrees(rate_targets.z),
|
||||
yaw : degrees(get_gyro().z),
|
||||
yaw_out : motors.get_yaw(),
|
||||
control_accel : (float)accel_target.z,
|
||||
accel : (float)(-(get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
|
||||
accel_out : motors.get_throttle()
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
|
||||
}
|
||||
@ -21,6 +21,11 @@
|
||||
*/
|
||||
|
||||
#include "AP_AHRS.h"
|
||||
#include <AP_Motors/AP_Motors.h>
|
||||
|
||||
// fwd declarations to avoid include errors
|
||||
class AC_AttitudeControl;
|
||||
class AC_PosControl;
|
||||
|
||||
class AP_AHRS_View
|
||||
{
|
||||
@ -172,6 +177,11 @@ public:
|
||||
return ahrs.get_error_yaw();
|
||||
}
|
||||
|
||||
// Logging Functions
|
||||
void Write_AttitudeView(const Vector3f &targets) const;
|
||||
void Write_Rate( const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
|
||||
const AC_PosControl &pos_control) const;
|
||||
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
|
||||
157
libraries/AP_AHRS/LogStructure.h
Normal file
157
libraries/AP_AHRS/LogStructure.h
Normal file
@ -0,0 +1,157 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_Logger/LogStructure.h>
|
||||
|
||||
#define LOG_IDS_FROM_AHRS \
|
||||
LOG_AHR2_MSG, \
|
||||
LOG_AOA_SSA_MSG, \
|
||||
LOG_ATTITUDE_MSG, \
|
||||
LOG_ORGN_MSG, \
|
||||
LOG_POS_MSG, \
|
||||
LOG_RATE_MSG
|
||||
|
||||
// @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
|
||||
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;
|
||||
};
|
||||
|
||||
// @LoggerMessage: AOA
|
||||
// @Description: Angle of attack and Side Slip Angle values
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: AOA: Angle of Attack calculated from airspeed, wind vector,velocity vector
|
||||
// @Field: SSA: Side Slip Angle calculated from airspeed, wind vector,velocity vector
|
||||
struct PACKED log_AOA_SSA {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
float AOA;
|
||||
float SSA;
|
||||
};
|
||||
|
||||
// @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
|
||||
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;
|
||||
};
|
||||
|
||||
// @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
|
||||
struct PACKED log_ORGN {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t origin_type;
|
||||
int32_t latitude;
|
||||
int32_t longitude;
|
||||
int32_t altitude;
|
||||
};
|
||||
|
||||
// @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
|
||||
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;
|
||||
};
|
||||
|
||||
// @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
|
||||
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;
|
||||
};
|
||||
|
||||
|
||||
#define LOG_STRUCTURE_FROM_AHRS \
|
||||
{ LOG_AHR2_MSG, sizeof(log_AHRS), \
|
||||
"AHR2","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4","sddhmDU????", "FBBB0GG????" }, \
|
||||
{ LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \
|
||||
"AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" }, \
|
||||
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
|
||||
"ATT", "QccccCCCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw,AEKF", "sddddhhdh-", "FBBBBBBBB-" }, \
|
||||
{ LOG_ORGN_MSG, sizeof(log_ORGN), \
|
||||
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s-DUm", "F-GGB" }, \
|
||||
{ LOG_POS_MSG, sizeof(log_POS), \
|
||||
"POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" }, \
|
||||
{ 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-" },
|
||||
Loading…
Reference in New Issue
Block a user