Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : 
       4           7 : bool ModeStabilize::init(bool ignore_checks) {
       5             :     // set target altitude to zero for reporting
       6           7 :     position_control->set_pos_desired_U_cm(0);
       7           7 :     sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
       8             : 
       9           7 :     return true;
      10             :     return true;
      11             : }
      12             : 
      13       17939 : void ModeStabilize::run()
      14             : {
      15       17939 :   uint32_t tnow = AP_HAL::millis();
      16             :     float target_roll, target_pitch;
      17             : 
      18             :     // if not armed set throttle to zero and exit immediately
      19       17939 :     if (!motors.armed()) {
      20         441 :         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      21         441 :         attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
      22         441 :         attitude_control->relax_attitude_controllers();
      23         441 :         sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      24         441 :         return;
      25             :     }
      26             : 
      27       17498 :     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      28             : 
      29             :     // convert pilot input to lean angles
      30             :     // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
      31             :     // TODO2: move into mode.h
      32       17498 :     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());
      33             : 
      34             :     // get pilot's desired yaw rate
      35       17498 :     float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
      36       17498 :     float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
      37             : 
      38             :     // call attitude controller
      39             :     // update attitude controller targets
      40             : 
      41       17498 :     if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
      42        3352 :         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      43        3352 :         sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
      44        3352 :         sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
      45             : 
      46             :     } else { // hold current heading
      47             : 
      48             :         // this check is required to prevent bounce back after very fast yaw maneuvers
      49             :         // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
      50       14146 :         if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
      51          99 :             target_yaw_rate = 0;  // Stop rotation on yaw axis
      52             : 
      53             :             // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
      54          99 :             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
      55          99 :             sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold
      56             : 
      57             :         } else { // call attitude controller holding absolute absolute bearing
      58       14047 :             attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true);
      59             :         }
      60             :     }
      61             : 
      62             :     // output pilot's throttle
      63       17498 :     attitude_control->set_throttle_out((channel_throttle->norm_input() + 1.0f) / 2.0f, false, g.throttle_filt);
      64             : 
      65             :     //control_in is range -1000-1000
      66             :     //radio_in is raw pwm value
      67       17498 :     motors.set_forward(channel_forward->norm_input());
      68       17498 :     motors.set_lateral(channel_lateral->norm_input());
      69             : }