mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Copter: add throw mode logging
This commit is contained in:
parent
8c5c8eec44
commit
f4f13bbe6b
@ -277,8 +277,10 @@ private:
|
||||
// throw mode state
|
||||
struct {
|
||||
ThrowModeStage stage;
|
||||
ThrowModeStage prev_stage;
|
||||
uint32_t last_log_ms;
|
||||
bool nextmode_attempted;
|
||||
} throw_state = {Throw_Disarmed, false};
|
||||
} throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false};
|
||||
|
||||
uint32_t precland_last_update_ms;
|
||||
|
||||
@ -705,6 +707,7 @@ private:
|
||||
#endif
|
||||
void Log_Write_Precland();
|
||||
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
|
||||
void Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool position_ok);
|
||||
void Log_Write_Vehicle_Startup_Messages();
|
||||
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
|
||||
void start_logging() ;
|
||||
|
@ -714,6 +714,40 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// precision landing logging
|
||||
struct PACKED log_Throw {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t stage;
|
||||
float velocity;
|
||||
float velocity_z;
|
||||
float accel;
|
||||
float ef_accel_z;
|
||||
uint8_t throw_detect;
|
||||
uint8_t attitude_ok;
|
||||
uint8_t height_ok;
|
||||
uint8_t pos_ok;
|
||||
};
|
||||
|
||||
// Write a Throw mode details
|
||||
void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool pos_ok)
|
||||
{
|
||||
struct log_Throw pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_THROW_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
stage : (uint8_t)stage,
|
||||
velocity : velocity,
|
||||
velocity_z : velocity_z,
|
||||
accel : accel,
|
||||
ef_accel_z : ef_accel_z,
|
||||
throw_detect : throw_detect,
|
||||
attitude_ok : attitude_ok,
|
||||
height_ok : height_ok,
|
||||
pos_ok : pos_ok
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
const struct LogStructure Copter::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
@ -754,6 +788,8 @@ const struct LogStructure Copter::log_structure[] = {
|
||||
"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" },
|
||||
{ LOG_THROW_MSG, sizeof(log_Throw),
|
||||
"THRO", "QBffffbbbb", "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk" },
|
||||
};
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
|
@ -191,6 +191,30 @@ void Copter::throw_run()
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
// log at 10hz or if stage changes
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if ((throw_state.stage != throw_state.prev_stage) || (now - throw_state.last_log_ms) > 100) {
|
||||
throw_state.prev_stage = throw_state.stage;
|
||||
throw_state.last_log_ms = now;
|
||||
float velocity = inertial_nav.get_velocity().length();
|
||||
float velocity_z = inertial_nav.get_velocity().z;
|
||||
float accel = ins.get_accel().length();
|
||||
float ef_accel_z = ahrs.get_accel_ef().z;
|
||||
bool throw_detect = (throw_state.stage > Throw_Detecting) || throw_detected();
|
||||
bool attitude_ok = (throw_state.stage > Throw_Uprighting) || throw_attitude_good();
|
||||
bool height_ok = (throw_state.stage > Throw_HgtStabilise) || throw_height_good();
|
||||
bool pos_ok = (throw_state.stage > Throw_PosHold) || throw_position_good();
|
||||
Log_Write_Throw(throw_state.stage,
|
||||
velocity,
|
||||
velocity_z,
|
||||
accel,
|
||||
ef_accel_z,
|
||||
throw_detect,
|
||||
attitude_ok,
|
||||
height_ok,
|
||||
pos_ok);
|
||||
}
|
||||
}
|
||||
|
||||
bool Copter::throw_detected()
|
||||
|
@ -293,6 +293,7 @@ enum ThrowModeType {
|
||||
#define LOG_HELI_MSG 0x20
|
||||
#define LOG_PRECLAND_MSG 0x21
|
||||
#define LOG_GUIDEDTARGET_MSG 0x22
|
||||
#define LOG_THROW_MSG 0x23
|
||||
|
||||
#define MASK_LOG_ATTITUDE_FAST (1<<0)
|
||||
#define MASK_LOG_ATTITUDE_MED (1<<1)
|
||||
|
Loading…
Reference in New Issue
Block a user