Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: stop using custom pos controller

Browse Source
This commit is contained in:
Iampete1 2022-07-03 19:57:24 +01:00 committed by Willian Galvani
parent 084bae21ef
commit 720117298d
3 changed files with 2 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/ArduSub.cpp
View File

@ -275,9 +275,6 @@ void Sub::one_hz_loop()
// update assigned functions and enable auxiliary servos // update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos(); SRV_Channels::enable_aux_servos();
// update position controller alt limits
update_poscon_alt_max();
// log terrain data // log terrain data
terrain_logging(); terrain_logging();

23
ArduSub/Attitude.cpp
View File

@ -163,29 +163,6 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
#endif #endif
} }
// updates position controller's maximum altitude using fence and EKF limits
void Sub::update_poscon_alt_max()
{
// minimum altitude, ie. maximum depth
// interpreted as no limit if left as zero
float min_alt_cm = 0.0;
// no limit if greater than 100, a limit is necessary,
// or the vehicle will try to fly out of the water
float max_alt_cm = g.surface_depth; // minimum depth
#if AC_FENCE == ENABLED
// set fence altitude limit in position controller
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
min_alt_cm = fence.get_safe_alt_min()*100.0f;
max_alt_cm = fence.get_safe_alt_max()*100.0f;
}
#endif
// pass limit to pos controller
pos_control.set_alt_min(min_alt_cm);
pos_control.set_alt_max(max_alt_cm);
}
// rotate vector from vehicle's perspective to North-East frame // rotate vector from vehicle's perspective to North-East frame
void Sub::rotate_body_frame_to_NE(float &x, float &y) void Sub::rotate_body_frame_to_NE(float &x, float &y)
{ {

5
ArduSub/Sub.h
View File

@ -46,7 +46,7 @@
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Mission/AP_Mission.h> // Mission command library #include <AP_Mission/AP_Mission.h> // Mission command library
#include <AC_AttitudeControl/AC_AttitudeControl_Sub.h> // Attitude control library #include <AC_AttitudeControl/AC_AttitudeControl_Sub.h> // Attitude control library
#include <AC_AttitudeControl/AC_PosControl_Sub.h> // Position control library #include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <AP_Motors/AP_Motors.h> // AP Motors library #include <AP_Motors/AP_Motors.h> // AP Motors library
#include <Filter/Filter.h> // Filter library #include <Filter/Filter.h> // Filter library
#include <AP_Relay/AP_Relay.h> // APM relay #include <AP_Relay/AP_Relay.h> // APM relay
@ -328,7 +328,7 @@ private:
// To-Do: move inertial nav up or other navigation variables down here // To-Do: move inertial nav up or other navigation variables down here
AC_AttitudeControl_Sub attitude_control; AC_AttitudeControl_Sub attitude_control;
AC_PosControl_Sub pos_control; AC_PosControl pos_control;
AC_WPNav wp_nav; AC_WPNav wp_nav;
AC_Loiter loiter_nav; AC_Loiter loiter_nav;
@ -401,7 +401,6 @@ private:
float get_look_ahead_yaw(); float get_look_ahead_yaw();
float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_climb_rate(float throttle_control);
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
void update_poscon_alt_max();
void rotate_body_frame_to_NE(float &x, float &y); void rotate_body_frame_to_NE(float &x, float &y);
void Log_Write_Control_Tuning(); void Log_Write_Control_Tuning();
void Log_Write_Attitude(); void Log_Write_Attitude();