5
0
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:
Randy Mackay 2016-08-02 14:41:39 +09:00
parent 8c5c8eec44
commit f4f13bbe6b
4 changed files with 65 additions and 1 deletions

View File

@ -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() ;

View File

@ -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

View File

@ -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()

View File

@ -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)