Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Plane: move guided roll, pitch and throttle overrides upto guided mode #22926

Merged
merged 2 commits into from Apr 17, 2023
Merged

Plane: move guided roll, pitch and throttle overrides upto guided mode #22926

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e8247e6
Plane: move guided roll, pitch and throttle overrides upto guided mode
IamPete1 Feb 14, 2023
1cffe5b
Tools: autotest: Plane: expand guided attitude target test
IamPete1 Apr 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
55 changes: 0 additions & 55 deletions ArduPlane/Attitude.cpp
View file
Open in desktop
Expand Up @@ -453,14 +453,6 @@ void Plane::calc_throttle()
}

float commanded_throttle = TECS_controller.get_throttle_demand();

// Received an external msg that guides throttle in the last 3 seconds?
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_throttle_ms > 0 &&
millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
commanded_throttle = plane.guided_state.forced_throttle;
}

SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
}

Expand Down Expand Up @@ -587,17 +579,7 @@ void Plane::calc_nav_yaw_ground(void)
*/
void Plane::calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
int32_t commanded_pitch = TECS_controller.get_pitch_demand();

// Received an external msg that guides roll in the last 3 seconds?
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_rpy_ms.y > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
commanded_pitch = plane.guided_state.forced_rpy_cd.y;
}

nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}

Expand All @@ -608,43 +590,6 @@ void Plane::calc_nav_pitch()
void Plane::calc_nav_roll()
{
int32_t commanded_roll = nav_controller->nav_roll_cd();

// Received an external msg that guides roll in the last 3 seconds?
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_rpy_ms.x > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
commanded_roll = plane.guided_state.forced_rpy_cd.x;
#if OFFBOARD_GUIDED == ENABLED
// guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 )
} else if ((control_mode == &mode_guided) && (guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
uint32_t tnow = AP_HAL::millis();
float delta = (tnow - guided_state.target_heading_time_ms) * 1e-3f;
guided_state.target_heading_time_ms = tnow;

float error = 0.0f;
if (guided_state.target_heading_type == GUIDED_HEADING_HEADING) {
error = wrap_PI(guided_state.target_heading - AP::ahrs().yaw);
} else {
Vector2f groundspeed = AP::ahrs().groundspeed_vector();
error = wrap_PI(guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI);
}

float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;

g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.?

float i = g2.guidedHeading.get_i(); // get integrator TODO
if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) {
i = g2.guidedHeading.get_i();
}

float desired = g2.guidedHeading.get_p() + i + g2.guidedHeading.get_d();
guided_state.target_heading_limit_low = (desired <= -bank_limit);
guided_state.target_heading_limit_high = (desired >= bank_limit);
commanded_roll = constrain_float(desired, -bank_limit, bank_limit);
#endif // OFFBOARD_GUIDED == ENABLED
}

nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
update_load_factor();
}
Expand Down
64 changes: 61 additions & 3 deletions ArduPlane/mode_guided.cpp
View file
Open in desktop
Expand Up @@ -35,9 +35,67 @@ void ModeGuided::update()
return;
}
#endif
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();

// Received an external msg that guides roll in the last 3 seconds?
if (plane.guided_state.last_forced_rpy_ms.x > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
plane.update_load_factor();

#if OFFBOARD_GUIDED == ENABLED
// guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 )
// This function is used in Guided and AvoidADSB, check for guided
} else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
uint32_t tnow = AP_HAL::millis();
float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f;
plane.guided_state.target_heading_time_ms = tnow;

float error = 0.0f;
if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) {
error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().yaw);
} else {
Vector2f groundspeed = AP::ahrs().groundspeed_vector();
error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI);
}

float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
bank_limit = MIN(bank_limit, plane.roll_limit_cd);

plane.g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.?

float i = plane.g2.guidedHeading.get_i(); // get integrator TODO
if (((is_negative(error) && !plane.guided_state.target_heading_limit_low) || (is_positive(error) && !plane.guided_state.target_heading_limit_high))) {
i = plane.g2.guidedHeading.get_i();
}

float desired = plane.g2.guidedHeading.get_p() + i + plane.g2.guidedHeading.get_d();
plane.guided_state.target_heading_limit_low = (desired <= -bank_limit);
plane.guided_state.target_heading_limit_high = (desired >= bank_limit);

plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
plane.update_load_factor();

#endif // OFFBOARD_GUIDED == ENABLED
} else {
plane.calc_nav_roll();
}

if (plane.guided_state.last_forced_rpy_ms.y > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get());
} else {
plane.calc_nav_pitch();
}

// Received an external msg that guides throttle in the last 3 seconds?
if (plane.aparm.throttle_cruise > 1 &&
plane.guided_state.last_forced_throttle_ms > 0 &&
millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);
} else {
plane.calc_throttle();
}

}

void ModeGuided::navigate()
Expand Down
143 changes: 74 additions & 69 deletions Tools/autotest/arduplane.py
View file
Open in desktop
Expand Up @@ -318,78 +318,83 @@ def inside_loop(self, count=1):
def set_attitude_target(self, tolerance=10):
"""Test setting of attitude target in guided mode."""
self.change_mode("GUIDED")
# self.set_parameter("STALL_PREVENTION", 0)

state_roll_over = "roll-over"
state_stabilize_roll = "stabilize-roll"
state_hold = "hold"
state_roll_back = "roll-back"
state_done = "done"

tstart = self.get_sim_time()
steps = [{"name": "roll-over", "roll": 60, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000001},
{"name": "roll-back", "roll": 0, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000001},
{"name": "pitch-up+throttle", "roll": 0, "pitch": 20, "yaw": 0, "throttle": 1, "type_mask": 0b11000010},
{"name": "pitch-back", "roll": 0, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000010}]

state_wait = "wait"
state_hold = "hold"
try:
state = state_roll_over
while state != state_done:

m = self.mav.recv_match(type='ATTITUDE',
blocking=True,
timeout=0.1)
now = self.get_sim_time_cached()
if now - tstart > 20:
raise AutoTestTimeoutException("Manuevers not completed")
if m is None:
continue

r = math.degrees(m.roll)
if state == state_roll_over:
target_roll_degrees = 60
if abs(r - target_roll_degrees) < tolerance:
state = state_stabilize_roll
stabilize_start = now
elif state == state_stabilize_roll:
# just give it a little time to sort it self out
if now - stabilize_start > 2:
state = state_hold
hold_start = now
elif state == state_hold:
target_roll_degrees = 60
if now - hold_start > tolerance:
state = state_roll_back
if abs(r - target_roll_degrees) > tolerance:
raise NotAchievedException("Failed to hold attitude")
elif state == state_roll_back:
target_roll_degrees = 0
if abs(r - target_roll_degrees) < tolerance:
state = state_done
else:
raise ValueError("Unknown state %s" % str(state))

m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT']
self.progress("%s Roll: %f desired=%f set=%f" %
(state, r, m_nav.nav_roll, target_roll_degrees))

time_boot_millis = 0 # FIXME
target_system = 1 # FIXME
target_component = 1 # FIXME
type_mask = 0b10000001 ^ 0xFF # FIXME
# attitude in radians:
q = quaternion.Quaternion([math.radians(target_roll_degrees),
0,
0])
roll_rate_radians = 0.5
pitch_rate_radians = 0
yaw_rate_radians = 0
thrust = 1.0
self.mav.mav.set_attitude_target_send(time_boot_millis,
target_system,
target_component,
type_mask,
q,
roll_rate_radians,
pitch_rate_radians,
yaw_rate_radians,
thrust)
for step in steps:
step_start = self.get_sim_time_cached()
state = state_wait
state_start = self.get_sim_time_cached()
while True:
m = self.mav.recv_match(type='ATTITUDE',
blocking=True,
timeout=0.1)
now = self.get_sim_time_cached()
if now - step_start > 30:
raise AutoTestTimeoutException("Manuevers not completed")
if m is None:
continue

angle_error = 0
if (step["type_mask"] & 0b00000001) or (step["type_mask"] == 0b10000000):
angle_error += abs(math.degrees(m.roll) - step["roll"])

if (step["type_mask"] & 0b00000010) or (step["type_mask"] == 0b10000000):
angle_error += abs(math.degrees(m.pitch) - step["pitch"])

if (step["type_mask"] & 0b00000100) or (step["type_mask"] == 0b10000000):
# Strictly we should angle wrap, by plane doesn't support yaw correctly anyway so its not tested here
angle_error += abs(math.degrees(m.yaw) - step["yaw"])

# Note were not checking throttle, however the SITL plane needs full throttle to meet the
# target pitch attitude, Pitch test will fail without throttle override

if state == state_wait:
# Reduced tolerance for initial trigger
if angle_error < (tolerance * 0.25):
state = state_hold
state_start = now

# Allow 10 seconds to reach attitude
if (now - state_start) > 10:
raise NotAchievedException(step["name"] + ": Failed to get to set attitude")

elif state == state_hold:
# Give 2 seconds to stabilize
if (now - state_start) > 2 and not (angle_error < tolerance):
raise NotAchievedException(step["name"] + ": Failed to hold set attitude")

# Hold for 10 seconds
if (now - state_start) > 12:
# move onto next step
self.progress("%s Done" % (step["name"]))
break

self.progress("%s %s error: %f" % (step["name"], state, angle_error))

time_boot_millis = 0 # FIXME
target_system = 1 # FIXME
target_component = 1 # FIXME
type_mask = step["type_mask"] ^ 0xFF # FIXME
# attitude in radians:
q = quaternion.Quaternion([math.radians(step["roll"]),
math.radians(step["pitch"]),
math.radians(step["yaw"])])
self.mav.mav.set_attitude_target_send(time_boot_millis,
target_system,
target_component,
type_mask,
q,
0, # roll rate, not used in AP
0, # pitch rate, not used in AP
0, # yaw rate, not used in AP
step["throttle"])
except Exception as e:
self.change_mode('FBWA')
self.set_rc(3, 1700)
Expand Down