Line data    Source code

       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          22 : bool ModeCircle::init(bool ignore_checks)
      12             : {
      13          22 :     speed_changing = false;
      14             : 
      15             :     // set speed and acceleration limits
      16          22 :     pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      17          22 :     pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      18          22 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      19          22 :     pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      20             : 
      21             :     // initialise circle controller including setting the circle center based on vehicle speed
      22          22 :     copter.circle_nav->init();
      23             : 
      24             : #if HAL_MOUNT_ENABLED
      25             :     // Check if the CIRCLE_OPTIONS parameter have roi_at_center
      26          22 :     if (copter.circle_nav->roi_at_center()) {
      27           0 :         const Vector3p &pos_ned_m { copter.circle_nav->get_center_NED_m() };
      28           0 :         Location circle_center;
      29           0 :         if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos_ned_m)) {
      30           0 :             return false;
      31             :         }
      32             :         // point at the ground:
      33           0 :         circle_center.set_alt_m(0, Location::AltFrame::ABOVE_TERRAIN);
      34           0 :         AP_Mount *s = AP_Mount::get_singleton();
      35           0 :         s->set_roi_target(circle_center);
      36             :     }
      37             : #endif
      38             : 
      39             :     // set auto yaw circle mode
      40          22 :     auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
      41             : 
      42          22 :     return true;
      43             : }
      44             : 
      45             : // circle_run - runs the circle flight mode
      46             : // should be called at 100hz or more
      47      115671 : void ModeCircle::run()
      48             : {
      49             :     // set speed and acceleration limits
      50      115671 :     pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      51      115671 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      52             : 
      53             :     // Check for any change in params and update in real time
      54      115671 :     copter.circle_nav->check_param_change();
      55             : 
      56             :     // pilot changes to circle rate and radius
      57             :     // skip if in radio failsafe
      58      115671 :     if (rc().has_valid_input() && copter.circle_nav->pilot_control_enabled()) {
      59             :         // update the circle controller's radius target based on pilot pitch stick inputs
      60      115671 :         const float radius_current_m = copter.circle_nav->get_radius_m();               // circle controller's radius target, which begins as the circle_radius parameter
      61      115671 :         const float pitch_stick_norm = channel_pitch->norm_input_dz();                  // pitch stick normalized -1 to 1
      62      115671 :         const float nav_speed_ms = copter.wp_nav->get_default_speed_NE_ms();            // copter WP_NAV parameter speed
      63      115671 :         const float radius_pilot_change_m = (pitch_stick_norm * nav_speed_ms) * G_Dt;   // rate of change (pitch stick up reduces the radius, as in moving forward)
      64      115671 :         const float radius_new_m = MAX(radius_current_m + radius_pilot_change_m,0);     // new radius target
      65             : 
      66      115671 :         if (!is_equal(radius_current_m, radius_new_m)) {
      67        6408 :             copter.circle_nav->set_radius_m(radius_new_m);
      68             :         }
      69             : 
      70             :         // update the orbicular rate target based on pilot roll stick inputs
      71             : #if AP_RC_TRANSMITTER_TUNING_ENABLED
      72             :         // skip if using transmitter based tuning knob for circle rate
      73      115671 :         if (!copter.being_tuned(TUNING_CIRCLE_RATE)) {
      74             : #else
      75             :         {
      76             : #endif
      77      115671 :             const float roll_stick_norm = channel_roll->norm_input_dz();         // roll stick normalized -1 to 1
      78             : 
      79      115671 :             if (is_zero(roll_stick_norm)) {
      80             :                 // no speed change, so reset speed changing flag
      81       99910 :                 speed_changing = false;
      82             :             } else {
      83       15761 :                 const float rate_degs = copter.circle_nav->get_rate_degs();           // circle controller's rate target, which begins as the circle_rate parameter
      84       15761 :                 const float rate_target_degs = copter.circle_nav->get_rate_target_degs(); // current adjusted rate target, which is probably different from _rate_degs
      85       15761 :                 const float rate_pilot_change_degs = (roll_stick_norm * G_Dt);        // rate of change from 0 to 1 degrees per second
      86       15761 :                 float rate_new_degs = rate_target_degs;                               // new rate target
      87       15761 :                 if (is_positive(rate_degs)) {
      88             :                     // currently moving clockwise, constrain 0 to 90
      89       15761 :                     rate_new_degs = constrain_float(rate_target_degs + rate_pilot_change_degs, 0, 90);
      90             : 
      91           0 :                 } else if (is_negative(rate_degs)) {
      92             :                     // currently moving counterclockwise, constrain -90 to 0
      93           0 :                     rate_new_degs = constrain_float(rate_target_degs + rate_pilot_change_degs, -90, 0);
      94             : 
      95           0 :                 } else if (is_zero(rate_degs) && !speed_changing) {
      96             :                     // Stopped, pilot has released the roll stick, and pilot now wants to begin moving with the roll stick
      97           0 :                     rate_new_degs = rate_pilot_change_degs;
      98             :                 }
      99             : 
     100       15761 :                 speed_changing = true;
     101       15761 :                 copter.circle_nav->set_rate_degs(rate_new_degs);
     102             :             }
     103             :         }
     104             :     }
     105             : 
     106             :     // get pilot desired climb rate (or zero if in radio failsafe)
     107      115671 :     float target_climb_rate_ms = get_pilot_desired_climb_rate_ms();
     108             : 
     109             :     // get avoidance adjusted climb rate
     110      115671 :     target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     111             : 
     112             :     // if not armed set throttle to zero and exit immediately
     113      115671 :     if (is_disarmed_or_landed()) {
     114        5790 :         make_safe_ground_handling();
     115        5790 :         return;
     116             :     }
     117             : 
     118             :     // set motors to full range
     119      109881 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     120             : 
     121             : #if AP_RANGEFINDER_ENABLED
     122             :     // update the vertical offset based on the surface measurement
     123      109881 :     copter.surface_tracking.update_surface_offset();
     124             : #endif
     125             : 
     126      109881 :     copter.failsafe_terrain_set_status(copter.circle_nav->update_ms(target_climb_rate_ms));
     127      109881 :     pos_control->D_update_controller();
     128             : 
     129             :     // call attitude controller with auto yaw
     130      109881 :     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
     131             : }
     132             : 
     133        1724 : float ModeCircle::wp_distance_m() const
     134             : {
     135        1724 :     return copter.circle_nav->get_distance_to_target_m();
     136             : }
     137             : 
     138        1724 : float ModeCircle::wp_bearing_deg() const
     139             : {
     140        1724 :     return degrees(copter.circle_nav->get_bearing_to_target_rad());
     141             : }
     142             : 
     143             : #endif