Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: separate logging for position and attitude targets in guided mode 

We now log position and attitude targets in guided mode separately. Earlier we were using same messages for both which was causing some confusion in field names
This commit is contained in:
Shiv Tyagi 2022-01-25 21:58:04 +05:30 committed by Randy Mackay
parent 9ec1faf3f9
commit c1dafae84a
4 changed files with 79 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Copter.h
View File

@ -795,7 +795,8 @@ private:
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void); void Log_Write_Heli(void);
#endif #endif
void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target); void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target);
void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate);
void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
void Log_Write_Vehicle_Startup_Messages(); void Log_Write_Vehicle_Startup_Messages();

79
ArduCopter/Log.cpp
View File

@ -330,8 +330,8 @@ void Copter::Log_Write_Heli()
} }
#endif #endif
// guided target logging // guided position target logging
struct PACKED log_GuidedTarget { struct PACKED log_Guided_Position_Target {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
uint8_t type; uint8_t type;
@ -347,14 +347,29 @@ struct PACKED log_GuidedTarget {
float accel_target_z; float accel_target_z;
}; };
// Write a Guided mode target // guided attitude target logging
// pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees struct PACKED log_Guided_Attitude_Target {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t type;
float roll;
float pitch;
float yaw;
float roll_rate;
float pitch_rate;
float yaw_rate;
float thrust;
float climb_rate;
};
// Write a Guided mode position target
// pos_target is lat, lon, alt OR offset from ekf origin in cm
// terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain // terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain
// vel_target is cm/s // vel_target is cm/s
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) void Copter::Log_Write_Guided_Position_Target(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target)
{ {
struct log_GuidedTarget pkt = { const log_Guided_Position_Target pkt {
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), LOG_PACKET_HEADER_INIT(LOG_GUIDED_POSITION_TARGET_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
type : (uint8_t)target_type, type : (uint8_t)target_type,
pos_target_x : pos_target.x, pos_target_x : pos_target.x,
@ -371,6 +386,29 @@ void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vecto
logger.WriteBlock(&pkt, sizeof(pkt)); logger.WriteBlock(&pkt, sizeof(pkt));
} }
// Write a Guided mode attitude target
// roll, pitch and yaw are in radians
// ang_vel: angular velocity, [roll rate, pitch_rate, yaw_rate] in radians/sec
// thrust is between 0 to 1
// climb_rate is in (m/s)
void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate)
{
const log_Guided_Attitude_Target pkt {
LOG_PACKET_HEADER_INIT(LOG_GUIDED_ATTITUDE_TARGET_MSG),
time_us : AP_HAL::micros64(),
type : (uint8_t)target_type,
roll : degrees(roll), // rad to deg
pitch : degrees(pitch), // rad to deg
yaw : degrees(yaw), // rad to deg
roll_rate : degrees(ang_vel.x), // rad/s to deg/s
pitch_rate : degrees(ang_vel.y), // rad/s to deg/s
yaw_rate : degrees(ang_vel.z), // rad/s to deg/s
thrust : thrust,
climb_rate : climb_rate
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
// type and unit information can be found in // type and unit information can be found in
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
// units and "Format characters" for field type information // units and "Format characters" for field type information
@ -491,8 +529,8 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_SYSIDS_MSG, sizeof(log_SysIdS), { LOG_SYSIDS_MSG, sizeof(log_SysIdS),
"SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" , true }, "SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" , true },
// @LoggerMessage: GUID // @LoggerMessage: GUIP
// @Description: Guided mode target information // @Description: Guided mode position target information
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: Type: Type of guided mode // @Field: Type: Type of guided mode
// @Field: pX: Target position, X-Axis // @Field: pX: Target position, X-Axis
@ -506,8 +544,24 @@ const struct LogStructure Copter::log_structure[] = {
// @Field: aY: Target acceleration, Y-Axis // @Field: aY: Target acceleration, Y-Axis
// @Field: aZ: Target acceleration, Z-Axis // @Field: aZ: Target acceleration, Z-Axis
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), { LOG_GUIDED_POSITION_TARGET_MSG, sizeof(log_Guided_Position_Target),
"GUID", "QBfffbffffff", "TimeUS,Type,pX,pY,pZ,Terrain,vX,vY,vZ,aX,aY,aZ", "s-mmm-nnnooo", "F-BBB-BBBBBB" , true }, "GUIP", "QBfffbffffff", "TimeUS,Type,pX,pY,pZ,Terrain,vX,vY,vZ,aX,aY,aZ", "s-mmm-nnnooo", "F-BBB-BBBBBB" , true },
// @LoggerMessage: GUIA
// @Description: Guided mode attitude target information
// @Field: TimeUS: Time since system startup
// @Field: Type: Type of guided mode
// @Field: Roll: Target attitude, Roll
// @Field: Pitch: Target attitude, Pitch
// @Field: Yaw: Target attitude, Yaw
// @Field: RollRt: Roll rate
// @Field: PitchRt: Pitch rate
// @Field: YawRt: Yaw rate
// @Field: Thrust: Thrust
// @Field: ClimbRt: Climb rate
{ LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(log_Guided_Attitude_Target),
"GUIA", "QBffffffff", "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true },
}; };
void Copter::Log_Write_Vehicle_Startup_Messages() void Copter::Log_Write_Vehicle_Startup_Messages()
@ -538,7 +592,8 @@ void Copter::Log_Write_Data(LogDataID id, uint16_t value) {}
void Copter::Log_Write_Data(LogDataID id, float value) {} void Copter::Log_Write_Data(LogDataID id, float value) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
void Copter::Log_Sensor_Health() {} void Copter::Log_Sensor_Health() {}
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) {} void Copter::Log_Write_Guided_Position_Target(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) {}
void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate) {}
void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {}
void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {}
void Copter::Log_Write_Vehicle_Startup_Messages() {} void Copter::Log_Write_Vehicle_Startup_Messages() {}

3
ArduCopter/defines.h
View File

@ -118,9 +118,10 @@ enum LoggingParameters {
LOG_DATA_FLOAT_MSG, LOG_DATA_FLOAT_MSG,
LOG_PARAMTUNE_MSG, LOG_PARAMTUNE_MSG,
LOG_HELI_MSG, LOG_HELI_MSG,
LOG_GUIDEDTARGET_MSG, LOG_GUIDED_POSITION_TARGET_MSG,
LOG_SYSIDD_MSG, LOG_SYSIDD_MSG,
LOG_SYSIDS_MSG, LOG_SYSIDS_MSG,
LOG_GUIDED_ATTITUDE_TARGET_MSG
}; };
#define MASK_LOG_ATTITUDE_FAST (1<<0) #define MASK_LOG_ATTITUDE_FAST (1<<0)

19
ArduCopter/mode_guided.cpp
View File

@ -324,7 +324,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
wp_nav->set_wp_destination(destination, terrain_alt); wp_nav->set_wp_destination(destination, terrain_alt);
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, destination, terrain_alt, Vector3f(), Vector3f()); copter.Log_Write_Guided_Position_Target(guided_mode, destination, terrain_alt, Vector3f(), Vector3f());
send_notification = true; send_notification = true;
return true; return true;
} }
@ -364,7 +364,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
update_time_ms = millis(); update_time_ms = millis();
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
send_notification = true; send_notification = true;
@ -419,7 +419,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f()); copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f());
send_notification = true; send_notification = true;
return true; return true;
} }
@ -465,7 +465,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
update_time_ms = millis(); update_time_ms = millis();
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
send_notification = true; send_notification = true;
@ -492,7 +492,7 @@ void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw
// log target // log target
if (log_request) { if (log_request) {
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
} }
} }
@ -522,7 +522,7 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera
// log target // log target
if (log_request) { if (log_request) {
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
} }
} }
@ -560,7 +560,7 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
guided_accel_target_cmss = acceleration; guided_accel_target_cmss = acceleration;
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
return true; return true;
} }
@ -621,10 +621,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad); attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
Vector3f{ToDeg(roll_rad), ToDeg(pitch_rad), ToDeg(yaw_rad)} * 100.0,
false,
ang_vel, Vector3f{0.0, 0.0, climb_rate_cms_or_thrust});
} }
// takeoff_run - takeoff in guided mode // takeoff_run - takeoff in guided mode