ArduCopter/mode_circle.cpp

   1 #include "Copter.h"
   2 #include <AP_Mount/AP_Mount.h>
   3
   4 #if MODE_CIRCLE_ENABLED
   5
   6 /*
   7  * Init and run calls for circle flight mode
   8  */
   9
  10 // circle_init - initialise circle controller flight mode
  11 bool ModeCircle::init(bool ignore_checks)
  12 {
  13     speed_changing = false;
  14
  15     // set speed and acceleration limits
  16     pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
  17     pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
  18     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
  19     pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
  20
  21     // initialise circle controller including setting the circle center based on vehicle speed
  22     copter.circle_nav->init();
  23
  24 #if HAL_MOUNT_ENABLED
  25     // Check if the CIRCLE_OPTIONS parameter have roi_at_center
  26     if (copter.circle_nav->roi_at_center()) {
  27         const Vector3p &pos { copter.circle_nav->get_center() };
  28         Location circle_center;
  29         if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos * 0.01)) {
  30             return false;
  31         }
  32         // point at the ground:
  33         circle_center.set_alt_cm(0, Location::AltFrame::ABOVE_TERRAIN);
  34         AP_Mount *s = AP_Mount::get_singleton();
  35         s->set_roi_target(circle_center);
  36     }
  37 #endif
  38
  39     // set auto yaw circle mode
  40     auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
  41
  42     return true;
  43 }
  44
  45 // circle_run - runs the circle flight mode
  46 // should be called at 100hz or more
  47 void ModeCircle::run()
  48 {
  49     // set speed and acceleration limits
  50     pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
  51     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
  52
  53     // Check for any change in params and update in real time
  54     copter.circle_nav->check_param_change();
  55
  56     // pilot changes to circle rate and radius
  57     // skip if in radio failsafe
  58     if (!copter.failsafe.radio && copter.circle_nav->pilot_control_enabled()) {
  59         // update the circle controller's radius target based on pilot pitch stick inputs
  60         const float radius_current = copter.circle_nav->get_radius();           // circle controller's radius target, which begins as the circle_radius parameter
  61         const float pitch_stick = channel_pitch->norm_input_dz();               // pitch stick normalized -1 to 1
  62         const float nav_speed = copter.wp_nav->get_default_speed_xy();          // copter WP_NAV parameter speed
  63         const float radius_pilot_change = (pitch_stick * nav_speed) * G_Dt;     // rate of change (pitch stick up reduces the radius, as in moving forward)
  64         const float radius_new = MAX(radius_current + radius_pilot_change,0);   // new radius target
  65
  66         if (!is_equal(radius_current, radius_new)) {
  67             copter.circle_nav->set_radius_cm(radius_new);
  68         }
  69
  70         // update the orbicular rate target based on pilot roll stick inputs
  71         // skip if using transmitter based tuning knob for circle rate
  72         if (g.radio_tuning != TUNING_CIRCLE_RATE) {
  73             const float roll_stick = channel_roll->norm_input_dz();         // roll stick normalized -1 to 1
  74
  75             if (is_zero(roll_stick)) {
  76                 // no speed change, so reset speed changing flag
  77                 speed_changing = false;
  78             } else {
  79                 const float rate = copter.circle_nav->get_rate();           // circle controller's rate target, which begins as the circle_rate parameter
  80                 const float rate_current = copter.circle_nav->get_rate_current(); // current adjusted rate target, which is probably different from _rate
  81                 const float rate_pilot_change = (roll_stick * G_Dt);        // rate of change from 0 to 1 degrees per second
  82                 float rate_new = rate_current;                              // new rate target
  83                 if (is_positive(rate)) {
  84                     // currently moving clockwise, constrain 0 to 90
  85                     rate_new = constrain_float(rate_current + rate_pilot_change, 0, 90);
  86
  87                 } else if (is_negative(rate)) {
  88                     // currently moving counterclockwise, constrain -90 to 0
  89                     rate_new = constrain_float(rate_current + rate_pilot_change, -90, 0);
  90
  91                 } else if (is_zero(rate) && !speed_changing) {
  92                     // Stopped, pilot has released the roll stick, and pilot now wants to begin moving with the roll stick
  93                     rate_new = rate_pilot_change;
  94                 }
  95
  96                 speed_changing = true;
  97                 copter.circle_nav->set_rate(rate_new);
  98             }
  99         }
 100     }
 101
 102     // get pilot desired climb rate (or zero if in radio failsafe)
 103     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
 104
 105     // get avoidance adjusted climb rate
 106     target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 107
 108     // if not armed set throttle to zero and exit immediately
 109     if (is_disarmed_or_landed()) {
 110         make_safe_ground_handling();
 111         return;
 112     }
 113
 114     // set motors to full range
 115     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 116
 117 #if AP_RANGEFINDER_ENABLED
 118     // update the vertical offset based on the surface measurement
 119     copter.surface_tracking.update_surface_offset();
 120 #endif
 121
 122     copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
 123     pos_control->update_z_controller();
 124
 125     // call attitude controller with auto yaw
 126     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 127 }
 128
 129 uint32_t ModeCircle::wp_distance() const
 130 {
 131     return copter.circle_nav->get_distance_to_target();
 132 }
 133
 134 int32_t ModeCircle::wp_bearing() const
 135 {
 136     return copter.circle_nav->get_bearing_to_target();
 137 }
 138
 139 #endif