Attitude.cpp 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205
  1. #include "Sub.h"
  2. // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
  3. // returns desired angle in centi-degrees
  4. void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
  5. {
  6. // sanity check angle max parameter
  7. aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
  8. // limit max lean angle
  9. angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
  10. // scale roll_in, pitch_in to ANGLE_MAX parameter range
  11. float scaler = aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
  12. roll_in *= scaler;
  13. pitch_in *= scaler;
  14. // do circular limit
  15. float total_in = norm(pitch_in, roll_in);
  16. if (total_in > angle_max) {
  17. float ratio = angle_max / total_in;
  18. roll_in *= ratio;
  19. pitch_in *= ratio;
  20. }
  21. // do lateral tilt to euler roll conversion
  22. roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
  23. // return
  24. roll_out = roll_in;
  25. pitch_out = pitch_in;
  26. }
  27. // get_pilot_desired_heading - transform pilot's yaw input into a
  28. // desired yaw rate
  29. // returns desired yaw rate in centi-degrees per second
  30. float Sub::get_pilot_desired_yaw_rate(int16_t stick_angle)
  31. {
  32. // convert pilot input to the desired yaw rate
  33. return stick_angle * g.acro_yaw_p;
  34. }
  35. // check for ekf yaw reset and adjust target heading
  36. void Sub::check_ekf_yaw_reset()
  37. {
  38. float yaw_angle_change_rad;
  39. uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
  40. if (new_ekfYawReset_ms != ekfYawReset_ms) {
  41. attitude_control.inertial_frame_reset();
  42. ekfYawReset_ms = new_ekfYawReset_ms;
  43. }
  44. }
  45. /*************************************************************
  46. * yaw controllers
  47. *************************************************************/
  48. // get_roi_yaw - returns heading towards location held in roi_WP
  49. // should be called at 100hz
  50. float Sub::get_roi_yaw()
  51. {
  52. static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz
  53. roi_yaw_counter++;
  54. if (roi_yaw_counter >= 4) {
  55. roi_yaw_counter = 0;
  56. yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
  57. }
  58. return yaw_look_at_WP_bearing;
  59. }
  60. float Sub::get_look_ahead_yaw()
  61. {
  62. const Vector3f& vel = inertial_nav.get_velocity();
  63. float speed = norm(vel.x,vel.y);
  64. // Commanded Yaw to automatically look ahead.
  65. if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
  66. yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
  67. }
  68. return yaw_look_ahead_bearing;
  69. }
  70. /*************************************************************
  71. * throttle control
  72. ****************************************************************/
  73. // get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
  74. // without any deadzone at the bottom
  75. float Sub::get_pilot_desired_climb_rate(float throttle_control)
  76. {
  77. // throttle failsafe check
  78. if (failsafe.pilot_input) {
  79. return 0.0f;
  80. }
  81. float desired_rate = 0.0f;
  82. float mid_stick = channel_throttle->get_control_mid();
  83. float deadband_top = mid_stick + g.throttle_deadzone;
  84. float deadband_bottom = mid_stick - g.throttle_deadzone;
  85. // ensure a reasonable throttle value
  86. throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
  87. // ensure a reasonable deadzone
  88. g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
  89. // check throttle is above, below or in the deadband
  90. if (throttle_control < deadband_bottom) {
  91. // below the deadband
  92. desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
  93. } else if (throttle_control > deadband_top) {
  94. // above the deadband
  95. desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
  96. } else {
  97. // must be in the deadband
  98. desired_rate = 0.0f;
  99. }
  100. // desired climb rate for logging
  101. desired_climb_rate = desired_rate;
  102. return desired_rate;
  103. }
  104. // get_surface_tracking_climb_rate - hold vehicle at the desired distance above the ground
  105. // returns climb rate (in cm/s) which should be passed to the position controller
  106. float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
  107. {
  108. #if RANGEFINDER_ENABLED == ENABLED
  109. static uint32_t last_call_ms = 0;
  110. float distance_error;
  111. float velocity_correction;
  112. float current_alt = inertial_nav.get_altitude();
  113. uint32_t now = AP_HAL::millis();
  114. // reset target altitude if this controller has just been engaged
  115. if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
  116. target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
  117. }
  118. last_call_ms = now;
  119. // adjust rangefinder target alt if motors have not hit their limits
  120. if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
  121. target_rangefinder_alt += target_rate * dt;
  122. }
  123. // do not let target altitude get too far from current altitude above ground
  124. // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
  125. target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
  126. // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
  127. distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
  128. velocity_correction = distance_error * g.rangefinder_gain;
  129. velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
  130. // return combined pilot climb rate + rate to correct rangefinder alt error
  131. return (target_rate + velocity_correction);
  132. #else
  133. return (float)target_rate;
  134. #endif
  135. }
  136. // updates position controller's maximum altitude using fence and EKF limits
  137. void Sub::update_poscon_alt_max()
  138. {
  139. // minimum altitude, ie. maximum depth
  140. // interpreted as no limit if left as zero
  141. float min_alt_cm = 0.0;
  142. // no limit if greater than 100, a limit is necessary,
  143. // or the vehicle will try to fly out of the water
  144. float max_alt_cm = g.surface_depth; // minimum depth
  145. #if AC_FENCE == ENABLED
  146. // set fence altitude limit in position controller
  147. if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
  148. min_alt_cm = fence.get_safe_alt_min()*100.0f;
  149. max_alt_cm = fence.get_safe_alt_max()*100.0f;
  150. }
  151. #endif
  152. // pass limit to pos controller
  153. pos_control.set_alt_min(min_alt_cm);
  154. pos_control.set_alt_max(max_alt_cm);
  155. }
  156. // rotate vector from vehicle's perspective to North-East frame
  157. void Sub::rotate_body_frame_to_NE(float &x, float &y)
  158. {
  159. float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
  160. float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
  161. x = ne_x;
  162. y = ne_y;
  163. }
  164. // It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
  165. uint16_t Sub::get_pilot_speed_dn()
  166. {
  167. if (g.pilot_speed_dn == 0) {
  168. return abs(g.pilot_speed_up);
  169. }
  170. return abs(g.pilot_speed_dn);
  171. }