ArduCopter/AP_ExternalControl_Copter.cpp

   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 bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
  16 {
  17     if (!ready_for_external_control()) {
  18         return false;
  19     }
  20     const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;
  21
  22     // Copter velocity is positive if aircraft is moving up which is opposite the incoming NED frame.
  23     Vector3f velocity_NEU_ms {
  24         linear_velocity.x,
  25         linear_velocity.y,
  26         -linear_velocity.z };
  27     Vector3f velocity_up_cms = velocity_NEU_ms * 100;
  28     copter.mode_guided.set_velocity(velocity_up_cms, false, 0, !isnan(yaw_rate_rads), yaw_rate_cds);
  29     return true;
  30 }
  31
  32 bool AP_ExternalControl_Copter::set_global_position(const Location& loc)
  33 {
  34     // Check if copter is ready for external control and returns false if it is not.
  35     if (!ready_for_external_control()) {
  36         return false;
  37     }
  38     return copter.set_target_location(loc);
  39 }
  40
  41 bool AP_ExternalControl_Copter::ready_for_external_control()
  42 {
  43     return copter.flightmode->in_guided_mode() && copter.motors->armed();
  44 }
  45
  46 #endif // AP_EXTERNAL_CONTROL_ENABLED