Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : 
       4           6 : bool ModeSurface::init(bool ignore_checks)
       5             : {
       6           6 :     nobaro_mode = !sub.control_check_barometer();
       7             : 
       8             :     // initialize vertical speeds and acceleration
       9             :     // All limits must be positive
      10           6 :     position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      11           6 :     position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      12             : 
      13             :     // initialise position and desired velocity
      14           6 :     position_control->D_init_controller();
      15             : 
      16           6 :     return true;
      17             : 
      18             : }
      19             : 
      20       24428 : void ModeSurface::run()
      21             : {
      22             :     float target_roll, target_pitch;
      23             : 
      24             :     // if not armed set throttle to zero and exit immediately
      25       24428 :     if (!motors.armed()) {
      26        4721 :         motors.output_min();
      27        4721 :         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      28        4721 :         attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
      29        4721 :         attitude_control->relax_attitude_controllers();
      30        4721 :         position_control->D_init_controller();
      31        4721 :         return;
      32             :     }
      33             : 
      34             :     // If no barometer is available, use the surface_nobaro_thrust parameter to set the throttle output
      35       19707 :     if (nobaro_mode) {
      36       19704 :         float thrust_output = 0.5f + g2.surface_nobaro_thrust * 0.005f; // map -100, 100 to 0, 1
      37       19704 :         attitude_control->set_throttle_out(thrust_output, true, g.throttle_filt);
      38             :     } else {
      39             :         // Already at surface, hold depth at surface
      40           3 :         if (sub.ap.at_surface) {
      41           2 :             set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE);
      42             :         }
      43             : 
      44             :         // convert pilot input to lean angles
      45             :         // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
      46           3 :         sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd());
      47             : 
      48             :         // get pilot's desired yaw rate
      49           3 :         float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
      50             : 
      51             :         // call attitude controller
      52           3 :         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      53             : 
      54             :         // set target climb rate
      55           3 :         float cmb_rate_cms = constrain_float(fabsf(sub.wp_nav.get_default_speed_up_cms()), 1, position_control->get_max_speed_up_cms());
      56             : 
      57             :         // update altitude target and call position controller
      58           3 :         position_control->D_set_pos_target_from_climb_rate_cms(cmb_rate_cms);
      59           3 :         position_control->D_update_controller();
      60             :     }
      61             :     // pilot has control for repositioning
      62       19707 :     motors.set_forward(channel_forward->norm_input());
      63       19707 :     motors.set_lateral(channel_lateral->norm_input());
      64             : }