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

Sub: updates for offset handling

This commit is contained in:
Leonard Hall 2024-09-17 13:49:43 +09:00 committed by Randy Mackay
parent ae01a8f26d
commit 6db90646b9
7 changed files with 12 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/GCS_Mavlink.cpp
View File

@ -822,7 +822,7 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
*/ */
if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD
sub.pos_control.set_pos_target_z_cm(packet.alt*100); sub.pos_control.set_pos_desired_z_cm(packet.alt*100);
break; break;
} }

2
ArduSub/mode_acro.cpp
View File

@ -7,7 +7,7 @@
bool ModeAcro::init(bool ignore_checks) { bool ModeAcro::init(bool ignore_checks) {
// set target altitude to zero for reporting // set target altitude to zero for reporting
position_control->set_pos_target_z_cm(0); position_control->set_pos_desired_z_cm(0);
// attitude hold inputs become thrust inputs in acro mode // attitude hold inputs become thrust inputs in acro mode
// set to neutral to prevent chaotic behavior (esp. roll/pitch) // set to neutral to prevent chaotic behavior (esp. roll/pitch)

4
ArduSub/mode_althold.cpp
View File

@ -111,9 +111,9 @@ void ModeAlthold::control_depth() {
//we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
if (fabsf(target_climb_rate_cm_s) < 0.05f) { if (fabsf(target_climb_rate_cm_s) < 0.05f) {
if (sub.ap.at_surface) { if (sub.ap.at_surface) {
position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
} else if (sub.ap.at_bottom) { } else if (sub.ap.at_bottom) {
position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm())); // set target to 10 cm above bottom
} }
} }

2
ArduSub/mode_guided.cpp
View File

@ -811,7 +811,7 @@ float ModeGuided::get_auto_heading()
float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy()); float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy());
// Bearing from current position towards intermediate position target (centidegrees) // Bearing from current position towards intermediate position target (centidegrees)
const Vector2f target_vel_xy{position_control->get_vel_target_cms().x, position_control->get_vel_target_cms().y}; const Vector2f target_vel_xy = position_control->get_vel_target_cms().xy();
float angle_error = 0.0f; float angle_error = 0.0f;
if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) { if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) {
const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f; const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;

2
ArduSub/mode_manual.cpp
View File

@ -3,7 +3,7 @@
bool ModeManual::init(bool ignore_checks) { bool ModeManual::init(bool ignore_checks) {
// set target altitude to zero for reporting // set target altitude to zero for reporting
position_control->set_pos_target_z_cm(0); position_control->set_pos_desired_z_cm(0);
// attitude hold inputs become thrust inputs in manual mode // attitude hold inputs become thrust inputs in manual mode
// set to neutral to prevent chaotic behavior (esp. roll/pitch) // set to neutral to prevent chaotic behavior (esp. roll/pitch)

2
ArduSub/mode_stabilize.cpp
View File

@ -3,7 +3,7 @@
bool ModeStabilize::init(bool ignore_checks) { bool ModeStabilize::init(bool ignore_checks) {
// set target altitude to zero for reporting // set target altitude to zero for reporting
position_control->set_pos_target_z_cm(0); position_control->set_pos_desired_z_cm(0);
sub.last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
return true; return true;

12
ArduSub/mode_surftrak.cpp
View File

@ -81,8 +81,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
// Initialize the terrain offset // Initialize the terrain offset
auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm; auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm;
sub.pos_control.set_pos_offset_z_cm(terrain_offset_cm); sub.pos_control.init_pos_terrain_cm(terrain_offset_cm);
sub.pos_control.set_pos_offset_target_z_cm(terrain_offset_cm);
} else { } else {
reset(); reset();
@ -97,8 +96,7 @@ void ModeSurftrak::reset()
rangefinder_target_cm = INVALID_TARGET; rangefinder_target_cm = INVALID_TARGET;
// Reset the terrain offset // Reset the terrain offset
sub.pos_control.set_pos_offset_z_cm(0); sub.pos_control.init_pos_terrain_cm(0);
sub.pos_control.set_pos_offset_target_z_cm(0);
} }
/* /*
@ -117,11 +115,11 @@ void ModeSurftrak::control_range() {
} }
if (sub.ap.at_surface) { if (sub.ap.at_surface) {
// Set target depth to 5 cm below SURFACE_DEPTH and reset // Set target depth to 5 cm below SURFACE_DEPTH and reset
position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f));
reset(); reset();
} else if (sub.ap.at_bottom) { } else if (sub.ap.at_bottom) {
// Set target depth to 10 cm above bottom and reset // Set target depth to 10 cm above bottom and reset
position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm()));
reset(); reset();
} else { } else {
// Typical operation // Typical operation
@ -164,7 +162,7 @@ void ModeSurftrak::update_surface_offset()
} }
// Set the offset target, AC_PosControl will do the rest // Set the offset target, AC_PosControl will do the rest
sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm); sub.pos_control.set_pos_terrain_target_cm(rangefinder_terrain_offset_cm);
} }
} }
#endif // AP_RANGEFINDER_ENABLED #endif // AP_RANGEFINDER_ENABLED