Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: fixes for suggested changes

This commit is contained in:
bnsgeyer 2024-02-13 22:26:31 -05:00 committed by Bill Geyer
parent d0dfe86a4b
commit 75d9163571
2 changed files with 25 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -1631,6 +1631,7 @@ protected:
private:
void log_data() const;
bool is_poscontrol_axis_type() const;
enum class AxisType {
NONE = 0, // none

36
ArduCopter/mode_systemid.cpp
View File

@ -80,17 +80,17 @@ bool ModeSystemId::init(bool ignore_checks)
return false;
}
// 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() || copter.flightmode->mode_number() == Mode::Number::LOITER)) {
// ensure we are flying
if (!copter.motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
gcs().send_text(MAV_SEVERITY_WARNING, "Aircraft must be flying");
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 (!is_poscontrol_axis_type()) {
// System ID is being done on the attitude control loops
// Can only switch into System ID Axes 1-13 with a flight mode that has manual throttle
if (!copter.flightmode->has_manual_throttle()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Axis requires manual throttle");
return false;
@ -104,6 +104,7 @@ bool ModeSystemId::init(bool ignore_checks)
// System ID is being done on the position control loops
// Can only switch into System ID Axes 14-19 from Loiter flight mode
if (copter.flightmode->mode_number() != Mode::Number::LOITER) {
gcs().send_text(MAV_SEVERITY_WARNING, "Axis requires switch from Loiter");
return false;
@ -162,9 +163,7 @@ void ModeSystemId::run()
float target_climb_rate = 0.0f;
Vector2f input_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) {
if (!is_poscontrol_axis_type()) {
// apply simple mode transform to pilot inputs
update_simple_mode();
@ -343,9 +342,7 @@ void ModeSystemId::run()
break;
}
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 (!is_poscontrol_axis_type()) {
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
@ -410,9 +407,7 @@ void ModeSystemId::log_data() const
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) {
if (is_poscontrol_axis_type()) {
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());
@ -420,4 +415,17 @@ void ModeSystemId::log_data() const
}
}
bool ModeSystemId::is_poscontrol_axis_type() const
{
bool ret = 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) {
ret = true;
}
return ret;
}
#endif