Skip to content

Commit

Permalink
ShipOps: Peters Suggestions 2
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Jun 6, 2024
1 parent dba5aa5 commit ca645b2
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 31 deletions.
18 changes: 9 additions & 9 deletions ArduCopter/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1538,15 +1538,15 @@ class ModeShipOperation : public Mode {
} ship;

// Ship Operations parameters
AP_Float ship_perch_angle;
AP_Float ship_perch_radius;
AP_Float ship_perch_altitude;
AP_Float ship_accel_xy;
AP_Float ship_jerk_xy;
AP_Float ship_accel_z;
AP_Float ship_jerk_z;
AP_Float ship_accel_h;
AP_Float ship_jerk_h;
AP_Float perch_angle;
AP_Float perch_radius;
AP_Float perch_altitude;
AP_Float ship_max_accel_xy;
AP_Float ship_max_jerk_xy;
AP_Float ship_max_accel_z;
AP_Float ship_max_jerk_z;
AP_Float ship_max_accel_h;
AP_Float ship_max_jerk_h;
AP_Float keep_out_CW;
AP_Float keep_out_CCW;
AP_Float keep_out_radius;
Expand Down
44 changes: 22 additions & 22 deletions ArduCopter/mode_ship_ops.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,71 +11,71 @@ const AP_Param::GroupInfo ModeShipOperation::var_info[] = {
// @Range: 0 1
// @Units: deg
// @User: Advanced
AP_GROUPINFO("PCH_ANG", 1, ModeShipOperation, ship_perch_angle, 180.0f),
AP_GROUPINFO("PCH_ANG", 1, ModeShipOperation, perch_angle, 180.0f),

// @Param: PCH_RAD
// @DisplayName: Distance of the perch position from the ship
// @Description: Distance in m of the perch position from the ship where the aircraft will wait until it is commanded to land.
// @Range: 0 1
// @Units: m
// @User: Advanced
AP_GROUPINFO("PCH_RAD", 2, ModeShipOperation, ship_perch_radius, 25.0f),
AP_GROUPINFO("PCH_RAD", 2, ModeShipOperation, perch_radius, 25.0f),

// @Param: PCH_ALT
// @DisplayName: Altitude of the perch position from the ship
// @Description: Altitude in m of the perch position relative to the ship where the aircraft will wait until it is commanded to land.
// @Range: 0 1
// @Units: m
// @User: Advanced
AP_GROUPINFO("PCH_ALT", 3, ModeShipOperation, ship_perch_altitude, 25.0f),
AP_GROUPINFO("PCH_ALT", 3, ModeShipOperation, perch_altitude, 25.0f),

// @Param: ACCELXY
// @DisplayName: Acceleration limit for the horizontal kinematic input shaping
// @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
// @Range: 0 1
// @Units: m/s/s
// @User: Advanced
AP_GROUPINFO("ACCELXY", 4, ModeShipOperation, ship_accel_xy, 2.5f),
AP_GROUPINFO("ACCELXY", 4, ModeShipOperation, ship_max_accel_xy, 2.5f),

// @Param: JERKXY
// @DisplayName: Jerk limit for the horizontal kinematic input shaping
// @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the ship varies in acceleration
// @Range: 0 1
// @Units: m/s/s/s
// @User: Advanced
AP_GROUPINFO("JERKXY", 5, ModeShipOperation, ship_jerk_xy, 5.0f),
AP_GROUPINFO("JERKXY", 5, ModeShipOperation, ship_max_jerk_xy, 5.0f),

// @Param: ACCELZ
// @DisplayName: Acceleration limit for the vertical kinematic input shaping
// @Description: Acceleration limit of the vertical kinematic path generation used to determine how quickly the ship varies in velocity
// @Range: 0 1
// @Units: m/s/s
// @User: Advanced
AP_GROUPINFO("ACCELZ", 6, ModeShipOperation, ship_accel_z, 2.5f),
AP_GROUPINFO("ACCELZ", 6, ModeShipOperation, ship_max_accel_z, 2.5f),

// @Param: JERKZ
// @DisplayName: Jerk limit for the vertical kinematic input shaping
// @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the ship varies in acceleration
// @Range: 0 1
// @Units: m/s/s/s
// @User: Advanced
AP_GROUPINFO("JERKZ", 7, ModeShipOperation, ship_jerk_z, 5.0f),
AP_GROUPINFO("JERKZ", 7, ModeShipOperation, ship_max_jerk_z, 5.0f),

// @Param: ACCELH
// @DisplayName: Angular acceleration limit for the heading kinematic input shaping
// @Description: Angular acceleration limit of the heading kinematic path generation used to determine how quickly the ship varies in angular velocity
// @Range: 0 1
// @Units: deg/s/s
// @User: Advanced
AP_GROUPINFO("ACCELH", 8, ModeShipOperation, ship_accel_h, 90.0f),
AP_GROUPINFO("ACCELH", 8, ModeShipOperation, ship_max_accel_h, 90.0f),

// @Param: JERKH
// @DisplayName: Angular jerk limit for the heading kinematic input shaping
// @Description: Angular jerk limit of the heading kinematic path generation used to determine how quickly the ship varies in angular acceleration
// @Range: 0 1
// @Units: deg/s/s/s
// @User: Advanced
AP_GROUPINFO("JERKH", 9, ModeShipOperation, ship_jerk_h, 360.0f),
AP_GROUPINFO("JERKH", 9, ModeShipOperation, ship_max_jerk_h, 360.0f),

// @Param: KOZ_CW
// @DisplayName: CW angular offset in degrees from ship heading for keep out zone
Expand Down Expand Up @@ -259,9 +259,9 @@ void ModeShipOperation::run()
float yaw_cd = attitude_control->get_att_target_euler_cd().z;
float yaw_rate_cds = 0.0f;

Vector2f perch_offset = { ship_perch_radius * 100.0f, 0.0f };
perch_offset.rotate(radians(ship_perch_angle));
const float perch_height = ship_perch_altitude * 100.0;
Vector2f perch_offset = { perch_radius * 100.0f, 0.0f };
perch_offset.rotate(radians(perch_angle));
const float perch_height = perch_altitude * 100.0;

// get pilot desired climb rate if enabled
bool valid_pilot_input = rc().has_valid_input() && g.land_repositioning;
Expand Down Expand Up @@ -315,20 +315,20 @@ void ModeShipOperation::run()

shape_pos_vel_accel_xy(pos_with_ofs_ned.xy().topostype(), vel_ned_ms.xy(), accel_ned.xy(),
ship.pos_ned.xy(), ship.vel_ned.xy(), ship.accel_ned.xy(),
0.0, ship_accel_xy * 100.0,
ship_jerk_xy * 100.0, G_Dt, false);
0.0, ship_max_accel_xy * 100.0,
ship_max_jerk_xy * 100.0, G_Dt, false);
shape_pos_vel_accel(pos_with_ofs_ned.z, vel_ned_ms.z, accel_ned.z,
ship.pos_ned.z, ship.vel_ned.z, ship.accel_ned.z,
0.0, 0.0,
-ship_accel_z * 100.0, ship_accel_z * 100.0,
ship_jerk_z * 100.0, G_Dt, false);
-ship_max_accel_z * 100.0, ship_max_accel_z * 100.0,
ship_max_jerk_z * 100.0, G_Dt, false);
update_pos_vel_accel_xy(ship.pos_ned.xy(), ship.vel_ned.xy(), ship.accel_ned.xy(), G_Dt, Vector2f(), Vector2f(), Vector2f());
update_pos_vel_accel(ship.pos_ned.z, ship.vel_ned.z, ship.accel_ned.z, G_Dt, 0.0, 0.0, 0.0);

shape_angle_vel_accel(radians(target_heading_deg), 0.0, 0.0,
ship.heading, ship.heading_rate, ship.heading_accel,
0.0, radians(ship_accel_h),
radians(ship_jerk_h), G_Dt, false);
0.0, radians(ship_max_accel_h),
radians(ship_max_jerk_h), G_Dt, false);
postype_t ship_heading_p = ship.heading;
update_pos_vel_accel(ship_heading_p, ship.heading_rate, ship.heading_accel, G_Dt, 0.0, 0.0, 0.0);
ship.heading = wrap_PI(ship_heading_p);
Expand Down Expand Up @@ -425,9 +425,9 @@ void ModeShipOperation::run()
float keep_out_angle_rad = wrap_2PI(keep_out_CW_rad - keep_out_CCW_rad);
float koz_center_heading_rad = ship.heading + (keep_out_CW_rad + keep_out_CCW_rad) / 2.0;

float extension_distance_cm = ship_perch_radius * 10.0 + stopping_distance(wp_nav->get_default_speed_xy() + vel_ned_ms.xy().length(), 0.5 * pos_control->get_shaping_jerk_xy_cmsss() / wp_nav->get_wp_acceleration(), 0.5 * wp_nav->get_wp_acceleration());
extension_distance_cm = MIN(extension_distance_cm, ship_perch_radius * 100.0);
extension_distance_cm = MIN(extension_distance_cm, ship_perch_radius * 100.0 * sinf(0.5 * (2 * M_PI - keep_out_angle_rad * 100.0)));
float extension_distance_cm = perch_radius * 10.0 + stopping_distance(wp_nav->get_default_speed_xy() + vel_ned_ms.xy().length(), 0.5 * pos_control->get_shaping_jerk_xy_cmsss() / wp_nav->get_wp_acceleration(), 0.5 * wp_nav->get_wp_acceleration());
extension_distance_cm = MIN(extension_distance_cm, perch_radius * 100.0);
extension_distance_cm = MIN(extension_distance_cm, perch_radius * 100.0 * sinf(0.5 * (2 * M_PI - keep_out_angle_rad * 100.0)));
Vector2f extension_cm = { extension_distance_cm, 0.0 };

if (fabsf(wrap_PI(aircraft_bearing_rad - koz_center_heading_rad)) > keep_out_angle_rad / 2.0) {
Expand Down Expand Up @@ -658,7 +658,7 @@ void ModeShipOperation::run()
case SubMode::RETURN_TO_PERCH:
// check that position is within 10% of the Perch radius in x and y
// if throttle is low then reduce altitude to Perch altitude
pos_check = pos_error.xy().length() < ship_perch_radius * 10.0f;
pos_check = pos_error.xy().length() < perch_radius * 10.0f;
if (pos_check) {
set_state(SubMode::PERCH);
}
Expand Down

0 comments on commit ca645b2

Please sign in to comment.