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

Copter: add Precision Landing log message

Browse Source
This commit is contained in:
Randy Mackay 2015-08-28 17:14:02 +09:00
parent 26ded641db
commit 0da38ba2bf
3 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -632,6 +632,7 @@ private:
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void); void Log_Write_Heli(void);
#endif #endif
void Log_Write_Precland();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void start_logging() ; void start_logging() ;
void load_parameters(void); void load_parameters(void);

42
ArduCopter/Log.cpp
View File

@ -681,6 +681,46 @@ void Copter::Log_Write_Heli()
} }
#endif #endif
// precision landing logging
struct PACKED log_Precland {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t healthy;
float bf_angle_x;
float bf_angle_y;
float ef_angle_x;
float ef_angle_y;
float pos_x;
float pos_y;
};
// Write an optical flow packet
void Copter::Log_Write_Precland()
{
#if PRECISION_LANDING == ENABLED
// exit immediately if not enabled
if (!precland.enabled()) {
return;
}
const Vector2f &bf_angle = precland.last_bf_angle_to_target();
const Vector2f &ef_angle = precland.last_ef_angle_to_target();
const Vector3f &target_pos_ofs = precland.last_target_pos_offset();
struct log_Precland pkt = {
LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG),
time_us : hal.scheduler->micros64(),
healthy : precland.healthy(),
bf_angle_x : degrees(bf_angle.x),
bf_angle_y : degrees(bf_angle.y),
ef_angle_x : degrees(ef_angle.x),
ef_angle_y : degrees(ef_angle.y),
pos_x : target_pos_ofs.x,
pos_y : target_pos_ofs.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif // PRECISION_LANDING == ENABLED
}
const struct LogStructure Copter::log_structure[] PROGMEM = { const struct LogStructure Copter::log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES, LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED #if AUTOTUNE_ENABLED == ENABLED
@ -721,6 +761,8 @@ const struct LogStructure Copter::log_structure[] PROGMEM = {
"ERR", "QBB", "TimeUS,Subsys,ECode" }, "ERR", "QBB", "TimeUS,Subsys,ECode" },
{ LOG_HELI_MSG, sizeof(log_Heli), { LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qhh", "TimeUS,DRRPM,ERRPM" }, "HELI", "Qhh", "TimeUS,DRRPM,ERRPM" },
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
}; };
#if CLI_ENABLED == ENABLED #if CLI_ENABLED == ENABLED

1
ArduCopter/defines.h
View File

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