Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: log guided mode targets

Browse Source
This commit is contained in:
Randy Mackay 2016-01-11 11:41:28 +09:00
parent d5e7b321f6
commit 89c147184f
4 changed files with 49 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -664,6 +664,7 @@ private:
void Log_Write_Heli(void);
#endif
void Log_Write_Precland();
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_Vehicle_Startup_Messages();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void start_logging() ;

33
ArduCopter/Log.cpp
View File

@ -719,6 +719,36 @@ void Copter::Log_Write_Precland()
#endif // PRECISION_LANDING == ENABLED
}
// precision landing logging
struct PACKED log_GuidedTarget {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t type;
float pos_target_x;
float pos_target_y;
float pos_target_z;
float vel_target_x;
float vel_target_y;
float vel_target_z;
};
// Write a Guided mode target
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
{
struct log_GuidedTarget pkt = {
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
time_us : AP_HAL::micros64(),
type : target_type,
pos_target_x : pos_target.x,
pos_target_y : pos_target.y,
pos_target_z : pos_target.z,
vel_target_x : vel_target.x,
vel_target_y : vel_target.y,
vel_target_z : vel_target.z
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
const struct LogStructure Copter::log_structure[] = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
@ -761,6 +791,8 @@ const struct LogStructure Copter::log_structure[] = {
"HELI", "Qhh", "TimeUS,DRRPM,ERRPM" },
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
};
#if CLI_ENABLED == ENABLED
@ -854,6 +886,7 @@ void Copter::Log_Write_Baro(void) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Write_Home_And_Origin() {}
void Copter::Log_Sensor_Health() {}
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {};
#if FRAME_CONFIG == HELI_FRAME
void Copter::Log_Write_Heli() {}

14
ArduCopter/control_guided.cpp
View File

@ -164,6 +164,9 @@ void Copter::guided_set_destination(const Vector3f& destination)
}
wp_nav.set_wp_destination(destination);
// log target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
}
// guided_set_velocity - sets guided mode's target velocity
@ -178,6 +181,9 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
// set guided mode posvel target
@ -192,6 +198,9 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve
posvel_vel_target_cms = velocity;
pos_control.set_pos_target(posvel_pos_target_cm);
// log target
Log_Write_GuidedTarget(guided_mode, destination, velocity);
}
// set guided mode angle target
@ -210,6 +219,11 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// log target
Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
}
// guided_run - runs the guided controller

1
ArduCopter/defines.h
View File

@ -256,6 +256,7 @@ enum FlipState {
#define LOG_PARAMTUNE_MSG 0x1F
#define LOG_HELI_MSG 0x20
#define LOG_PRECLAND_MSG 0x21
#define LOG_GUIDEDTARGET_MSG 0x22
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)