mirror of https://github.com/ArduPilot/ardupilot
Copter: add support for sysid of vel and pos loops
This commit is contained in:
parent
84e52378cf
commit
1bf865e4eb
|
@ -122,6 +122,15 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y)
|
||||||
y = ne_y;
|
y = ne_y;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// rotate vector from vehicle's target frame perspective to North-East frame
|
||||||
|
void Copter::rotate_target_body_frame_to_NE(float &x, float &y)
|
||||||
|
{
|
||||||
|
float ne_x = x*cosf(attitude_control->get_att_target_euler_rad().z) - y*sinf(attitude_control->get_att_target_euler_rad().z);
|
||||||
|
float ne_y = x*sinf(attitude_control->get_att_target_euler_rad().z) + y*cosf(attitude_control->get_att_target_euler_rad().z);
|
||||||
|
x = ne_x;
|
||||||
|
y = ne_y;
|
||||||
|
}
|
||||||
|
|
||||||
// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
|
// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
|
||||||
uint16_t Copter::get_pilot_speed_dn() const
|
uint16_t Copter::get_pilot_speed_dn() const
|
||||||
{
|
{
|
||||||
|
|
|
@ -722,6 +722,7 @@ private:
|
||||||
float get_non_takeoff_throttle();
|
float get_non_takeoff_throttle();
|
||||||
void set_accel_throttle_I_from_pilot_throttle();
|
void set_accel_throttle_I_from_pilot_throttle();
|
||||||
void rotate_body_frame_to_NE(float &x, float &y);
|
void rotate_body_frame_to_NE(float &x, float &y);
|
||||||
|
void rotate_target_body_frame_to_NE(float &x, float &y);
|
||||||
uint16_t get_pilot_speed_dn() const;
|
uint16_t get_pilot_speed_dn() const;
|
||||||
void run_rate_controller();
|
void run_rate_controller();
|
||||||
|
|
||||||
|
|
|
@ -1647,6 +1647,12 @@ private:
|
||||||
MIX_PITCH = 11, // mixer pitch axis is being excited
|
MIX_PITCH = 11, // mixer pitch axis is being excited
|
||||||
MIX_YAW = 12, // mixer pitch axis is being excited
|
MIX_YAW = 12, // mixer pitch axis is being excited
|
||||||
MIX_THROTTLE = 13, // mixer throttle axis is being excited
|
MIX_THROTTLE = 13, // mixer throttle axis is being excited
|
||||||
|
DISTURB_POS_LAT = 14,
|
||||||
|
DISTURB_POS_LONG = 15,
|
||||||
|
DISTURB_VEL_LAT = 16,
|
||||||
|
DISTURB_VEL_LONG = 17,
|
||||||
|
INPUT_LOITER_LAT = 18,
|
||||||
|
INPUT_LOITER_LONG = 19,
|
||||||
};
|
};
|
||||||
|
|
||||||
AP_Int8 axis; // Controls which axis are being excited. Set to non-zero to display other parameters
|
AP_Int8 axis; // Controls which axis are being excited. Set to non-zero to display other parameters
|
||||||
|
@ -1663,7 +1669,8 @@ private:
|
||||||
float waveform_freq_rads; // Instantaneous waveform frequency
|
float waveform_freq_rads; // Instantaneous waveform frequency
|
||||||
float time_const_freq; // Time at constant frequency before chirp starts
|
float time_const_freq; // Time at constant frequency before chirp starts
|
||||||
int8_t log_subsample; // Subsample multiple for logging.
|
int8_t log_subsample; // Subsample multiple for logging.
|
||||||
|
Vector2f target_vel; // target velocity for position controller modes
|
||||||
|
|
||||||
// System ID states
|
// System ID states
|
||||||
enum class SystemIDModeState {
|
enum class SystemIDModeState {
|
||||||
SYSTEMID_STATE_STOPPED,
|
SYSTEMID_STATE_STOPPED,
|
||||||
|
|
|
@ -81,14 +81,50 @@ bool ModeSystemId::init(bool ignore_checks)
|
||||||
}
|
}
|
||||||
|
|
||||||
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
||||||
if (motors->armed() && copter.ap.land_complete && !copter.flightmode->has_manual_throttle()) {
|
if (motors->armed() && copter.ap.land_complete && (!copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::LOITER)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG
|
||||||
|
&& (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG
|
||||||
|
&& (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) {
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
copter.input_manager.set_use_stab_col(true);
|
copter.input_manager.set_use_stab_col(true);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
if (!copter.failsafe.radio) {
|
||||||
|
float target_roll, target_pitch;
|
||||||
|
// apply SIMPLE mode transform to pilot inputs
|
||||||
|
update_simple_mode();
|
||||||
|
|
||||||
|
// convert pilot input to lean angles
|
||||||
|
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
|
||||||
|
|
||||||
|
// process pilot's roll and pitch input
|
||||||
|
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
|
||||||
|
} else {
|
||||||
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
||||||
|
loiter_nav->clear_pilot_desired_acceleration();
|
||||||
|
}
|
||||||
|
loiter_nav->init_target();
|
||||||
|
|
||||||
|
// initialise the vertical position controller
|
||||||
|
if (!pos_control->is_active_z()) {
|
||||||
|
pos_control->init_z_controller();
|
||||||
|
}
|
||||||
|
|
||||||
|
// set vertical speed and acceleration limits
|
||||||
|
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||||
|
pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||||
|
|
||||||
|
// #if AC_PRECLAND_ENABLED
|
||||||
|
// _precision_loiter_active = false;
|
||||||
|
// #endif
|
||||||
|
}
|
||||||
|
|
||||||
att_bf_feedforward = attitude_control->get_bf_feedforward();
|
att_bf_feedforward = attitude_control->get_bf_feedforward();
|
||||||
waveform_time = 0.0f;
|
waveform_time = 0.0f;
|
||||||
time_const_freq = 2.0f / frequency_start; // Two full cycles at the starting frequency
|
time_const_freq = 2.0f / frequency_start; // Two full cycles at the starting frequency
|
||||||
|
@ -117,65 +153,95 @@ void ModeSystemId::exit()
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
void ModeSystemId::run()
|
void ModeSystemId::run()
|
||||||
{
|
{
|
||||||
// apply simple mode transform to pilot inputs
|
|
||||||
update_simple_mode();
|
|
||||||
|
|
||||||
// convert pilot input to lean angles
|
|
||||||
float target_roll, target_pitch;
|
float target_roll, target_pitch;
|
||||||
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
|
float target_yaw_rate = 0.0f;
|
||||||
|
float pilot_throttle_scaled = 0.0f;
|
||||||
|
float target_climb_rate = 0.0f;
|
||||||
|
Vector2f input_vel;
|
||||||
|
Vector2f pilot_vel;
|
||||||
|
|
||||||
// get pilot's desired yaw rate
|
if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG
|
||||||
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
|
&& (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG
|
||||||
|
&& (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) {
|
||||||
|
|
||||||
if (!motors->armed()) {
|
// apply simple mode transform to pilot inputs
|
||||||
// Motors should be Stopped
|
update_simple_mode();
|
||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
|
||||||
// Tradheli doesn't set spool state to ground idle when throttle stick is zero. Ground idle only set when
|
|
||||||
// motor interlock is disabled.
|
|
||||||
} else if (copter.ap.throttle_zero && !copter.is_tradheli()) {
|
|
||||||
// Attempting to Land
|
|
||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
|
||||||
} else {
|
|
||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (motors->get_spool_state()) {
|
// convert pilot input to lean angles
|
||||||
case AP_Motors::SpoolState::SHUT_DOWN:
|
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
|
||||||
// Motors Stopped
|
|
||||||
attitude_control->reset_yaw_target_and_rate();
|
|
||||||
attitude_control->reset_rate_controller_I_terms();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case AP_Motors::SpoolState::GROUND_IDLE:
|
// get pilot's desired yaw rate
|
||||||
// Landed
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
|
||||||
// Tradheli initializes targets when going from disarmed to armed state.
|
|
||||||
// init_targets_on_arming is always set true for multicopter.
|
if (!motors->armed()) {
|
||||||
if (motors->init_targets_on_arming()) {
|
// Motors should be Stopped
|
||||||
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
||||||
|
// Tradheli doesn't set spool state to ground idle when throttle stick is zero. Ground idle only set when
|
||||||
|
// motor interlock is disabled.
|
||||||
|
} else if (copter.ap.throttle_zero && !copter.is_tradheli()) {
|
||||||
|
// Attempting to Land
|
||||||
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
||||||
|
} else {
|
||||||
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (motors->get_spool_state()) {
|
||||||
|
case AP_Motors::SpoolState::SHUT_DOWN:
|
||||||
|
// Motors Stopped
|
||||||
attitude_control->reset_yaw_target_and_rate();
|
attitude_control->reset_yaw_target_and_rate();
|
||||||
attitude_control->reset_rate_controller_I_terms_smoothly();
|
attitude_control->reset_rate_controller_I_terms();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AP_Motors::SpoolState::GROUND_IDLE:
|
||||||
|
// Landed
|
||||||
|
// Tradheli initializes targets when going from disarmed to armed state.
|
||||||
|
// init_targets_on_arming is always set true for multicopter.
|
||||||
|
if (motors->init_targets_on_arming()) {
|
||||||
|
attitude_control->reset_yaw_target_and_rate();
|
||||||
|
attitude_control->reset_rate_controller_I_terms_smoothly();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
|
||||||
|
// clear landing flag above zero throttle
|
||||||
|
if (!motors->limit.throttle_lower) {
|
||||||
|
set_land_complete(false);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AP_Motors::SpoolState::SPOOLING_UP:
|
||||||
|
case AP_Motors::SpoolState::SPOOLING_DOWN:
|
||||||
|
// do nothing
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
|
|
||||||
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
|
// get pilot's desired throttle
|
||||||
// clear landing flag above zero throttle
|
|
||||||
if (!motors->limit.throttle_lower) {
|
|
||||||
set_land_complete(false);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case AP_Motors::SpoolState::SPOOLING_UP:
|
|
||||||
case AP_Motors::SpoolState::SPOOLING_DOWN:
|
|
||||||
// do nothing
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// get pilot's desired throttle
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
float pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
||||||
#else
|
#else
|
||||||
float pilot_throttle_scaled = get_pilot_desired_throttle();
|
pilot_throttle_scaled = get_pilot_desired_throttle();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// set xy speed and acceleration limits
|
||||||
|
pos_control->set_max_speed_accel_xy(500.0f, 250.0f);
|
||||||
|
pos_control->set_correction_speed_accel_xy(500.0f, 250.0f);
|
||||||
|
|
||||||
|
// set vertical speed and acceleration limits
|
||||||
|
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||||
|
|
||||||
|
// process pilot inputs unless we are in radio failsafe
|
||||||
|
// this was split into two parts to allow pilot input to accomplish closed loop sweeps
|
||||||
|
if (!copter.failsafe.radio) {
|
||||||
|
// apply SIMPLE mode transform to pilot inputs
|
||||||
|
update_simple_mode();
|
||||||
|
|
||||||
|
pilot_vel = get_pilot_desired_velocity(250.0f);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
if ((systemid_state == SystemIDModeState::SYSTEMID_STATE_TESTING) &&
|
if ((systemid_state == SystemIDModeState::SYSTEMID_STATE_TESTING) &&
|
||||||
(!is_positive(frequency_start) || !is_positive(frequency_stop) || is_negative(time_fade_in) || !is_positive(time_record) || is_negative(time_fade_out) || (time_record <= time_const_freq))) {
|
(!is_positive(frequency_start) || !is_positive(frequency_stop) || is_negative(time_fade_in) || !is_positive(time_record) || is_negative(time_fade_out) || (time_record <= time_const_freq))) {
|
||||||
systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
|
systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
|
||||||
|
@ -185,7 +251,7 @@ void ModeSystemId::run()
|
||||||
waveform_time += G_Dt;
|
waveform_time += G_Dt;
|
||||||
waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude);
|
waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude);
|
||||||
waveform_freq_rads = chirp_input.get_frequency_rads();
|
waveform_freq_rads = chirp_input.get_frequency_rads();
|
||||||
|
Vector2f disturb_state;
|
||||||
switch (systemid_state) {
|
switch (systemid_state) {
|
||||||
case SystemIDModeState::SYSTEMID_STATE_STOPPED:
|
case SystemIDModeState::SYSTEMID_STATE_STOPPED:
|
||||||
attitude_control->bf_feedforward(att_bf_feedforward);
|
attitude_control->bf_feedforward(att_bf_feedforward);
|
||||||
|
@ -255,15 +321,152 @@ void ModeSystemId::run()
|
||||||
case AxisType::MIX_THROTTLE:
|
case AxisType::MIX_THROTTLE:
|
||||||
pilot_throttle_scaled += waveform_sample;
|
pilot_throttle_scaled += waveform_sample;
|
||||||
break;
|
break;
|
||||||
|
case AxisType::DISTURB_POS_LAT:
|
||||||
|
disturb_state.x = 0.0f;
|
||||||
|
disturb_state.y = waveform_sample * 100.0f;
|
||||||
|
copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y);
|
||||||
|
pos_control->set_disturb_pos_cm(disturb_state);
|
||||||
|
break;
|
||||||
|
case AxisType::DISTURB_POS_LONG:
|
||||||
|
disturb_state.x = waveform_sample * 100.0f;
|
||||||
|
disturb_state.y = 0.0f;
|
||||||
|
copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y);
|
||||||
|
pos_control->set_disturb_pos_cm(disturb_state);
|
||||||
|
break;
|
||||||
|
case AxisType::DISTURB_VEL_LAT:
|
||||||
|
disturb_state.x = 0.0f;
|
||||||
|
disturb_state.y = waveform_sample * 100.0f;
|
||||||
|
copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y);
|
||||||
|
pos_control->set_disturb_vel_cms(disturb_state);
|
||||||
|
break;
|
||||||
|
case AxisType::DISTURB_VEL_LONG:
|
||||||
|
disturb_state.x = waveform_sample * 100.0f;
|
||||||
|
disturb_state.y = 0.0f;
|
||||||
|
copter.rotate_target_body_frame_to_NE(disturb_state.x, disturb_state.y);
|
||||||
|
pos_control->set_disturb_vel_cms(disturb_state);
|
||||||
|
break;
|
||||||
|
case AxisType::INPUT_LOITER_LAT:
|
||||||
|
input_vel.x = 0.0f;
|
||||||
|
input_vel.y = waveform_sample * 100.0f;
|
||||||
|
copter.rotate_target_body_frame_to_NE(input_vel.x, input_vel.y);
|
||||||
|
break;
|
||||||
|
case AxisType::INPUT_LOITER_LONG:
|
||||||
|
input_vel.x = waveform_sample * 100.0f;
|
||||||
|
input_vel.y = 0.0f;
|
||||||
|
copter.rotate_target_body_frame_to_NE(input_vel.x, input_vel.y);
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// call attitude controller
|
if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG
|
||||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
|
&& (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG
|
||||||
|
&& (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) {
|
||||||
|
|
||||||
// output pilot's throttle
|
// call attitude controller
|
||||||
attitude_control->set_throttle_out(pilot_throttle_scaled, !copter.is_tradheli(), g.throttle_filt);
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||||
|
|
||||||
|
// output pilot's throttle
|
||||||
|
attitude_control->set_throttle_out(pilot_throttle_scaled, !copter.is_tradheli(), g.throttle_filt);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
// process pilot inputs unless we are in radio failsafe
|
||||||
|
// this was split into two parts to allow pilot input to accomplish closed loop sweeps
|
||||||
|
if (!copter.failsafe.radio) {
|
||||||
|
|
||||||
|
// Set pilot's roll and pitch input to zero
|
||||||
|
loiter_nav->set_pilot_desired_acceleration(0.0f, 0.0f);
|
||||||
|
|
||||||
|
// get pilot's desired yaw rate
|
||||||
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
|
||||||
|
|
||||||
|
// get pilot desired climb rate
|
||||||
|
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
|
||||||
|
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
|
||||||
|
} else {
|
||||||
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
||||||
|
loiter_nav->clear_pilot_desired_acceleration();
|
||||||
|
}
|
||||||
|
|
||||||
|
// relax loiter target if we might be landed
|
||||||
|
if (copter.ap.land_complete_maybe) {
|
||||||
|
pos_control->soften_for_landing_xy();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Loiter State Machine Determination
|
||||||
|
AltHoldModeState althold_state = get_alt_hold_state(target_climb_rate);
|
||||||
|
|
||||||
|
// Loiter State Machine
|
||||||
|
switch (althold_state) {
|
||||||
|
|
||||||
|
case AltHold_MotorStopped:
|
||||||
|
attitude_control->reset_rate_controller_I_terms();
|
||||||
|
attitude_control->reset_yaw_target_and_rate();
|
||||||
|
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
|
||||||
|
pos_control->init_xy_controller();
|
||||||
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AltHold_Landed_Ground_Idle:
|
||||||
|
attitude_control->reset_yaw_target_and_rate();
|
||||||
|
FALLTHROUGH;
|
||||||
|
|
||||||
|
case AltHold_Landed_Pre_Takeoff:
|
||||||
|
attitude_control->reset_rate_controller_I_terms_smoothly();
|
||||||
|
pos_control->init_xy_controller();
|
||||||
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
|
||||||
|
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AltHold_Takeoff:
|
||||||
|
// initiate take-off
|
||||||
|
if (!takeoff.running()) {
|
||||||
|
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
|
||||||
|
}
|
||||||
|
|
||||||
|
// get avoidance adjusted climb rate
|
||||||
|
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
|
||||||
|
|
||||||
|
// set position controller targets adjusted for pilot input
|
||||||
|
takeoff.do_pilot_takeoff(target_climb_rate);
|
||||||
|
|
||||||
|
// run loiter controller
|
||||||
|
pos_control->update_xy_controller();
|
||||||
|
|
||||||
|
// call attitude controller
|
||||||
|
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate, false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AltHold_Flying:
|
||||||
|
// set motors to full range
|
||||||
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||||
|
|
||||||
|
input_vel += pilot_vel;
|
||||||
|
Vector2f accel = Vector2f((input_vel.x - target_vel.x) / G_Dt, (input_vel.y - target_vel.y) / G_Dt);
|
||||||
|
|
||||||
|
target_vel = input_vel;
|
||||||
|
pos_control->input_vel_accel_xy(target_vel, accel);
|
||||||
|
pos_control->update_xy_controller();
|
||||||
|
|
||||||
|
// call attitude controller
|
||||||
|
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate, false);
|
||||||
|
|
||||||
|
// get avoidance adjusted climb rate
|
||||||
|
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
|
||||||
|
|
||||||
|
// update the vertical offset based on the surface measurement
|
||||||
|
copter.surface_tracking.update_surface_offset();
|
||||||
|
|
||||||
|
// Send the commanded climb rate to the position controller
|
||||||
|
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// run the vertical position controller and set output throttle
|
||||||
|
pos_control->update_z_controller();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
if (log_subsample <= 0) {
|
if (log_subsample <= 0) {
|
||||||
log_data();
|
log_data();
|
||||||
|
@ -298,6 +501,15 @@ void ModeSystemId::log_data() const
|
||||||
// Full rate logging of attitude, rate and pid loops
|
// Full rate logging of attitude, rate and pid loops
|
||||||
copter.Log_Write_Attitude();
|
copter.Log_Write_Attitude();
|
||||||
copter.Log_Write_PIDS();
|
copter.Log_Write_PIDS();
|
||||||
|
|
||||||
|
if ((AxisType)axis.get() == AxisType::DISTURB_POS_LAT || (AxisType)axis.get() == AxisType::DISTURB_POS_LONG
|
||||||
|
|| (AxisType)axis.get() == AxisType::DISTURB_VEL_LAT || (AxisType)axis.get() == AxisType::DISTURB_VEL_LONG
|
||||||
|
|| (AxisType)axis.get() == AxisType::INPUT_LOITER_LAT || (AxisType)axis.get() == AxisType::INPUT_LOITER_LONG) {
|
||||||
|
pos_control->write_log();
|
||||||
|
copter.logger.Write_PID(LOG_PIDN_MSG, pos_control->get_vel_xy_pid().get_pid_info_x());
|
||||||
|
copter.logger.Write_PID(LOG_PIDE_MSG, pos_control->get_vel_xy_pid().get_pid_info_y());
|
||||||
|
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue