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

Copter: change position controller method for making inputs

This commit is contained in:
bnsgeyer 2024-02-14 19:01:40 -05:00 committed by Bill Geyer
parent 75d9163571
commit 9e4340ba33
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/mode.h
View File

@ -3,6 +3,7 @@
#include "Copter.h"
#include <AP_Math/chirp.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
#include <AP_Math/control.h>
class Parameters;
class ParametersG2;
@ -1671,7 +1672,8 @@ private:
float time_const_freq; // Time at constant frequency before chirp starts
int8_t log_subsample; // Subsample multiple for logging.
Vector2f target_vel; // target velocity for position controller modes
Vector2f target_pos; // target positon
Vector2f input_vel_last; // last cycle input velocity
// System ID states
enum class SystemIDModeState {
SYSTEMID_STATE_STOPPED,

11
ArduCopter/mode_systemid.cpp
View File

@ -127,6 +127,10 @@ bool ModeSystemId::init(bool ignore_checks)
if (!pos_control->is_active_z()) {
pos_control->init_z_controller();
}
Vector3f curr_pos;
curr_pos = inertial_nav.get_position_neu_cm();
target_pos.x = curr_pos.x;
target_pos.y = curr_pos.y;
}
att_bf_feedforward = attitude_control->get_bf_feedforward();
@ -358,7 +362,12 @@ void ModeSystemId::run()
}
Vector2f accel;
pos_control->input_vel_accel_xy(input_vel, accel);
target_pos += input_vel * G_Dt;
if (is_positive(G_Dt)) {
accel = (input_vel - input_vel_last) / G_Dt;
input_vel_last = input_vel;
}
pos_control->set_pos_vel_accel_xy(target_pos.topostype(), input_vel, accel);
// run pos controller
pos_control->update_xy_controller();