Controller

implementation for
important modes
 MODES
• Stabilize mode allows to fly vehicle manually. It self-levels the roll and pitch axis.
• Altitude Hold Mode - In this mode, the throttle is automatically controlled to maintain the
current altitude. Roll, Pitch and yaw operate the same as in Stabilize. The pilot directly controls
the roll, pitch lean angles and the heading.
• Acro Mode uses the RC sticks to control the angular velocity of the copter in each axis.
Release the sticks and the vehicle will maintain its current attitude and will not return to level
(attitude hold). Acro mode is useful for aerobatics such as flips or rolls, or FPV when smooth
and fast control is desired.
• Loiter Mode automatically attempts to maintain the current location, heading and altitude. It is
like manual flight mode but when the sticks are released, the vehicle will slow to a stop and hold
position.
• Circle Mode - UAV will orbit a point located CIRCLE_RADIUS centimeters in front of the
vehicle with the nose of the vehicle pointed at the center.
 STABLIZE MODE
stabilize_run - runs the main stabilize controller
❑ Apply simple mode [It allows to fly copter as though it were pointed in the direction it was pointed when it was armed
regardless of its current heading orientation]
❑ convert pilot input to lean angles
❑ get pilot's desired yaw rate

➢ If motor not armed => stop motor [SHUT_DOWN] resets the target yaw angle and rate and integral terms of the rate
controller to zero.
• Elseif UAV is attempting to land => set motors to ground idle(no spin) [GROUND_IDLE] resets the target yaw angle and
rate to zero and integral terms of the rate controller to zero gradually.
• Else => set Motor to Full throtle [THROTTLE_UNLIMITED] clears landing flags (inform that copter is not
performing landing manuever) above zero throttle.
• For SPOOLING_UP and SPOOLING_DOWN do nothing.

❑ Update the copter's attitude control setpoints euler angles generated based on the copter's current attitude and the desired
attitude [desired roll, pitch, and yaw rate].
❑ Set the copter's throtle output of attitude controller based on desired throttle value that is specified by the pilot by moving
sticks.
 ALTITUDE HOLD MODE
o If vertical Position Controller is not active, then initialize the vertical position controller.
o Set the maximum and correction speed and acceleration limit values for the copter's position controller.
althold_run - runs the althold controller
❑ Set vertical speed and acceleration limits
❑ Apply simple mode transform to pilot inputs
❑ get pilot desired lean angles (target_roll, target_pitch)
❑ get pilot's desired yaw rate based on yaw channel inputs from pilot.
❑ get pilot's desired climb rate from the throttle channel input.
➢ AltHold_MotorStopped resets the target yaw angle and rate and integral terms of the rate controller to zero. pos_control-
>relax_z_controller(0.0f) forces throttle output to decay to zero.
➢ AltHold_Landed_Ground_Idle resets the target yaw angle and rate and integral terms of the rate controller.
• AltHold_Landed_Pre_Takeoff reset integral terms of the rate controllerto zero gradually and Z controller forces throttle
output to decay to zero.
➢ AltHold_Takeoff
• firstly checks if the takeoff is running. If not, it starts the takeoff process using the takeoff.start() function and passes the
desired takeoff altitude as an argument.
• Next, It calculates the target climb rate, which is adjusted for avoidance if necessary.
• Finally, it sets the position controller targets that are adjusted for the pilot input using the takeoff.do_pilot_takeoff() function.
The target_climb_rate is passed as an argument to this function to adjust the climb rate during takeoff.
➢ AltHold_Flying
• Motors are set to the [THROTTLE_UNLIMITED no throttle limits imposed on the motors].
• If obstacle avoidance is enabled [AC_AVOID_ENABLED], then the commanded roll and pitch angles are adjusted to avoid
obstacles using the copter.avoid.adjust_roll_pitch() function.
• The target climb rate is adjusted based on obstacle avoidance using the get_avoidance_adjusted_climbrate() function.
• The vertical offset based on surface measurement is updated using the copter.surface_tracking.update_surface_offset()
function.
• Finally, the commanded climb rate is sent to the position controller using the pos_control-
>set_pos_target_z_from_climb_rate_cm() function.
❑ Update the copter's attitude control setpoints euler angles generated based on the copter's current altitude and the deired altitude
[desired roll, pitch, and yaw rate].
❑ Run the vertical position controller and set output throttle.
 ACRO MODE
• ModeAcro::run()
• Get desired attitude(Roll, Pitch, Yaw) from input on Roll, Pitch and Yaw channels by pilot.
➢ If motor not armed => stop motor [SHUT_DOWN] resets the target yaw angle and rate and integral terms of the
rate controller to zero.
• Elseif UAV is attempting to land => set motors to ground idle(no spin) [GROUND_IDLE] resets the target yaw angle
and rate to zero and integral terms of the rate controller to zero gradually.
• Else => set Motor to Full throtle [THROTTLE_UNLIMITED] clears landing flags (inform that copter is
not performing landing manuever) above zero throttle.
• For SPOOLING_UP and SPOOLING_DOWN do nothing.
• If the RATE_LOOP_ONLY flag is set, the copter is configured to use only the rate controller to control its roll, pitch, and yaw
rates in acro mode. set the desired roll, pitch, and yaw rates directly as input to the rate controller. Using only the rate
controller provides more precise control over the copter's rates.
• Else set the desired roll, pitch, and yaw angles as input to the attitude controller, which then generates the desired roll, pitch,
and yaw rates as input to the rate controller. Using both controllers can provide smoother and more stable flight.
• Set the desired throttle output for the copter based on the pilot's desired throttle input and the current state of the copter.
• If AIR_MODE option is enabled in the g2.acro_options parameter, disable_air_mode_reset flag is set to false [Air
Mode reset feature should not be disabled]. Additionally, the copter.air_mode flag is set to AIRMODE_ENABLED[Air Mode
is currently active].
• If the disable_air_mode_reset flag is not set to true and the "air mode" option is enabled in g2.acro_options, the air_mode
property of the copter object is set to AIRMODE_DISABLED. This property controls whether the flight controller should
apply special handling to the throttle output in acro mode when the aircraft is descending.
• air_mode_aux_changed() sets the disable_air_mode_reset flag to true, which prevents the air_mode property from being
reset to its default value of AIRMODE_DISABLED when the Acro mode is exited. This flag is cleared when the Acro mode is
exited or when air_mode_aux_changed() is called again.
• ModeAcro::throttle_hover() - If the acro_thr_mid parameter is greater than zero, the method returns that value. Otherwise, it
calls the throttle_hover() method of the parent Mode class to get the default hover throttle value.
• get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
 LOITER MODE
• loiter_init - initialise loiter controller
➢ if the copter's radio failsafe is not active then target_roll and target_pitch is set, and simple mode transform are aplied to pilot
inputs. convert pilot's stick input to lean angles. process pilot's roll and pitch input
➢ Else in case of radio failsafe event occurs then reset the pilot's desired acceleration to zero[maintain current velocity and
position].
❑ initialize the loiter target for the copter based on the current GPS location of the copter. This target is used by the copter's
autopilot system to calculate the required attitude and thrust to maintain the copter's position and altitude in loiter mode.
❑ initialise the vertical position controller
❑ Set the maximum and correction verical speed and acceleration limit values for the copter's vertical position controller.
• loiter_run - runs the loiter controller
❑ set vertical speed and acceleration limits
❑ convert pilot input to lean angles
❑ process pilot's roll and pitch input
❑ get pilot's desired yaw rate
❑ get pilot desired climb rate
❑ clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
❑ relax loiter target if we might be landed
• Loiter State Machine
➢ AltHold_MotorStopped
➢ AltHold_Takeoff
➢ AltHold_Landed_Ground_Idle
➢ AltHold_Landed_Pre_Takeoff
➢ AltHold_Flying
❑ call attitude controller
❑ get avoidance adjusted climb rate
❑ update the vertical offset based on the surface measurement
❑ Send the commanded climb rate to the position controller
❑ run the vertical position controller and set output throttle
❑ Get distance and bearing to target
 CIRCLE MODE
• circle_init - initialise circle controller flight mode
❑ Set pilot_yaw_override to zero [copter's yaw control will be determined by both the pilot's inputs and the autopilot system].
❑ Set speed_changing to zero [copter's speed is stable, and the autopilot system only needs to maintain the copter's attitude and heading].
❑ Set the maximum and correction speed and acceleration limit values for the copter's both vertical and XY position controller.
❑ initialise circle controller including setting the circle center based on vehicle speed.
• circle_run - runs the circle flight mode
❑ set speed and acceleration limits
❑ get pilot's desired yaw rate from yaw channel input from pilot (zero if in radio failsafe).
❑ Check for any change in parameters and update in real time.
❑ update the circle controller's radius target based on pilot pitch stick inputs.
❑ update the orbicular rate target based on pilot roll stick inputs
❑ get pilot desired climb rate (or zero if in radio failsafe)
❑ get avoidance adjusted climb rate
❑ if not armed set throttle to zero and exit immediately
❑ set motors to full range
❑ update the vertical offset based on the surface measurement
❑ call attitude controller and calculate distance and bearing to target.
 REFRENCES
• https://github.com/Gopesh03/Copter-4.2.3A/tree/Gopesh03-patch-
1/ArduCopter
• https://ardupilot.org/copter/docs