mirror of https://github.com/ArduPilot/ardupilot
66 lines
2.3 KiB
C++
66 lines
2.3 KiB
C++
#include "Plane.h"
|
|
#include "qautotune.h"
|
|
|
|
#if QAUTOTUNE_ENABLED
|
|
|
|
/*
|
|
initialise QAUTOTUNE mode
|
|
*/
|
|
bool QAutoTune::init()
|
|
{
|
|
if (!plane.quadplane.available()) {
|
|
return false;
|
|
}
|
|
|
|
// use position hold while tuning if we were in QLOITER
|
|
bool position_hold = (plane.previous_mode == &plane.mode_qloiter);
|
|
|
|
return init_internals(position_hold,
|
|
plane.quadplane.attitude_control,
|
|
plane.quadplane.pos_control,
|
|
plane.quadplane.ahrs_view,
|
|
&plane.quadplane.inertial_nav);
|
|
}
|
|
|
|
float QAutoTune::get_pilot_desired_climb_rate_cms(void) const
|
|
{
|
|
return plane.quadplane.get_pilot_desired_climb_rate_cms();
|
|
}
|
|
|
|
void QAutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds)
|
|
{
|
|
if (plane.channel_roll->get_control_in() == 0 && plane.channel_pitch->get_control_in() == 0) {
|
|
des_roll_cd = 0;
|
|
des_pitch_cd = 0;
|
|
} else {
|
|
des_roll_cd = plane.nav_roll_cd;
|
|
des_pitch_cd = plane.nav_pitch_cd;
|
|
}
|
|
yaw_rate_cds = plane.quadplane.get_desired_yaw_rate_cds();
|
|
}
|
|
|
|
void QAutoTune::init_z_limits()
|
|
{
|
|
// set vertical speed and acceleration limits
|
|
plane.quadplane.pos_control->set_max_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(),
|
|
plane.quadplane.pilot_speed_z_max_up*100,
|
|
plane.quadplane.pilot_accel_z*100);
|
|
plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(),
|
|
plane.quadplane.pilot_speed_z_max_up*100,
|
|
plane.quadplane.pilot_accel_z*100);
|
|
}
|
|
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
// log VTOL PIDs for during twitch
|
|
void QAutoTune::log_pids(void)
|
|
{
|
|
AP::logger().Write_PID(LOG_PIQR_MSG, plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
|
AP::logger().Write_PID(LOG_PIQP_MSG, plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
|
AP::logger().Write_PID(LOG_PIQY_MSG, plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
|
}
|
|
#endif
|
|
|
|
#endif // QAUTOTUNE_ENABLED
|
|
|