Skip to content

Commit

Permalink
Copter: add support for sysid in loiter
Browse files Browse the repository at this point in the history
  • Loading branch information
bnsgeyer committed Aug 29, 2023
1 parent 8c50fd5 commit 11f285f
Show file tree
Hide file tree
Showing 4 changed files with 253 additions and 58 deletions.
9 changes: 9 additions & 0 deletions ArduCopter/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,15 @@ void Copter::rotate_body_frame_to_NE(float &x, float &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.
uint16_t Copter::get_pilot_speed_dn() const
{
Expand Down
1 change: 1 addition & 0 deletions ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -721,6 +721,7 @@ class Copter : public AP_Vehicle {
float get_non_takeoff_throttle();
void set_accel_throttle_I_from_pilot_throttle();
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;
void run_rate_controller();

Expand Down
9 changes: 8 additions & 1 deletion ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -1582,6 +1582,12 @@ class ModeSystemId : public Mode {
MIX_PITCH = 11, // mixer pitch axis is being excited
MIX_YAW = 12, // mixer pitch 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
Expand All @@ -1598,7 +1604,8 @@ class ModeSystemId : public Mode {
float waveform_freq_rads; // Instantaneous waveform frequency
float time_const_freq; // Time at constant frequency before chirp starts
int8_t log_subsample; // Subsample multiple for logging.

Vector2f target_vel; // target velocity for position controller modes

// System ID states
enum class SystemIDModeState {
SYSTEMID_STATE_STOPPED,
Expand Down
292 changes: 235 additions & 57 deletions ArduCopter/mode_systemid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,15 @@ 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 (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;
}

if (copter.flightmode->mode_number() != Mode::Number::LOITER) {
#if FRAME_CONFIG == HELI_FRAME
copter.input_manager.set_use_stab_col(true);
#endif
}

att_bf_feedforward = attitude_control->get_bf_feedforward();
waveform_time = 0.0f;
Expand Down Expand Up @@ -115,65 +117,97 @@ void ModeSystemId::exit()
// should be called at 100hz or more
void ModeSystemId::run()
{
// apply simple mode transform to pilot inputs
update_simple_mode();

// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);

// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());

if (!motors->armed()) {
// 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);
}
float target_yaw_rate = 0.0f;
float pilot_throttle_scaled = 0.0f;
float target_climb_rate = 0.0f;
Vector2f input_vel;
Vector2f pilot_vel;

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) {

// 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, copter.aparm.angle_max, copter.aparm.angle_max);

// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());

if (!motors->armed()) {
// 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_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()) {
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
attitude_control->reset_rate_controller_I_terms();
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::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::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
break;

// get pilot's desired throttle
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
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
float pilot_throttle_scaled = get_pilot_desired_throttle();
pilot_throttle_scaled = get_pilot_desired_throttle();
#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();

// convert pilot input to lean angles
// get_pilot_desired_lean_angles(target_roll, target_pitch, pos_control->get_lean_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
pilot_vel = get_pilot_desired_velocity(250.0f);

}

}

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))) {
systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
Expand All @@ -183,7 +217,7 @@ void ModeSystemId::run()
waveform_time += G_Dt;
waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude);
waveform_freq_rads = chirp_input.get_frequency_rads();

Vector2f disturb_state;
switch (systemid_state) {
case SystemIDModeState::SYSTEMID_STATE_STOPPED:
attitude_control->bf_feedforward(att_bf_feedforward);
Expand Down Expand Up @@ -253,18 +287,156 @@ void ModeSystemId::run()
case AxisType::MIX_THROTTLE:
pilot_throttle_scaled += waveform_sample;
break;
case AxisType::DISTURB_POS_LAT:
disturb_state.x = 0.0f;
disturb_state.y = waveform_sample;
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;
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;
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;
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;
}

// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
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) {

// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

// output pilot's throttle
if (copter.is_tradheli()) {
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
} else {
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}

// output pilot's throttle
if (copter.is_tradheli()) {
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
} else {
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);

// 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 target accels in position controller
// Vector3f target_attitude_rad = Vector3f(radians(target_roll*0.01), radians(target_pitch*0.01), attitude_control->get_att_target_euler_rad().z);
// target_accel = pos_control->lean_angles_to_accel(target_attitude_rad);

// 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);
// Vector2f accel;
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) {
Expand Down Expand Up @@ -300,6 +472,12 @@ void ModeSystemId::log_data() const
// Full rate logging of attitude, rate and pid loops
copter.Log_Write_Attitude();
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();
}
}

#endif

0 comments on commit 11f285f

Please sign in to comment.