Line data    Source code

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