Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : 
       4          16 : bool ModeAlthold::init(bool ignore_checks) {
       5          16 :     if(!sub.control_check_barometer()) {
       6           0 :         return false;
       7             :     }
       8             : 
       9             :     // initialize vertical maximum speeds and acceleration
      10             :     // sets the maximum speed up and down returned by position controller
      11             :     // All limits must be positive
      12          16 :     position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      13          16 :     position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      14             : 
      15             :     // initialise position and desired velocity
      16          16 :     position_control->D_init_controller();
      17             : 
      18          16 :     sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      19             : 
      20          16 :     return true;
      21             : }
      22             : 
      23             : // althold_run - runs the althold controller
      24             : // should be called at 100hz or more
      25      188363 : void ModeAlthold::run()
      26             : {
      27      188363 :     run_pre();
      28      188363 :     control_depth();
      29      188363 :     run_post();
      30      188363 : }
      31             : 
      32      288120 : void ModeAlthold::run_pre()
      33             : {
      34      288120 :     uint32_t tnow = AP_HAL::millis();
      35             : 
      36             :     // initialize vertical speeds and acceleration
      37             :     // All limits must be positive
      38      288120 :     position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      39             : 
      40      288120 :     if (!motors.armed()) {
      41       69768 :         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      42             :         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
      43       69768 :         attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
      44       69768 :         attitude_control->relax_attitude_controllers();
      45       69768 :         position_control->D_relax_controller(motors.get_throttle_hover());
      46       69768 :         sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      47       81328 :         return;
      48             :     }
      49             : 
      50      218352 :     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      51             : 
      52             :     // get pilot desired lean angles
      53             :     float target_roll, target_pitch;
      54             : 
      55             :     // Check if set_attitude_target_no_gps is valid
      56      218352 :     if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
      57             :         float target_yaw;
      58       11560 :         Quaternion(
      59             :             sub.set_attitude_target_no_gps.packet.q
      60       11560 :         ).to_euler(
      61             :             target_roll,
      62             :             target_pitch,
      63             :             target_yaw
      64             :         );
      65       11560 :         target_roll = degrees(target_roll);
      66       11560 :         target_pitch = degrees(target_pitch);
      67       11560 :         target_yaw = degrees(target_yaw);
      68             : 
      69       11560 :         attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
      70       11560 :         return;
      71             :     }
      72             : 
      73      206792 :     sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max_cd());
      74             : 
      75             :     // get pilot's desired yaw rate
      76      206792 :     float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
      77      206792 :     float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
      78             : 
      79             :     // call attitude controller
      80      206792 :     if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
      81       11958 :         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      82       11958 :         sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      83       11958 :         sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
      84             : 
      85             :     } else { // hold current heading
      86             : 
      87             :         // this check is required to prevent bounce back after very fast yaw maneuvers
      88             :         // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
      89      194834 :         if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
      90         297 :             target_yaw_rate = 0; // Stop rotation on yaw axis
      91             : 
      92             :             // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
      93         297 :             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      94         297 :             sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold
      95             : 
      96             :         } else { // call attitude controller holding absolute bearing
      97      194537 :             attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true);
      98             :         }
      99             :     }
     100             : }
     101             : 
     102      288120 : void ModeAlthold::run_post()
     103             : {
     104      288120 :     motors.set_forward(channel_forward->norm_input());
     105      288120 :     motors.set_lateral(channel_lateral->norm_input());
     106      288120 : }
     107             : 
     108      268392 : void ModeAlthold::control_depth() {
     109             :     // return 0.2f when at the surface to p
     110             :     // scale linearly between 0.2f and 1.0f as we approach the surface
     111      268392 :     float distance_to_surface = (g.surface_depth - position_control->get_pos_estimate_U_m() * 100.0f) * 0.01f;
     112      268392 :     distance_to_surface = constrain_float(distance_to_surface, 0.0f, 1.0f);
     113      268392 :     motors.set_max_throttle(g.surface_max_throttle + (1.0f - g.surface_max_throttle) * distance_to_surface);
     114             : 
     115      268392 :     float target_climb_rate_cms = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     116      268392 :     target_climb_rate_cms = constrain_float(target_climb_rate_cms, -sub.get_pilot_speed_dn(), g.pilot_speed_up);
     117             : 
     118             :     // desired_climb_rate returns 0 when within the deadzone.
     119             :     //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
     120      268392 :     if (fabsf(target_climb_rate_cms) < 0.05f)  {
     121      159804 :         if (sub.ap.at_surface) {
     122        2870 :             position_control->set_pos_desired_U_cm(MIN(position_control->get_pos_desired_U_cm(), g.surface_depth)); // set target to 5 cm below surface level
     123      156934 :         } else if (sub.ap.at_bottom) {
     124           0 :             position_control->set_pos_desired_U_cm(MAX(position_control->get_pos_estimate_U_m() * 100.0f + 10.0f, position_control->get_pos_desired_U_cm())); // set target to 10 cm above bottom
     125             :         }
     126             :     }
     127             : 
     128      268392 :     position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate_cms);
     129      268392 :     position_control->D_update_controller();
     130      268392 : }