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

Sub: introduce CONTROL_FRAME parameter, allowing users to switch control frame for yaw

This commit is contained in:
Willian Galvani 2023-02-14 13:54:44 -03:00
parent 3b760d41c9
commit d26d0dacf8
5 changed files with 101 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduSub/Parameters.cpp
View File

@ -407,6 +407,14 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Advanced
GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT),
// @Param: CONTROL_FRAME
// @DisplayName: Control Frame
// @Description: Control Frame used in stabilized modes
// @Values: 0: Standard, 1: Body frame Yaw/Pitch/Roll
// @User: Standard
GSCALAR(control_frame, "CONTROL_FRAME", CONTROL_FRAME_DEFAULT),
// variables not in the g class which contain EEPROM saved variables
#if CAMERA == ENABLED

2
ArduSub/Parameters.h
View File

@ -191,6 +191,7 @@ public:
k_param_throttle_gain,
k_param_cam_tilt_center, // deprecated
k_param_frame_configuration,
k_param_control_frame,
// Acro Mode parameters
k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate
@ -302,6 +303,7 @@ public:
AP_Float surface_depth;
AP_Int8 frame_configuration;
AP_Int8 control_frame;
// Note: keep initializers here in the same order as they are declared
// above.

5
ArduSub/config.h
View File

@ -153,6 +153,11 @@
#define ACRO_EXPO_DEFAULT 0.3f
#endif
// should be a subset of MAV_FRAME
#ifndef CONTROL_FRAME_DEFAULT
#define CONTROL_FRAME_DEFAULT 0.0f
#endif
// AUTO Mode
#ifndef WP_YAW_BEHAVIOR_DEFAULT
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_CORRECT_XTRACK

71
ArduSub/control_althold.cpp
View File

@ -50,7 +50,7 @@ void Sub::handle_attitude()
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// get pilot desired lean angles
// get pilot desired lean angles/rates
float target_roll, target_pitch, target_yaw;
// Check if set_attitude_target_no_gps is valid
@ -69,23 +69,57 @@ void Sub::handle_attitude()
last_pitch = target_pitch;
last_pilot_heading = target_yaw;
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
} else {
// If we don't have a mavlink attitude target, we use the pilot's input instead
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim());
target_yaw = get_pilot_desired_yaw_rate(yaw_input);
if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
last_roll = ahrs.roll_sensor;
last_pitch = ahrs.pitch_sensor;
last_pilot_heading = ahrs.yaw_sensor;
last_input_ms = tnow;
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
} else if (tnow < last_input_ms + 250) {
// just brake for a few mooments so we don't bounce
attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
} else {
// Lock attitude
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
return;
}
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim());
target_yaw = get_pilot_desired_yaw_rate(yaw_input);
// If we don't have a mavlink attitude target, we use the pilot's input instead
switch (g.control_frame) {
case MAV_FRAME_BODY_FRD:
{
if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
last_input_ms = tnow;
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
Quaternion attitude_target = attitude_control.get_attitude_target_quat();
last_roll = degrees(attitude_target.get_euler_roll()) * 100;
last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
} else {
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
}
}
break;
default:
{
// call attitude controller
if (!is_zero(target_yaw)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, target_yaw);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading
if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
last_roll = ahrs.roll_sensor;
last_pitch = ahrs.pitch_sensor;
last_pilot_heading = ahrs.yaw_sensor;
last_input_ms = tnow;
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
} else if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
target_yaw = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, target_yaw);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
}
}
}
}
}
@ -99,6 +133,7 @@ void Sub::althold_run()
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out(0.75,true,100.0);
attitude_control.relax_attitude_controllers();
pos_control.init_z_controller();
// initialise position and desired velocity
float pos = stopping_distance();

55
ArduSub/joystick.cpp
View File

@ -1,5 +1,7 @@
#include "Sub.h"
#define TRIM_STEP 0.5 // trim step in degrees
// Functions that will handle joystick/gamepad input
// ----------------------------------------------------------------------------
@ -141,12 +143,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
{
// Used for trimming level in vehicle frame
Quaternion attitudeTarget;
attitudeTarget.from_euler(
radians(last_roll * 0.01f),
radians(last_pitch * 0.01f),
radians(last_pilot_heading * 0.01f)
);
Quaternion attitude_target = attitude_control.get_attitude_target_quat();
Vector3f localPitch = Vector3f(0, 1, 0);
Vector3f localRoll = Vector3f(1, 0, 0);
@ -340,28 +338,41 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
}
break;
case JSButton::button_function_t::k_trim_roll_inc:
attitudeTarget.rotate(localRoll * radians(1));
last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
last_roll += TRIM_STEP * 100;
if (g.control_frame == MAV_FRAME_BODY_FRD) {
// TODO: change these to use input_quaternion()
attitude_target.rotate(localRoll * radians(TRIM_STEP));
last_roll = degrees(attitude_target.get_euler_roll()) * 100;
last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
}
break;
case JSButton::button_function_t::k_trim_roll_dec:
attitudeTarget.rotate(localRoll * radians(-1));
last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
last_roll -= TRIM_STEP * 100;
if (g.control_frame == MAV_FRAME_BODY_FRD) {
attitude_target.rotate(localRoll * radians(-TRIM_STEP));
last_roll = degrees(attitude_target.get_euler_roll()) * 100;
last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
}
break;
case JSButton::button_function_t::k_trim_pitch_inc:
attitudeTarget.rotate(localPitch * radians(1));
last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
last_pitch += TRIM_STEP * 100;
if (g.control_frame == MAV_FRAME_BODY_FRD) {
attitude_target.rotate(localPitch * radians(TRIM_STEP));
last_roll = degrees(attitude_target.get_euler_roll()) * 100;
last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
}
break;
case JSButton::button_function_t::k_trim_pitch_dec:
attitudeTarget.rotate(localPitch * radians(-1));
last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
last_pitch -= TRIM_STEP * 100;
if (g.control_frame == MAV_FRAME_BODY_FRD) {
attitude_target.rotate(localPitch * radians(-TRIM_STEP));
last_roll = degrees(attitude_target.get_euler_roll()) * 100;
last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
}
break;
case JSButton::button_function_t::k_input_hold_set:
if(!motors.armed()) {