Line data    Source code

       1             : /*
       2             :   external control library for copter
       3             :  */
       4             : 
       5             : 
       6             : #include "AP_ExternalControl_Copter.h"
       7             : #if AP_EXTERNAL_CONTROL_ENABLED
       8             : 
       9             : #include "Copter.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_Copter::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           0 :     const float checked_yaw_rate_rad = isnan(yaw_rate_rads)? 0: yaw_rate_rads;
      21             : 
      22             :     // Copter velocity is positive if aircraft is moving up which is opposite the incoming NED frame.
      23           0 :     copter.mode_guided.set_vel_NED_ms(linear_velocity_ned_ms, false, 0, !isnan(yaw_rate_rads), checked_yaw_rate_rad);
      24           0 :     return true;
      25             : }
      26             : 
      27           0 : bool AP_ExternalControl_Copter::set_global_position(const Location& loc)
      28             : {
      29             :     // Check if copter is ready for external control and returns false if it is not.
      30           0 :     if (!ready_for_external_control()) {
      31           0 :         return false;
      32             :     }
      33           0 :     return copter.set_target_location(loc);
      34             : }
      35             : 
      36           0 : bool AP_ExternalControl_Copter::ready_for_external_control()
      37             : {
      38           0 :     return copter.flightmode->in_guided_mode() && copter.motors->armed();
      39             : }
      40             : 
      41             : #endif // AP_EXTERNAL_CONTROL_ENABLED