AntennaTracker/Log.cpp

   1 #include "Tracker.h"
   2
   3 #if HAL_LOGGING_ENABLED
   4
   5 // Code to Write and Read packets from AP_Logger log memory
   6
   7 // Write an attitude packet
   8 void Tracker::Log_Write_Attitude()
   9 {
  10     Vector3f targets;
  11     targets.y = nav_status.pitch * 100.0f;
  12     targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
  13     ahrs.Write_Attitude(targets);
  14     AP::ahrs().Log_Write();
  15 }
  16
  17 struct PACKED log_Vehicle_Baro {
  18     LOG_PACKET_HEADER;
  19     uint64_t time_us;
  20     float    press;
  21     float    alt_diff;
  22 };
  23
  24 // Write a vehicle baro packet
  25 void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude)
  26 {
  27     struct log_Vehicle_Baro pkt = {
  28         LOG_PACKET_HEADER_INIT(LOG_V_BAR_MSG),
  29         time_us         : AP_HAL::micros64(),
  30         press           : pressure,
  31         alt_diff        : altitude
  32     };
  33     logger.WriteBlock(&pkt, sizeof(pkt));
  34 }
  35
  36 struct PACKED log_Vehicle_Pos {
  37     LOG_PACKET_HEADER;
  38     uint64_t time_us;
  39     int32_t vehicle_lat;
  40     int32_t vehicle_lng;
  41     int32_t vehicle_alt;
  42     float vehicle_vel_x;
  43     float vehicle_vel_y;
  44     float vehicle_vel_z;
  45 };
  46
  47 // Write a vehicle pos packet
  48 void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel)
  49 {
  50     struct log_Vehicle_Pos pkt = {
  51         LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG),
  52         time_us         : AP_HAL::micros64(),
  53         vehicle_lat     : lat,
  54         vehicle_lng     : lng,
  55         vehicle_alt     : alt,
  56         vehicle_vel_x   : vel.x,
  57         vehicle_vel_y   : vel.y,
  58         vehicle_vel_z   : vel.z,
  59     };
  60     logger.WriteBlock(&pkt, sizeof(pkt));
  61 }
  62
  63 // @LoggerMessage: VBAR
  64 // @Description: Information received from tracked vehicle; barometer data
  65 // @Field: TimeUS: Time since system startup
  66 // @Field: Press: vehicle barometric pressure
  67 // @Field: AltDiff: altitude difference based on difference on barometric pressure
  68
  69 // @LoggerMessage: VPOS
  70 // @Description: Information received from tracked vehicle; barometer position data
  71 // @Field: TimeUS: Time since system startup
  72 // @Field: Lat: tracked vehicle latitude
  73 // @Field: Lng: tracked vehicle longitude
  74 // @Field: Alt: tracked vehicle altitude
  75 // @Field: VelX: tracked vehicle velocity, latitude component
  76 // @Field: VelY: tracked vehicle velocity, longitude component
  77 // @Field: VelZ: tracked vehicle velocity, vertical component, down
  78
  79 // type and unit information can be found in
  80 // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
  81 // units and "Format characters" for field type information
  82 const struct LogStructure Tracker::log_structure[] = {
  83     LOG_COMMON_STRUCTURES,
  84     {LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
  85         "VBAR", "Qff", "TimeUS,Press,AltDiff", "sPm", "F00" , true },
  86     {LOG_V_POS_MSG, sizeof(log_Vehicle_Pos),
  87        "VPOS", "QLLefff", "TimeUS,Lat,Lng,Alt,VelX,VelY,VelZ", "sddmnnn", "FGGB000", true }
  88 };
  89
  90 uint8_t Tracker::get_num_log_structures() const
  91 {
  92     return ARRAY_SIZE(log_structure);
  93 }
  94
  95 void Tracker::Log_Write_Vehicle_Startup_Messages()
  96 {
  97     logger.Write_Mode((uint8_t)mode->number(), ModeReason::INITIALISED);
  98     gps.Write_AP_Logger_Log_Startup_messages();
  99 }
 100
 101 #endif // HAL_LOGGING_ENABLED