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

Copter: add beacon data flash logging

Browse Source 
Added to Copter vehicle because currently only used by Copter
This commit is contained in:
Randy Mackay 2016-10-27 13:40:55 +09:00
parent f2a6a4f4f9
commit 74c9c4aa9d
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/ArduCopter.cpp
View File

@ -406,6 +406,7 @@ void Copter::ten_hz_logging_loop()
if (should_log(MASK_LOG_CTUN)) {
attitude_control.control_monitor_log();
Log_Write_Proximity();
Log_Write_Beacon();
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();

1
ArduCopter/Copter.h
View File

@ -753,6 +753,7 @@ private:
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_Proximity();
void Log_Write_Beacon();
void Log_Write_Vehicle_Startup_Messages();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void start_logging() ;

46
ArduCopter/Log.cpp
View File

@ -806,6 +806,50 @@ void Copter::Log_Write_Proximity()
#endif
}
// beacon sensor logging
struct PACKED log_Beacon {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t health;
uint8_t count;
float dist0;
float dist1;
float dist2;
float dist3;
float posx;
float posy;
float posz;
};
// Write beacon position and distances
void Copter::Log_Write_Beacon()
{
// exit immediately if feature is disabled
if (!g2.beacon.enabled()) {
return;
}
// position
Vector3f pos;
float accuracy = 0.0f;
g2.beacon.get_vehicle_position_ned(pos, accuracy);
struct log_Beacon pkt = {
LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG),
time_us : AP_HAL::micros64(),
health : (uint8_t)g2.beacon.healthy(),
count : (uint8_t)g2.beacon.count(),
dist0 : g2.beacon.beacon_distance(0),
dist1 : g2.beacon.beacon_distance(1),
dist2 : g2.beacon.beacon_distance(2),
dist3 : g2.beacon.beacon_distance(3),
posx : pos.x,
posy : pos.y,
posz : pos.z
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
const struct LogStructure Copter::log_structure[] = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
@ -850,6 +894,8 @@ const struct LogStructure Copter::log_structure[] = {
"THRO", "QBffffbbbb", "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk" },
{ LOG_PROXIMITY_MSG, sizeof(log_Proximity),
"PRX", "QBffffffffff","TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315,CAng,CDist" },
{ LOG_BEACON_MSG, sizeof(log_Beacon),
"BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ" },
};
#if CLI_ENABLED == ENABLED

1
ArduCopter/defines.h
View File

@ -308,6 +308,7 @@ enum DevOptions {
#define LOG_GUIDEDTARGET_MSG 0x22
#define LOG_THROW_MSG 0x23
#define LOG_PROXIMITY_MSG 0x24
#define LOG_BEACON_MSG 0x25
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)