Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
       4             : 
       5             : #if HAL_LOGGING_ENABLED
       6             : 
       7             : // Write an attitude packet
       8      600310 : void Rover::Log_Write_Attitude()
       9             : {
      10      600310 :     float desired_pitch = degrees(g2.attitude_control.get_desired_pitch());
      11      600310 :     const Vector3f targets(0.0f, desired_pitch, 0.0f);
      12             : 
      13      600310 :     ahrs.Write_Attitude(targets);
      14             : 
      15      600310 :     AP::ahrs().Log_Write();
      16             : 
      17             :     // log steering rate controller
      18      600310 :     logger.Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
      19      600310 :     logger.Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid_info());
      20             : 
      21             :     // log pitch control for balance bots
      22      600310 :     if (is_balancebot()) {
      23       50077 :         logger.Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info());
      24             :     }
      25             : 
      26             :     // log heel to sail control for sailboats
      27      600310 :     if (g2.sailboat.sail_enabled()) {
      28       20419 :         logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
      29             :     }
      30             : 
      31             :     // log wheel rate controller instance 0
      32      600310 :     if (g2.wheel_rate_control.enabled(0)) {
      33           0 :         logger.Write_PID(LOG_PIDW_MSG, g2.wheel_rate_control.get_pid(0).get_pid_info());
      34             :     }
      35      600310 : }
      36             : 
      37             : #if AP_RANGEFINDER_ENABLED
      38             : // Write a range finder depth message
      39      629048 : void Rover::Log_Write_Depth()
      40             : {
      41             :     // only log depth on boats
      42      629048 :     if (!rover.is_boat() || !rangefinder.has_orientation(ROTATION_PITCH_270)) {
      43      622393 :         return;
      44             :     }
      45             : 
      46             :     // get position
      47        6655 :     Location loc;
      48        6655 :     IGNORE_RETURN(ahrs.get_location(loc));
      49             : 
      50       46585 :     for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
      51       39930 :         const AP_RangeFinder_Backend *s = rangefinder.get_backend(i);
      52             :         
      53       39930 :         if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
      54       37933 :             continue;
      55             :         }
      56             : 
      57             :         // check if new sensor reading has arrived
      58       13305 :         const uint32_t reading_ms = s->last_reading_ms();
      59       13305 :         if (reading_ms == rangefinder_last_reading_ms[i]) {
      60       11308 :             continue;
      61             :         }
      62        1997 :         rangefinder_last_reading_ms[i] = reading_ms;
      63             : 
      64             :         float temp_C;
      65        1997 :         if (!s->get_temp(temp_C)) {
      66        1331 :             temp_C = 0.0f;
      67             :         }
      68             : 
      69             :         // @LoggerMessage: DPTH
      70             :         // @Description: Depth messages on boats with downwards facing range finder
      71             :         // @Field: TimeUS: Time since system startup
      72             :         // @Field: Inst: Instance
      73             :         // @Field: Lat: Latitude 
      74             :         // @Field: Lng: Longitude   
      75             :         // @Field: Depth: Depth as detected by the sensor
      76             :         // @Field: Temp: Temperature
      77             : 
      78        1997 :         logger.Write("DPTH", "TimeUS,Inst,Lat,Lng,Depth,Temp",
      79             :                             "s#DUmO", "F-GG00", "QBLLff",
      80             :                             AP_HAL::micros64(),
      81             :                             i,
      82             :                             loc.lat,
      83             :                             loc.lng,
      84        1997 :                             (double)(s->distance()),
      85             :                             temp_C);
      86             :     }
      87             : }
      88             : #endif
      89             : 
      90             : // guided mode logging
      91             : struct PACKED log_GuidedTarget {
      92             :     LOG_PACKET_HEADER;
      93             :     uint64_t time_us;
      94             :     uint8_t type;
      95             :     float pos_target_x;
      96             :     float pos_target_y;
      97             :     float pos_target_z;
      98             :     float vel_target_x;
      99             :     float vel_target_y;
     100             :     float vel_target_z;
     101             : };
     102             : 
     103             : // Write a Guided mode target
     104        2119 : void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
     105             : {
     106        2119 :     struct log_GuidedTarget pkt = {
     107             :         LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
     108        2119 :         time_us         : AP_HAL::micros64(),
     109             :         type            : target_type,
     110        2119 :         pos_target_x    : pos_target.x,
     111        2119 :         pos_target_y    : pos_target.y,
     112        2119 :         pos_target_z    : pos_target.z,
     113        2119 :         vel_target_x    : vel_target.x,
     114        2119 :         vel_target_y    : vel_target.y,
     115        2119 :         vel_target_z    : vel_target.z
     116        2119 :     };
     117        2119 :     logger.WriteBlock(&pkt, sizeof(pkt));
     118        2119 : }
     119             : 
     120             : struct PACKED log_Nav_Tuning {
     121             :     LOG_PACKET_HEADER;
     122             :     uint64_t time_us;
     123             :     float wp_distance;
     124             :     float wp_bearing;
     125             :     float nav_bearing;
     126             :     uint16_t yaw;
     127             :     float xtrack_error;
     128             : };
     129             : 
     130             : // Write a navigation tuning packet
     131      119840 : void Rover::Log_Write_Nav_Tuning()
     132             : {
     133      119840 :     struct log_Nav_Tuning pkt = {
     134             :         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
     135      119840 :         time_us             : AP_HAL::micros64(),
     136      119840 :         wp_distance         : control_mode->get_distance_to_destination(),
     137      119840 :         wp_bearing          : control_mode->wp_bearing(),
     138      119840 :         nav_bearing         : control_mode->nav_bearing(),
     139      119840 :         yaw                 : (uint16_t)ahrs.yaw_sensor,
     140      119840 :         xtrack_error        : control_mode->crosstrack_error_m()
     141      119840 :     };
     142      119840 :     logger.WriteBlock(&pkt, sizeof(pkt));
     143      119840 : }
     144             : 
     145      600310 : void Rover::Log_Write_Sail()
     146             : {
     147             :     // only log sail if present
     148      600310 :     if (!g2.sailboat.sail_enabled()) {
     149      579891 :         return;
     150             :     }
     151             : 
     152       20419 :     float wind_dir_tack = AP_Logger::quiet_nanf();
     153       20419 :     uint8_t current_tack = 0;
     154       20419 :     if (g2.windvane.enabled()) {
     155       20419 :         wind_dir_tack = degrees(g2.windvane.get_tack_threshold_wind_dir_rad());
     156       20419 :         current_tack = uint8_t(g2.windvane.get_current_tack());
     157             :     }
     158             : 
     159             : // @LoggerMessage: SAIL
     160             : // @Description: Sailboat information
     161             : // @Field: TimeUS: Time since system startup
     162             : // @Field: Tack: Current tack, 0 = port, 1 = starboard
     163             : // @Field: TackThr: Apparent wind angle used for tack threshold
     164             : // @Field: MainOut: Normalized mainsail output
     165             : // @Field: WingOut: Normalized wingsail output
     166             : // @Field: MastRotOut: Normalized direct-rotation mast output
     167             : // @Field: VMG: Velocity made good (speed at which vehicle is making progress directly towards destination)
     168             : 
     169       20419 :     logger.Write("SAIL", "TimeUS,Tack,TackThr,MainOut,WingOut,MastRotOut,VMG",
     170             :                         "s-d%%%n", "F000000", "QBfffff",
     171             :                         AP_HAL::micros64(),
     172             :                         current_tack,
     173             :                         (double)wind_dir_tack,
     174       20419 :                         (double)g2.motors.get_mainsail(),
     175       20419 :                         (double)g2.motors.get_wingsail(),
     176       20419 :                         (double)g2.motors.get_mast_rotation(),
     177       20419 :                         (double)g2.sailboat.get_VMG());
     178             : }
     179             : 
     180             : struct PACKED log_Steering {
     181             :     LOG_PACKET_HEADER;
     182             :     uint64_t time_us;
     183             :     int16_t steering_in;
     184             :     float steering_out;
     185             :     float desired_lat_accel;
     186             :     float lat_accel;
     187             :     float desired_turn_rate;
     188             :     float turn_rate;
     189             : };
     190             : 
     191             : // Write a steering packet
     192      119840 : void Rover::Log_Write_Steering()
     193             : {
     194      119840 :     float lat_accel = AP_Logger::quiet_nanf();
     195      119840 :     g2.attitude_control.get_lat_accel(lat_accel);
     196      119840 :     struct log_Steering pkt = {
     197             :         LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
     198      119840 :         time_us        : AP_HAL::micros64(),
     199      119840 :         steering_in        : channel_steer->get_control_in(),
     200      119840 :         steering_out       : g2.motors.get_steering(),
     201      119840 :         desired_lat_accel  : control_mode->get_desired_lat_accel(),
     202             :         lat_accel          : lat_accel,
     203      119840 :         desired_turn_rate  : degrees(g2.attitude_control.get_desired_turn_rate()),
     204      119840 :         turn_rate          : degrees(ahrs.get_yaw_rate_earth())
     205      239680 :     };
     206      119840 :     logger.WriteBlock(&pkt, sizeof(pkt));
     207      119840 : }
     208             : 
     209             : struct PACKED log_Throttle {
     210             :     LOG_PACKET_HEADER;
     211             :     uint64_t time_us;
     212             :     int16_t throttle_in;
     213             :     float throttle_out;
     214             :     float desired_speed;
     215             :     float speed;
     216             :     float accel_x;
     217             : };
     218             : 
     219             : // Write a throttle control packet
     220      119840 : void Rover::Log_Write_Throttle()
     221             : {
     222      119840 :     const Vector3f accel = ins.get_accel();
     223      119840 :     float speed = AP_Logger::quiet_nanf();
     224      119840 :     g2.attitude_control.get_forward_speed(speed);
     225      119840 :     struct log_Throttle pkt = {
     226             :         LOG_PACKET_HEADER_INIT(LOG_THR_MSG),
     227      119840 :         time_us         : AP_HAL::micros64(),
     228      119840 :         throttle_in     : channel_throttle->get_control_in(),
     229      119840 :         throttle_out    : g2.motors.get_throttle(),
     230      119840 :         desired_speed   : g2.attitude_control.get_desired_speed(),
     231             :         speed           : speed,
     232      119840 :         accel_x         : accel.x
     233      119840 :     };
     234      119840 :     logger.WriteBlock(&pkt, sizeof(pkt));
     235      119840 : }
     236             : 
     237      119840 : void Rover::Log_Write_RC(void)
     238             : {
     239      119840 :     logger.Write_RCIN();
     240      119840 :     logger.Write_RCOUT();
     241             : #if AP_RSSI_ENABLED
     242      119840 :     if (rssi.enabled()) {
     243           0 :         logger.Write_RSSI();
     244             :     }
     245             : #endif
     246      119840 : }
     247             : 
     248         800 : void Rover::Log_Write_Vehicle_Startup_Messages()
     249             : {
     250             :     // only 200(?) bytes are guaranteed by AP_Logger
     251         800 :     logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason);
     252         800 :     ahrs.Log_Write_Home_And_Origin();
     253         800 :     gps.Write_AP_Logger_Log_Startup_messages();
     254         800 : }
     255             : 
     256             : // type and unit information can be found in
     257             : // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
     258             : // units and "Format characters" for field type information
     259             : 
     260             : const LogStructure Rover::log_structure[] = {
     261             :     LOG_COMMON_STRUCTURES,
     262             : 
     263             : // @LoggerMessage: THR
     264             : // @Description: Throttle related messages
     265             : // @Field: TimeUS: Time since system startup
     266             : // @Field: ThrIn: Throttle Input
     267             : // @Field: ThrOut: Throttle Output 
     268             : // @Field: DesSpeed: Desired speed 
     269             : // @Field: Speed: Actual speed
     270             : // @Field: AccX: Acceleration
     271             : 
     272             :     { LOG_THR_MSG, sizeof(log_Throttle),
     273             :       "THR", "Qhffff", "TimeUS,ThrIn,ThrOut,DesSpeed,Speed,AccX", "s--nno", "F--000" },
     274             : 
     275             : // @LoggerMessage: NTUN
     276             : // @Description: Navigation Tuning information - e.g. vehicle destination
     277             : // @URL: http://ardupilot.club/rover/docs/navigation.html
     278             : // @Field: TimeUS: Time since system startup
     279             : // @Field: WpDist: distance to the current navigation waypoint
     280             : // @Field: WpBrg: bearing to the current navigation waypoint
     281             : // @Field: DesYaw: the vehicle's desired heading
     282             : // @Field: Yaw: the vehicle's current heading
     283             : // @Field: XTrack: the vehicle's current distance from the current travel segment
     284             : 
     285             :     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
     286             :       "NTUN", "QfffHf", "TimeUS,WpDist,WpBrg,DesYaw,Yaw,XTrack", "smhhhm", "F000B0" },
     287             :     
     288             : // @LoggerMessage: STER
     289             : // @Description: Steering related messages
     290             : // @Field: TimeUS: Time since system startup
     291             : // @Field: SteerIn: Steering input
     292             : // @Field: SteerOut: Normalized steering output 
     293             : // @Field: DesLatAcc: Desired lateral acceleration
     294             : // @Field: LatAcc: Actual lateral acceleration
     295             : // @Field: DesTurnRate: Desired turn rate
     296             : // @Field: TurnRate: Actual turn rate
     297             :     
     298             :     { LOG_STEERING_MSG, sizeof(log_Steering),
     299             :       "STER", "Qhfffff",   "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" },
     300             : 
     301             : // @LoggerMessage: GUIP
     302             : // @Description: Guided mode target information
     303             : // @Field: TimeUS: Time since system startup
     304             : // @Field: Type: Type of guided mode
     305             : // @Field: pX: Target position, X-Axis
     306             : // @Field: pY: Target position, Y-Axis
     307             : // @Field: pZ: Target position, Z-Axis
     308             : // @Field: vX: Target velocity, X-Axis
     309             : // @Field: vY: Target velocity, Y-Axis
     310             : // @Field: vZ: Target velocity, Z-Axis
     311             :     
     312             :     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
     313             :       "GUIP",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
     314             : };
     315             : 
     316         285 : uint8_t Rover::get_num_log_structures() const
     317             : {
     318         285 :     return ARRAY_SIZE(log_structure);
     319             : }
     320             : 
     321             : #endif  // HAL_LOGGING_ENABLED