Line data    Source code

       1             : // ArduSub position hold flight mode
       2             : // GPS required
       3             : // Jacob Walser August 2016
       4             : 
       5             : #include "Sub.h"
       6             : 
       7             : #if POSHOLD_ENABLED
       8             : 
       9             : // poshold_init - initialise PosHold controller
      10           6 : bool ModePoshold::init(bool ignore_checks)
      11             : {
      12             :     // fail to initialise PosHold mode if no GPS lock
      13           6 :     if (!sub.position_ok()) {
      14           0 :         return false;
      15             :     }
      16             : 
      17             :     // initialize vertical speeds and acceleration
      18             :     // All limits must be positive
      19           6 :     position_control->NE_set_max_speed_accel_cm(g.pilot_speed, g.pilot_accel_z);
      20           6 :     position_control->NE_set_correction_speed_accel_cm(g.pilot_speed, g.pilot_accel_z);
      21           6 :     position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      22           6 :     position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
      23             : 
      24             :     // initialise position and desired velocity
      25           6 :     position_control->NE_init_controller_stopping_point();
      26           6 :     position_control->D_init_controller();
      27             : 
      28             :     // Stop all thrusters
      29           6 :     attitude_control->set_throttle_out(NEUTRAL_THROTTLE ,true, g.throttle_filt);
      30           6 :     attitude_control->relax_attitude_controllers();
      31           6 :     position_control->D_relax_controller(0.5f);
      32             : 
      33           6 :     sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      34             : 
      35           6 :     return true;
      36             : }
      37             : 
      38             : // poshold_run - runs the PosHold controller
      39             : // should be called at 100hz or more
      40       86087 : void ModePoshold::run()
      41             : {
      42       86087 :     uint32_t tnow = AP_HAL::millis();
      43             :     // When unarmed, disable motors and stabilization
      44       86087 :     if (!motors.armed()) {
      45        6058 :         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      46             :         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
      47        6058 :         attitude_control->set_throttle_out(NEUTRAL_THROTTLE ,true, g.throttle_filt);
      48        6058 :         attitude_control->relax_attitude_controllers();
      49        6058 :         position_control->NE_init_controller_stopping_point();
      50        6058 :         position_control->D_relax_controller(0.5f);
      51        6058 :         sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      52        6058 :         return;
      53             :     }
      54             : 
      55             :     // set motors to full range
      56       80029 :     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      57             : 
      58             :     /////////////////////
      59             :     // Update attitude //
      60             : 
      61             :     // get pilot's desired yaw rate
      62       80029 :     float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
      63       80029 :     float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
      64             : 
      65             :     // convert pilot input to lean angles
      66             :     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
      67             :     float target_roll, target_pitch;
      68       80029 :     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());
      69             : 
      70             :     // update attitude controller targets
      71       80029 :     if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
      72           0 :         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      73           0 :         sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      74           0 :         sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
      75             : 
      76             :     } else { // hold current heading
      77             : 
      78             :         // this check is required to prevent bounce back after very fast yaw manoeuvres
      79             :         // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
      80       80029 :         if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
      81           0 :             target_yaw_rate = 0; // Stop rotation on yaw axis
      82             : 
      83             :             // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
      84           0 :             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      85           0 :             sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold
      86             : 
      87             :         } else { // call attitude controller holding absolute bearing
      88       80029 :             attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true);
      89             :         }
      90             :     }
      91             : 
      92             :     // update z axis
      93       80029 :     control_depth();
      94             : 
      95             :     // update xy axis
      96             :     // call this after Sub::get_pilot_desired_climb_rate is called so that THR_DZ is reasonable
      97       80029 :     control_horizontal();
      98             : }
      99             : 
     100       80029 : void ModePoshold::control_horizontal() {
     101       80029 :     float lateral_out = 0;
     102       80029 :     float forward_out = 0;
     103             : 
     104             :     // get desired rates in the body frame
     105             :     Vector2f body_rates_cms = {
     106       80029 :         sub.get_pilot_desired_horizontal_rate(channel_forward),
     107       80029 :         sub.get_pilot_desired_horizontal_rate(channel_lateral)
     108       80029 :     };
     109             : 
     110       80029 :     if (sub.position_ok()) {
     111       73639 :         if (!position_control->NE_is_active()) {
     112             :             // the xy controller timed out, re-initialize
     113           1 :             position_control->NE_init_controller_stopping_point();
     114             :         }
     115             : 
     116             :         // convert to the earth frame and set target rates
     117       73639 :         auto earth_rates_cms = ahrs.body_to_earth2D(body_rates_cms);
     118       73639 :         position_control->input_vel_accel_NE_cm(earth_rates_cms, {0, 0});
     119             : 
     120             :         // convert pos control roll and pitch angles back to lateral and forward efforts
     121       73639 :         sub.translate_pos_control_rp(lateral_out, forward_out);
     122             : 
     123             :         // update the xy controller
     124       73639 :         position_control->NE_update_controller();
     125        6390 :     } else if (g.pilot_speed > 0) {
     126             :         // allow the pilot to reposition manually
     127        6390 :         forward_out = body_rates_cms.x / (float)g.pilot_speed;
     128        6390 :         lateral_out = body_rates_cms.y / (float)g.pilot_speed;
     129             :     }
     130             : 
     131       80029 :     motors.set_forward(forward_out);
     132       80029 :     motors.set_lateral(lateral_out);
     133       80029 : }
     134             : #endif  // POSHOLD_ENABLED