Line data    Source code

       1             : #include "mode.h"
       2             : 
       3             : #include "Tracker.h"
       4             : #include <GCS_MAVLink/GCS.h>
       5             : 
       6        4249 : void ModeGuided::update()
       7             : {
       8             :     static uint32_t last_debug;
       9        4249 :     const uint32_t now = AP_HAL::millis();
      10             :     float target_roll, target_pitch, target_yaw;
      11        4249 :     _target_att.to_euler(target_roll, target_pitch, target_yaw);
      12        4249 :     if (now - last_debug > 5000) {
      13          17 :         last_debug = now;
      14          17 :         gcs().send_text(MAV_SEVERITY_INFO, "target_yaw=%f target_pitch=%f", degrees(target_yaw), degrees(target_pitch));
      15             :     }
      16        4249 :     calc_angle_error(degrees(target_pitch)*100, degrees(target_yaw)*100, false);
      17             :     float bf_pitch;
      18             :     float bf_yaw;
      19        4249 :     convert_ef_to_bf(target_pitch, target_yaw, bf_pitch, bf_yaw);
      20        4249 :     tracker.update_pitch_servo(bf_pitch);
      21        4249 :     tracker.update_yaw_servo(bf_yaw);
      22        4249 : }