// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Plane.h" const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: ENABLE // @DisplayName: Enable QuadPlane // @Description: This enables QuadPlane functionality, assuming quad motors on outputs 5 to 8 // @Values: 0:Disable,1:Enable // @User: Standard AP_GROUPINFO_FLAGS("ENABLE", 1, QuadPlane, enable, 0, AP_PARAM_FLAG_ENABLE), // @Group: M_ // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp AP_SUBGROUPPTR(motors, "M_", 2, QuadPlane, AP_MotorsMulticopter), // @Param: RT_RLL_P // @DisplayName: Roll axis rate controller P gain // @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output // @Range: 0.08 0.30 // @Increment: 0.005 // @User: Standard // @Param: RT_RLL_I // @DisplayName: Roll axis rate controller I gain // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate // @Range: 0.01 0.5 // @Increment: 0.01 // @User: Standard // @Param: RT_RLL_IMAX // @DisplayName: Roll axis rate controller I gain maximum // @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output // @Range: 0 4500 // @Increment: 10 // @Units: Percent*10 // @User: Standard // @Param: RT_RLL_D // @DisplayName: Roll axis rate controller D gain // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate // @Range: 0.001 0.02 // @Increment: 0.001 // @User: Standard AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 3, QuadPlane, AC_PID), // @Param: RT_PIT_P // @DisplayName: Pitch axis rate controller P gain // @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output // @Range: 0.08 0.30 // @Increment: 0.005 // @User: Standard // @Param: RT_PIT_I // @DisplayName: Pitch axis rate controller I gain // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate // @Range: 0.01 0.5 // @Increment: 0.01 // @User: Standard // @Param: RT_PIT_IMAX // @DisplayName: Pitch axis rate controller I gain maximum // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output // @Range: 0 4500 // @Increment: 10 // @Units: Percent*10 // @User: Standard // @Param: RT_PIT_D // @DisplayName: Pitch axis rate controller D gain // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate // @Range: 0.001 0.02 // @Increment: 0.001 // @User: Standard AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 4, QuadPlane, AC_PID), // @Param: RT_YAW_P // @DisplayName: Yaw axis rate controller P gain // @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output // @Range: 0.150 0.50 // @Increment: 0.005 // @User: Standard // @Param: RT_YAW_I // @DisplayName: Yaw axis rate controller I gain // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate // @Range: 0.010 0.05 // @Increment: 0.01 // @User: Standard // @Param: RT_YAW_IMAX // @DisplayName: Yaw axis rate controller I gain maximum // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output // @Range: 0 4500 // @Increment: 10 // @Units: Percent*10 // @User: Standard // @Param: RT_YAW_D // @DisplayName: Yaw axis rate controller D gain // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate // @Range: 0.000 0.02 // @Increment: 0.001 // @User: Standard AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 5, QuadPlane, AC_PID), // P controllers //-------------- // @Param: STB_RLL_P // @DisplayName: Roll axis stabilize controller P gain // @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate // @Range: 3.000 12.000 // @User: Standard AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 6, QuadPlane, AC_P), // @Param: STB_PIT_P // @DisplayName: Pitch axis stabilize controller P gain // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate // @Range: 3.000 12.000 // @User: Standard AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 7, QuadPlane, AC_P), // @Param: STB_YAW_P // @DisplayName: Yaw axis stabilize controller P gain // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate // @Range: 3.000 6.000 // @User: Standard AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 8, QuadPlane, AC_P), // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp AP_SUBGROUPPTR(attitude_control, "A_", 9, QuadPlane, AC_AttitudeControl), // @Param: ANGLE_MAX // @DisplayName: Angle Max // @Description: Maximum lean angle in all flight modes // @Units: Centi-degrees // @Range: 1000 8000 // @User: Advanced AP_GROUPINFO("ANGLE_MAX", 10, QuadPlane, aparm.angle_max, 4500), // @Param: TRANSITION_MS // @DisplayName: Transition time // @Description: Transition time in milliseconds after minimum airspeed is reached // @Units: milli-seconds // @Range: 0 30000 // @User: Advanced AP_GROUPINFO("TRANSITION_MS", 11, QuadPlane, transition_time_ms, 5000), // @Param: PZ_P // @DisplayName: Position (vertical) controller P gain // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller // @Range: 1.000 3.000 // @User: Standard AP_SUBGROUPINFO(p_alt_hold, "PZ_", 12, QuadPlane, AC_P), // @Param: PXY_P // @DisplayName: Position (horizonal) controller P gain // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller // @Range: 0.500 2.000 // @User: Standard AP_SUBGROUPINFO(p_pos_xy, "PXY_", 13, QuadPlane, AC_P), // @Param: VXY_P // @DisplayName: Velocity (horizontal) P gain // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration // @Range: 0.1 6.0 // @Increment: 0.1 // @User: Advanced // @Param: VXY_I // @DisplayName: Velocity (horizontal) I gain // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration // @Range: 0.02 1.00 // @Increment: 0.01 // @User: Advanced // @Param: VXY_IMAX // @DisplayName: Velocity (horizontal) integrator maximum // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output // @Range: 0 4500 // @Increment: 10 // @Units: cm/s/s // @User: Advanced AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 14, QuadPlane, AC_PI_2D), // @Param: VZ_P // @DisplayName: Velocity (vertical) P gain // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller // @Range: 1.000 8.000 // @User: Standard AP_SUBGROUPINFO(p_vel_z, "VZ_", 15, QuadPlane, AC_P), // @Param: AZ_P // @DisplayName: Throttle acceleration controller P gain // @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output // @Range: 0.500 1.500 // @User: Standard // @Param: AZ_I // @DisplayName: Throttle acceleration controller I gain // @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration // @Range: 0.000 3.000 // @User: Standard // @Param: AZ_IMAX // @DisplayName: Throttle acceleration controller I gain maximum // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate // @Range: 0 1000 // @Units: Percent*10 // @User: Standard // @Param: AZ_D // @DisplayName: Throttle acceleration controller D gain // @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration // @Range: 0.000 0.400 // @User: Standard // @Param: AZ_FILT_HZ // @DisplayName: Throttle acceleration filter // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. // @Range: 1.000 100.000 // @Units: Hz // @User: Standard AP_SUBGROUPINFO(pid_accel_z, "AZ_", 16, QuadPlane, AC_PID), // @Group: P_ // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl), // @Param: VELZ_MAX // @DisplayName: Pilot maximum vertical speed // @Description: The maximum vertical velocity the pilot may request in cm/s // @Units: Centimeters/Second // @Range: 50 500 // @Increment: 10 // @User: Standard AP_GROUPINFO("VELZ_MAX", 18, QuadPlane, pilot_velocity_z_max, 250), // @Param: ACCEL_Z // @DisplayName: Pilot vertical acceleration // @Description: The vertical acceleration used when pilot is controlling the altitude // @Units: cm/s/s // @Range: 50 500 // @Increment: 10 // @User: Standard AP_GROUPINFO("ACCEL_Z", 19, QuadPlane, pilot_accel_z, 250), // @Group: WP_ // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp AP_SUBGROUPPTR(wp_nav, "WP_", 20, QuadPlane, AC_WPNav), // @Param: RC_SPEED // @DisplayName: RC output speed in Hz // @Description: This is the PWM refresh rate in Hz for QuadPlane quad motors // @Units: Hz // @Range: 50 500 // @Increment: 10 // @User: Standard AP_GROUPINFO("RC_SPEED", 21, QuadPlane, rc_speed, 490), // @Param: THR_MIN_PWM // @DisplayName: Minimum PWM output // @Description: This is the minimum PWM output for the quad motors // @Units: Hz // @Range: 800 2200 // @Increment: 1 // @User: Standard AP_GROUPINFO("THR_MIN_PWM", 22, QuadPlane, thr_min_pwm, 1000), // @Param: THR_MAX_PWM // @DisplayName: Maximum PWM output // @Description: This is the maximum PWM output for the quad motors // @Units: Hz // @Range: 800 2200 // @Increment: 1 // @User: Standard AP_GROUPINFO("THR_MAX_PWM", 23, QuadPlane, thr_max_pwm, 2000), // @Param: ASSIST_SPEED // @DisplayName: Quadplane assistance speed // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition // @Units: m/s // @Range: 0 100 // @Increment: 0.1 // @User: Standard AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist_speed, 0), // @Param: YAW_RATE_MAX // @DisplayName: Maximum yaw rate // @Description: This is the maximum yaw rate in degrees/second // @Units: degrees/second // @Range: 50 500 // @Increment: 1 // @User: Standard AP_GROUPINFO("YAW_RATE_MAX", 25, QuadPlane, yaw_rate_max, 100), // @Param: LAND_SPEED // @DisplayName: Land speed // @Description: The descent speed for the final stage of landing in cm/s // @Units: cm/s // @Range: 30 200 // @Increment: 10 // @User: Standard AP_GROUPINFO("LAND_SPEED", 26, QuadPlane, land_speed_cms, 50), // @Param: LAND_FINAL_ALT // @DisplayName: Land final altitude // @Description: The altitude at which we should switch to Q_LAND_SPEED descent rate // @Units: m // @Range: 0.5 50 // @Increment: 0.1 // @User: Standard AP_GROUPINFO("LAND_FINAL_ALT", 27, QuadPlane, land_final_alt, 6), // @Param: THR_MID // @DisplayName: Throttle Mid Position // @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover // @User: Standard // @Range: 300 700 // @Units: Percent*10 // @Increment: 1 AP_GROUPINFO("THR_MID", 28, QuadPlane, throttle_mid, 500), // @Param: TRAN_PIT_MAX // @DisplayName: Transition max pitch // @Description: Maximum pitch during transition to auto fixed wing flight // @User: Standard // @Range: 0 30 // @Units: Degrees // @Increment: 1 AP_GROUPINFO("TRAN_PIT_MAX", 29, QuadPlane, transition_pitch_max, 3), // @Param: FRAME_CLASS // @DisplayName: Frame Class // @Description: Controls major frame class for multicopter component // @Values: 0:Quad, 1:Hexa, 2:Octa // @User: Standard AP_GROUPINFO("FRAME_CLASS", 30, QuadPlane, frame_class, 0), // @Param: FRAME_TYPE // @DisplayName: Frame Type (+, X or V) // @Description: Controls motor mixing for multicopter component // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B // @User: Standard AP_GROUPINFO("FRAME_TYPE", 31, QuadPlane, frame_type, 1), AP_GROUPEND }; QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) : ahrs(_ahrs) { AP_Param::setup_object_defaults(this, var_info); } // setup default motors for the frame class void QuadPlane::setup_default_channels(uint8_t num_motors) { for (uint8_t i=0; iget_soft_armed()) { return false; } if (hal.util->available_memory() < 4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav)) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Not enough memory for quadplane"); goto failed; } /* dynamically allocate the key objects for quadplane. This ensures that the objects don't affect the vehicle unless enabled and also saves memory when not in use */ switch ((enum frame_class)frame_class.get()) { case FRAME_CLASS_QUAD: setup_default_channels(4); motors = new AP_MotorsQuad(plane.ins.get_sample_rate()); break; case FRAME_CLASS_HEXA: setup_default_channels(6); motors = new AP_MotorsHexa(plane.ins.get_sample_rate()); break; case FRAME_CLASS_OCTA: setup_default_channels(8); motors = new AP_MotorsOcta(plane.ins.get_sample_rate()); break; default: hal.console->printf("Unknown frame class %u\n", (unsigned)frame_class.get()); goto failed; } if (!motors) { hal.console->printf("Unable to allocate motors\n"); goto failed; } AP_Param::load_object_from_eeprom(motors, motors->var_info); attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw); if (!attitude_control) { hal.console->printf("Unable to allocate attitude_control\n"); goto failed; } AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); pos_control = new AC_PosControl(ahrs, inertial_nav, *motors, *attitude_control, p_alt_hold, p_vel_z, pid_accel_z, p_pos_xy, pi_vel_xy); if (!pos_control) { hal.console->printf("Unable to allocate pos_control\n"); goto failed; } AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); wp_nav = new AC_WPNav(inertial_nav, ahrs, *pos_control, *attitude_control); if (!pos_control) { hal.console->printf("Unable to allocate wp_nav\n"); goto failed; } AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); motors->set_frame_orientation(frame_type); motors->Init(); motors->set_throttle_range(0, thr_min_pwm, thr_max_pwm); motors->set_hover_throttle(throttle_mid); motors->set_update_rate(rc_speed); motors->set_interlock(true); attitude_control->set_dt(plane.ins.get_loop_delta_t()); pid_rate_roll.set_dt(plane.ins.get_loop_delta_t()); pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); pos_control->set_dt(plane.ins.get_loop_delta_t()); // setup the trim of any motors used by AP_Motors so px4io // failsafe will disable motors mask = motors->get_motor_mask(); for (uint8_t i=0; i<16; i++) { if (mask & 1U<radio_trim = thr_min_pwm; } } } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 // redo failsafe mixing on px4 plane.setup_failsafe_mixing(); #endif transition_state = TRANSITION_DONE; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "QuadPlane initialised"); initialised = true; return true; failed: initialised = false; enable.set(0); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "QuadPlane setup failed"); return false; } // init quadplane stabilize mode void QuadPlane::init_stabilize(void) { throttle_wait = false; } // hold in stabilize with given throttle void QuadPlane::hold_stabilize(float throttle_in) { // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, get_desired_yaw_rate_cds(), smoothing_gain); if (throttle_in <= 0) { attitude_control->set_throttle_out_unstabilized(0, true, 0); } else { attitude_control->set_throttle_out(throttle_in, true, 0); } } // quadplane stabilize mode void QuadPlane::control_stabilize(void) { int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; hold_stabilize(pilot_throttle_scaled); } // init quadplane hover mode void QuadPlane::init_hover(void) { // initialize vertical speeds and leash lengths pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_accel_z(pilot_accel_z); // initialise position and desired velocity pos_control->set_alt_target(inertial_nav.get_altitude()); pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); init_throttle_wait(); } /* hold hover with target climb rate */ void QuadPlane::hold_hover(float target_climb_rate) { // initialize vertical speeds and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_accel_z(pilot_accel_z); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, get_desired_yaw_rate_cds(), smoothing_gain); // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); pos_control->update_z_controller(); } /* control QHOVER mode */ void QuadPlane::control_hover(void) { if (throttle_wait) { attitude_control->set_throttle_out_unstabilized(0, true, 0); pos_control->relax_alt_hold_controllers(0); } else { hold_hover(get_pilot_desired_climb_rate_cms()); } } void QuadPlane::init_loiter(void) { // set target to current position wp_nav->init_loiter_target(); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_accel_z(pilot_accel_z); // initialise position and desired velocity pos_control->set_alt_target(inertial_nav.get_altitude()); pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); init_throttle_wait(); } // helper for is_flying() bool QuadPlane::is_flying(void) { if (!available()) { return false; } if (motors->get_throttle() > 200 && !motors->limit.throttle_lower) { return true; } return false; } // crude landing detector to prevent tipover bool QuadPlane::should_relax(void) { bool motor_at_lower_limit = motors->limit.throttle_lower && motors->is_throttle_mix_min(); if (motors->get_throttle() < 10) { motor_at_lower_limit = true; } if (!motor_at_lower_limit) { motors_lower_limit_start_ms = 0; } if (motor_at_lower_limit && motors_lower_limit_start_ms == 0) { motors_lower_limit_start_ms = millis(); } bool relax_loiter = motors_lower_limit_start_ms != 0 && (millis() - motors_lower_limit_start_ms) > 1000; return relax_loiter; } // run quadplane loiter controller void QuadPlane::control_loiter() { if (throttle_wait) { attitude_control->set_throttle_out_unstabilized(0, true, 0); pos_control->relax_alt_hold_controllers(0); wp_nav->init_loiter_target(); return; } if (should_relax()) { wp_nav->loiter_soften_for_landing(); } if (millis() - last_loiter_ms > 500) { wp_nav->init_loiter_target(); } last_loiter_ms = millis(); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_accel_z(pilot_accel_z); // process pilot's roll and pitch input wp_nav->set_pilot_desired_acceleration(plane.channel_roll->control_in, plane.channel_pitch->control_in); // Update EKF speed limit - used to limit speed when we are using optical flow float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_desired_yaw_rate_cds()); // nav roll and pitch are controller by loiter controller plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false); pos_control->update_z_controller(); } /* get pilot input yaw rate in cd/s */ float QuadPlane::get_pilot_input_yaw_rate_cds(void) { if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) { // the user may be trying to disarm return 0; } // add in rudder input return plane.channel_rudder->norm_input() * 100 * yaw_rate_max; } /* get overall desired yaw rate in cd/s */ float QuadPlane::get_desired_yaw_rate_cds(void) { float yaw_cds = 0; if (assisted_flight) { // use bank angle to get desired yaw rate yaw_cds += desired_auto_yaw_rate_cds(); } if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) { // the user may be trying to disarm return 0; } // add in pilot input yaw_cds += get_pilot_input_yaw_rate_cds(); return yaw_cds; } // get pilot desired climb rate in cm/s float QuadPlane::get_pilot_desired_climb_rate_cms(void) { if (plane.failsafe.ch3_failsafe || plane.failsafe.ch3_counter > 0) { // descend at 0.5m/s for now return -50; } uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); uint16_t trim = (plane.channel_throttle->radio_max + plane.channel_throttle->radio_min)/2; return pilot_velocity_z_max * plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) / 100.0f; } /* initialise throttle_wait based on throttle and is_flying() */ void QuadPlane::init_throttle_wait(void) { if (plane.channel_throttle->control_in >= 10 || plane.is_flying()) { throttle_wait = false; } else { throttle_wait = true; } } // set motor arming void QuadPlane::set_armed(bool armed) { if (!initialised) { return; } motors->armed(armed); if (armed) { motors->enable(); } } /* estimate desired climb rate for assistance (in cm/s) */ float QuadPlane::assist_climb_rate_cms(void) { float climb_rate; if (plane.auto_throttle_mode) { // use altitude_error_cm, spread over 10s interval climb_rate = plane.altitude_error_cm / 10; } else { // otherwise estimate from pilot input climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); climb_rate *= plane.channel_throttle->control_in; } climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up()); return climb_rate; } /* calculate desired yaw rate for assistance */ float QuadPlane::desired_auto_yaw_rate_cds(void) { float aspeed; if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) { aspeed = plane.aparm.airspeed_min; } if (aspeed < 1) { aspeed = 1; } float yaw_rate = degrees(GRAVITY_MSS * tanf(radians(plane.nav_roll_cd*0.01f))/aspeed) * 100; return yaw_rate; } /* update for transition from quadplane to fixed wing mode */ void QuadPlane::update_transition(void) { if (plane.control_mode == MANUAL || plane.control_mode == ACRO || plane.control_mode == TRAINING) { // in manual modes quad motors are always off motors->output_min(); transition_state = TRANSITION_DONE; return; } float aspeed; bool have_airspeed = ahrs.airspeed_estimate(&aspeed); /* see if we should provide some assistance */ if (have_airspeed && aspeed < assist_speed && (plane.auto_throttle_mode || plane.channel_throttle->control_in>0 || plane.is_flying())) { // the quad should provide some assistance to the plane transition_state = TRANSITION_AIRSPEED_WAIT; transition_start_ms = millis(); assisted_flight = true; } else { assisted_flight = false; } if (transition_state < TRANSITION_TIMER) { // set a single loop pitch limit in TECS plane.TECS_controller.set_pitch_max_limit(transition_pitch_max); } else if (transition_state < TRANSITION_DONE) { plane.TECS_controller.set_pitch_max_limit((transition_pitch_max+1)*2); } switch (transition_state) { case TRANSITION_AIRSPEED_WAIT: { // we hold in hover until the required airspeed is reached if (transition_start_ms == 0) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed wait"); transition_start_ms = millis(); } if (have_airspeed && aspeed > plane.aparm.airspeed_min && !assisted_flight) { transition_start_ms = millis(); transition_state = TRANSITION_TIMER; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", (double)aspeed); } assisted_flight = true; hold_hover(assist_climb_rate_cms()); attitude_control->rate_controller_run(); motors->output(); last_throttle = motors->get_throttle(); break; } case TRANSITION_TIMER: { // after airspeed is reached we degrade throttle over the // transition time, but continue to stabilize if (millis() - transition_start_ms > (unsigned)transition_time_ms) { transition_state = TRANSITION_DONE; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done"); } float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms; if (throttle_scaled < 0) { throttle_scaled = 0; } assisted_flight = true; hold_stabilize(throttle_scaled); attitude_control->rate_controller_run(); motors->output(); break; } case TRANSITION_DONE: motors->output_min(); break; } } /* update motor output for quadplane */ void QuadPlane::update(void) { if (!setup()) { return; } bool quad_mode = (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER || in_vtol_auto()); if (!quad_mode) { update_transition(); } else { assisted_flight = false; // run low level rate controllers attitude_control->rate_controller_run(); // output to motors motors->output(); transition_start_ms = 0; if (throttle_wait && !plane.is_flying()) { transition_state = TRANSITION_DONE; } else { transition_state = TRANSITION_AIRSPEED_WAIT; } last_throttle = motors->get_throttle(); } // disable throttle_wait when throttle rises above 10% if (throttle_wait && (plane.channel_throttle->control_in > 10 || plane.failsafe.ch3_failsafe || plane.failsafe.ch3_counter>0)) { throttle_wait = false; } } /* update control mode for quadplane modes */ void QuadPlane::control_run(void) { if (!initialised) { return; } switch (plane.control_mode) { case QSTABILIZE: control_stabilize(); break; case QHOVER: control_hover(); break; case QLOITER: control_loiter(); break; default: break; } // we also stabilize using fixed wing surfaces float speed_scaler = plane.get_speed_scaler(); plane.stabilize_roll(speed_scaler); plane.stabilize_pitch(speed_scaler); } /* enter a quadplane mode */ bool QuadPlane::init_mode(void) { if (!setup()) { return false; } if (!initialised) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "QuadPlane mode refused"); return false; } switch (plane.control_mode) { case QSTABILIZE: init_stabilize(); break; case QHOVER: init_hover(); break; case QLOITER: init_loiter(); break; default: break; } return true; } /* handle a MAVLink DO_VTOL_TRANSITION */ bool QuadPlane::handle_do_vtol_transition(const mavlink_command_long_t &packet) { if (!available()) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL not available"); return MAV_RESULT_FAILED; } if (plane.control_mode != AUTO) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO"); return MAV_RESULT_FAILED; } switch ((uint8_t)packet.param1) { case MAV_VTOL_STATE_MC: if (!plane.auto_state.vtol_mode) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); } plane.auto_state.vtol_mode = true; return MAV_RESULT_ACCEPTED; case MAV_VTOL_STATE_FW: if (plane.auto_state.vtol_mode) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Exited VTOL mode"); } plane.auto_state.vtol_mode = false; return MAV_RESULT_ACCEPTED; } plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Invalid VTOL mode"); return MAV_RESULT_FAILED; } /* are we in a VTOL auto state? */ bool QuadPlane::in_vtol_auto(void) { if (plane.control_mode != AUTO) { return false; } if (plane.auto_state.vtol_mode) { return true; } switch (plane.mission.get_current_nav_cmd().id) { case MAV_CMD_NAV_VTOL_LAND: case MAV_CMD_NAV_VTOL_TAKEOFF: return true; default: return false; } } /* handle auto-mode when auto_state.vtol_mode is true */ void QuadPlane::control_auto(const Location &loc) { if (!setup()) { return; } Location origin = inertial_nav.get_origin(); Vector2f diff2d; Vector3f target; diff2d = location_diff(origin, loc); target.x = diff2d.x * 100; target.y = diff2d.y * 100; target.z = loc.alt - origin.alt; if (!locations_are_same(loc, last_auto_target) || loc.alt != last_auto_target.alt || millis() - last_loiter_ms > 500) { wp_nav->set_wp_destination(target); last_auto_target = loc; } last_loiter_ms = millis(); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_accel_z(pilot_accel_z); if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) { /* for takeoff we need to use the loiter controller wpnav controller takes over the descent rate control */ float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds(), smoothing_gain); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll(); plane.nav_pitch_cd = pos_control->get_pitch(); } else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND && land_state >= QLAND_FINAL) { /* for land-final we use the loiter controller */ float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds(), smoothing_gain); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll(); plane.nav_pitch_cd = pos_control->get_pitch(); } else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND) { /* for land repositioning we run the loiter controller */ // also run fixed wing navigation plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); pos_control->set_xy_target(target.x, target.y); float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // nav roll and pitch are controller by position controller plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); if (land_state == QLAND_POSITION) { // during positioning we may be flying faster than the position // controller normally wants to fly. We let that happen by // limiting the pitch controller land_wp_proportion = constrain_float(MAX(land_wp_proportion, plane.auto_state.wp_proportion), 0, 1); int32_t limit = land_wp_proportion * plane.aparm.pitch_limit_max_cd; plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.aparm.pitch_limit_min_cd, limit); wp_nav->set_speed_xy(constrain_float((1-land_wp_proportion)*20*100.0, 500, 2000)); } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds(), smoothing_gain); } else { /* this is full copter control of auto flight */ // run wpnav controller wp_nav->update_wpnav(); // call attitude controller attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_nav->get_yaw(), true); // nav roll and pitch are controller by loiter controller plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); } switch (plane.mission.get_current_nav_cmd().id) { case MAV_CMD_NAV_VTOL_LAND: if (land_state == QLAND_POSITION) { pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false); } else if (land_state > QLAND_POSITION && land_state < QLAND_FINAL) { pos_control->set_alt_target_from_climb_rate(-wp_nav->get_speed_down(), plane.G_Dt, true); } else { pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true); } break; case MAV_CMD_NAV_VTOL_TAKEOFF: pos_control->set_alt_target_from_climb_rate(100, plane.G_Dt, true); break; default: pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false); break; } pos_control->update_z_controller(); } /* start a VTOL takeoff */ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) { if (!setup()) { return false; } plane.set_next_WP(cmd.content.location); plane.next_WP_loc.alt = plane.current_loc.alt + cmd.content.location.alt; throttle_wait = false; // set target to current position wp_nav->init_loiter_target(); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_accel_z(pilot_accel_z); // initialise position and desired velocity pos_control->set_alt_target(inertial_nav.get_altitude()); pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); // also update nav_controller for status output plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); return true; } /* start a VTOL landing */ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) { if (!setup()) { return false; } motors->slow_start(true); pid_rate_roll.reset_I(); pid_rate_pitch.reset_I(); pid_rate_yaw.reset_I(); pid_accel_z.reset_I(); pi_vel_xy.reset_I(); plane.set_next_WP(cmd.content.location); // initially aim for current altitude plane.next_WP_loc.alt = plane.current_loc.alt; land_state = QLAND_POSITION; throttle_wait = false; land_yaw_cd = get_bearing_cd(plane.prev_WP_loc, plane.next_WP_loc); land_wp_proportion = 0; motors_lower_limit_start_ms = 0; Location origin = inertial_nav.get_origin(); Vector2f diff2d; Vector3f target; diff2d = location_diff(origin, plane.next_WP_loc); target.x = diff2d.x * 100; target.y = diff2d.y * 100; target.z = plane.next_WP_loc.alt - origin.alt; wp_nav->set_wp_origin_and_destination(inertial_nav.get_position(), target); pos_control->set_alt_target(inertial_nav.get_altitude()); // also update nav_controller for status output plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); return true; } /* check if a VTOL takeoff has completed */ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) { if (!available()) { return true; } if (plane.current_loc.alt < plane.next_WP_loc.alt) { return false; } transition_state = TRANSITION_AIRSPEED_WAIT; return true; } /* check if a VTOL landing has completed */ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd) { if (!available()) { return true; } if (land_state == QLAND_POSITION && plane.auto_state.wp_distance < 2) { land_state = QLAND_DESCEND; plane.gcs_send_text(MAV_SEVERITY_INFO,"Land descend started"); plane.set_next_WP(cmd.content.location); } if (should_relax()) { wp_nav->loiter_soften_for_landing(); } // at land_final_alt begin final landing if (land_state == QLAND_DESCEND && plane.current_loc.alt < plane.next_WP_loc.alt+land_final_alt*100) { land_state = QLAND_FINAL; pos_control->set_alt_target(inertial_nav.get_altitude()); plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started"); } if (land_state == QLAND_FINAL && (motors_lower_limit_start_ms != 0 && millis() - motors_lower_limit_start_ms > 5000)) { plane.disarm_motors(); land_state = QLAND_COMPLETE; plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete"); } return false; }