aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: log proximity sensor at 10hz

Browse Source 
This is current in the Copter vehicle code but we can move to the dataflash library when other vehicles start using this type of sensor.  Until then adding it to common will just increase the dependencies unnecessarily for other vehicles.
This commit is contained in:
Randy Mackay 2016-08-16 21:09:17 +09:00
parent bba7369e9a
commit a07ecfe2b3
4 changed files with 51 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/ArduCopter.cpp
View File

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

1
ArduCopter/Copter.h
View File

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

48
ArduCopter/Log.cpp
View File

@ -750,6 +750,52 @@ void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocit
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// proximity sensor logging
struct PACKED log_Proximity {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t health;
float dist0;
float dist45;
float dist90;
float dist135;
float dist180;
float dist225;
float dist270;
float dist315;
};
// Write proximity sensor distances
void Copter::Log_Write_Proximity()
{
#if PROXIMITY_ENABLED == ENABLED
float sector_distance[8] = {0,0,0,0,0,0,0,0};
proximity.get_horizontal_distance(0, sector_distance[0]);
proximity.get_horizontal_distance(45, sector_distance[1]);
proximity.get_horizontal_distance(90, sector_distance[2]);
proximity.get_horizontal_distance(135, sector_distance[3]);
proximity.get_horizontal_distance(180, sector_distance[4]);
proximity.get_horizontal_distance(225, sector_distance[5]);
proximity.get_horizontal_distance(270, sector_distance[6]);
proximity.get_horizontal_distance(315, sector_distance[7]);
struct log_Proximity pkt = {
LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
time_us : AP_HAL::micros64(),
health : (uint8_t)proximity.get_status(),
dist0 : sector_distance[0],
dist45 : sector_distance[1],
dist90 : sector_distance[2],
dist135 : sector_distance[3],
dist180 : sector_distance[4],
dist225 : sector_distance[5],
dist270 : sector_distance[6],
dist315 : sector_distance[7]
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif
}
const struct LogStructure Copter::log_structure[] = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
@ -792,6 +838,8 @@ const struct LogStructure Copter::log_structure[] = {
"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" },
{ LOG_PROXIMITY_MSG, sizeof(log_Proximity),
"PRX", "QBffffffff", "TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315" },
};
#if CLI_ENABLED == ENABLED

1
ArduCopter/defines.h
View File

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