mirror of https://github.com/ArduPilot/ardupilot
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
Randy Mackay
parent
9ec1faf3f9
commit
c1dafae84a
|
|
@ -795,7 +795,8 @@ private:
|
|||
#if FRAME_CONFIG == HELI_FRAME
|
||||
void Log_Write_Heli(void);
|
||||
#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_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();
|
||||
|
|
|
|||
|
|
@ -330,8 +330,8 @@ void Copter::Log_Write_Heli()
|
|||
}
|
||||
#endif
|
||||
|
||||
// guided target logging
|
||||
struct PACKED log_GuidedTarget {
|
||||
// guided position target logging
|
||||
struct PACKED log_Guided_Position_Target {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t type;
|
||||
|
|
@ -347,14 +347,29 @@ struct PACKED log_GuidedTarget {
|
|||
float accel_target_z;
|
||||
};
|
||||
|
||||
// Write a Guided mode target
|
||||
// pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees
|
||||
// guided attitude target logging
|
||||
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
|
||||
// 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 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
|
||||
const log_Guided_Position_Target pkt {
|
||||
LOG_PACKET_HEADER_INIT(LOG_GUIDED_POSITION_TARGET_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
type : (uint8_t)target_type,
|
||||
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));
|
||||
}
|
||||
|
||||
// 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
|
||||
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
|
||||
// units and "Format characters" for field type information
|
||||
|
|
@ -491,8 +529,8 @@ const struct LogStructure Copter::log_structure[] = {
|
|||
{ LOG_SYSIDS_MSG, sizeof(log_SysIdS),
|
||||
"SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" , true },
|
||||
|
||||
// @LoggerMessage: GUID
|
||||
// @Description: Guided mode target information
|
||||
// @LoggerMessage: GUIP
|
||||
// @Description: Guided mode position target information
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: Type: Type of guided mode
|
||||
// @Field: pX: Target position, X-Axis
|
||||
|
|
@ -506,8 +544,24 @@ const struct LogStructure Copter::log_structure[] = {
|
|||
// @Field: aY: Target acceleration, Y-Axis
|
||||
// @Field: aZ: Target acceleration, Z-Axis
|
||||
|
||||
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
|
||||
"GUID", "QBfffbffffff", "TimeUS,Type,pX,pY,pZ,Terrain,vX,vY,vZ,aX,aY,aZ", "s-mmm-nnnooo", "F-BBB-BBBBBB" , true },
|
||||
{ LOG_GUIDED_POSITION_TARGET_MSG, sizeof(log_Guided_Position_Target),
|
||||
"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()
|
||||
|
|
@ -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_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
|
||||
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_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() {}
|
||||
|
|
|
|||
|
|
@ -118,9 +118,10 @@ enum LoggingParameters {
|
|||
LOG_DATA_FLOAT_MSG,
|
||||
LOG_PARAMTUNE_MSG,
|
||||
LOG_HELI_MSG,
|
||||
LOG_GUIDEDTARGET_MSG,
|
||||
LOG_GUIDED_POSITION_TARGET_MSG,
|
||||
LOG_SYSIDD_MSG,
|
||||
LOG_SYSIDS_MSG,
|
||||
LOG_GUIDED_ATTITUDE_TARGET_MSG
|
||||
};
|
||||
|
||||
#define MASK_LOG_ATTITUDE_FAST (1<<0)
|
||||
|
|
|
|||
|
|
@ -324,7 +324,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
|
|||
wp_nav->set_wp_destination(destination, terrain_alt);
|
||||
|
||||
// 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;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -364,7 +364,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
|
|||
update_time_ms = millis();
|
||||
|
||||
// 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;
|
||||
|
||||
|
|
@ -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);
|
||||
|
||||
// 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;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -465,7 +465,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
|
|||
update_time_ms = millis();
|
||||
|
||||
// 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;
|
||||
|
||||
|
|
@ -492,7 +492,7 @@ void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw
|
|||
|
||||
// log target
|
||||
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
|
||||
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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -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);
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode,
|
||||
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});
|
||||
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);
|
||||
}
|
||||
|
||||
// takeoff_run - takeoff in guided mode
|
||||
|
|
|
|||
Loading…
Reference in New Issue