Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
       4             : // returns desired angle in centi-degrees
       5      709827 : void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
       6             : {
       7             :     // sanity check angle max parameter
       8      709827 :     const float angle_max_cd = attitude_control.lean_angle_max_cd();
       9             : 
      10             :     // limit max lean angle
      11      709827 :     angle_max = constrain_float(angle_max, 1000, angle_max_cd);
      12             : 
      13             :     // scale roll_in, pitch_in to ATC_ANGLE_MAX parameter range
      14      709827 :     float scaler = angle_max_cd/(float)ROLL_PITCH_INPUT_MAX;
      15      709827 :     roll_in *= scaler;
      16      709827 :     pitch_in *= scaler;
      17             : 
      18             :     // do circular limit
      19      709827 :     float total_in = norm(pitch_in, roll_in);
      20      709827 :     if (total_in > angle_max) {
      21           0 :         float ratio = angle_max / total_in;
      22           0 :         roll_in *= ratio;
      23           0 :         pitch_in *= ratio;
      24             :     }
      25             : 
      26             :     // do lateral tilt to euler roll conversion
      27      709827 :     roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
      28             : 
      29             :     // return
      30      709827 :     roll_out = roll_in;
      31      709827 :     pitch_out = pitch_in;
      32      709827 : }
      33             : 
      34             : // get_pilot_desired_heading - transform pilot's yaw input into a
      35             : // desired yaw rate
      36             : // returns desired yaw rate in centi-degrees per second
      37      867550 : float Sub::get_pilot_desired_yaw_rate(int16_t stick_angle) const
      38             : {
      39             :     // convert pilot input to the desired yaw rate
      40      867550 :     return stick_angle * g.acro_yaw_p;
      41             : }
      42             : 
      43             : // check for ekf yaw reset and adjust target heading
      44     2180683 : void Sub::check_ekf_yaw_reset()
      45             : {
      46     2180683 :     const uint16_t new_ahrs_yaw_reset_count = ahrs.get_yaw_reset_count();
      47     2180683 :     if (new_ahrs_yaw_reset_count != ahrs_yaw_reset_count) {
      48         353 :         attitude_control.inertial_frame_reset();
      49         353 :         ahrs_yaw_reset_count = new_ahrs_yaw_reset_count;
      50             :     }
      51     2180683 : }
      52             : 
      53             : /*************************************************************
      54             :  * yaw controllers
      55             :  *************************************************************/
      56             : 
      57             : // get_roi_yaw - returns heading towards location held in roi_WP
      58             : // should be called at 100hz
      59           0 : float Sub::get_roi_yaw()
      60             : {
      61             :     static uint8_t roi_yaw_counter = 0;     // used to reduce update rate to 100hz
      62             : 
      63           0 :     roi_yaw_counter++;
      64           0 :     if (roi_yaw_counter >= 4) {
      65           0 :         roi_yaw_counter = 0;
      66           0 :         yaw_look_at_WP_bearing = get_bearing_cd((pos_control.get_pos_estimate_NED_m().xy() * 100.0f).tofloat(), roi_WP_neu_cm.xy());
      67             :     }
      68             : 
      69           0 :     return yaw_look_at_WP_bearing;
      70             : }
      71             : 
      72           0 : float Sub::get_look_ahead_yaw()
      73             : {
      74           0 :     Vector3f vel = (pos_control.get_vel_estimate_NED_ms() * 100.0f).tofloat();
      75           0 :     vel.z = -vel.z;
      76           0 :     const float speed_sq = vel.xy().length_squared();
      77             :     // Commanded Yaw to automatically look ahead.
      78           0 :     if (position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
      79           0 :         yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
      80             :     }
      81           0 :     return yaw_look_ahead_bearing;
      82             : }
      83             : 
      84             : /*************************************************************
      85             :  *  throttle control
      86             :  ****************************************************************/
      87             : 
      88             : // get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
      89             : // without any deadzone at the bottom
      90      364062 : float Sub::get_pilot_desired_climb_rate(float throttle_control)
      91             : {
      92             :     // throttle failsafe check
      93      364062 :     if (failsafe.pilot_input) {
      94           0 :         return 0.0f;
      95             :     }
      96             : 
      97             :     // ensure a reasonable throttle value
      98      364062 :     throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
      99             : 
     100             :     // ensure a reasonable deadzone
     101      364062 :     g.throttle_deadzone.set(constrain_int16(g.throttle_deadzone, 0, 400));
     102             : 
     103      364062 :     float mid_stick = channel_throttle->get_control_mid();
     104      364062 :     float deadband_top = mid_stick + g.throttle_deadzone * gain;
     105      364062 :     float deadband_bottom = mid_stick - g.throttle_deadzone * gain;
     106             : 
     107             :     // check throttle is above, below or in the deadband
     108      364062 :     if (throttle_control < deadband_bottom) {
     109             :         // below the deadband
     110       98651 :         return get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
     111      265411 :     } else if (throttle_control > deadband_top) {
     112             :         // above the deadband
     113       20491 :         return g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
     114             :     } else {
     115             :         // must be in the deadband
     116      244920 :         return 0.0f;
     117             :     }
     118             : }
     119             : 
     120             : // behavior is similar to Sub::get_pilot_desired_climb_rate
     121      160058 : float Sub::get_pilot_desired_horizontal_rate(RC_Channel *channel) const
     122             : {
     123      160058 :     if (failsafe.pilot_input) {
     124           0 :         return 0;
     125             :     }
     126             : 
     127             :     // forward and lateral sticks have center trim, unlike throttle
     128      160058 :     auto control = channel->norm_input();
     129             : 
     130             :     // normalize deadzone
     131      160058 :     auto dz = (float)g.throttle_deadzone * 2.0f / (float)(channel->get_radio_max() - channel->get_radio_min());
     132      160058 :     auto deadband_top = dz * gain;
     133      160058 :     auto deadband_bottom = -dz * gain;
     134             : 
     135      160058 :     if (control < deadband_bottom) {
     136             :         // below the deadband
     137           0 :         return (float)g.pilot_speed * (control - deadband_bottom);
     138      160058 :     } else if (control > deadband_top) {
     139             :         // above the deadband
     140       31108 :         return (float)g.pilot_speed * (control - deadband_top);
     141             :     } else {
     142             :         // must be in the deadband
     143      128950 :         return 0;
     144             :     }
     145             : }
     146             : 
     147             : // rotate vector from vehicle's perspective to North-East frame
     148           0 : void Sub::rotate_body_frame_to_NE(float &x, float &y)
     149             : {
     150           0 :     float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
     151           0 :     float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
     152           0 :     x = ne_x;
     153           0 :     y = ne_y;
     154           0 : }
     155             : 
     156             : // It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
     157      753145 : uint16_t Sub::get_pilot_speed_dn() const
     158             : {
     159      753145 :     if (g.pilot_speed_dn == 0) {
     160      753145 :         return abs(g.pilot_speed_up);
     161             :     }
     162           0 :     return abs(g.pilot_speed_dn);
     163             : }