Line data    Source code

       1             : /*
       2             :   external control library for rover
       3             :  */
       4             : 
       5             : 
       6             : #include "AP_ExternalControl_Rover.h"
       7             : #if AP_EXTERNAL_CONTROL_ENABLED
       8             : 
       9             : #include "Rover.h"
      10             : 
      11             : /*
      12             :   set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
      13             :   velocity is in earth frame, NED, m/s
      14             : */
      15           0 : bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity_ned_ms, float yaw_rate_rads)
      16             : {
      17           0 :     if (!ready_for_external_control()) {
      18           0 :         return false;
      19             :     }
      20             : 
      21             :     // rover is commanded in body-frame using FRD convention
      22           0 :     auto &ahrs = AP::ahrs();
      23           0 :     Vector3f linear_velocity_body = ahrs.earth_to_body(linear_velocity_ned_ms);
      24             : 
      25           0 :     const float target_speed = linear_velocity_body.x;
      26           0 :     const float turn_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;
      27             : 
      28           0 :     rover.mode_guided.set_desired_turn_rate_and_speed(turn_rate_cds, target_speed);
      29           0 :     return true;
      30             : }
      31             : 
      32           0 : bool AP_ExternalControl_Rover::set_global_position(const Location& loc)
      33             : {
      34             :     // set_target_location only checks if the rover is in guided mode or not
      35           0 :     return rover.set_target_location(loc);
      36             : }
      37             : 
      38           0 : bool AP_ExternalControl_Rover::ready_for_external_control()
      39             : {
      40           0 :     return rover.control_mode->in_guided_mode() && rover.arming.is_armed();
      41             : }
      42             : 
      43             : #endif // AP_EXTERNAL_CONTROL_ENABLED