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 "Copter.h"
#include <AP_Math/chirp.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_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
#include <AP_Math/control.h>
class Parameters; class Parameters;
class ParametersG2; class ParametersG2;
@ -1671,7 +1672,8 @@ private:
float time_const_freq; // Time at constant frequency before chirp starts float time_const_freq; // Time at constant frequency before chirp starts
int8_t log_subsample; // Subsample multiple for logging. int8_t log_subsample; // Subsample multiple for logging.
Vector2f target_vel; // target velocity for position controller modes 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 // System ID states
enum class SystemIDModeState { enum class SystemIDModeState {
SYSTEMID_STATE_STOPPED, 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()) { if (!pos_control->is_active_z()) {
pos_control->init_z_controller(); 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(); att_bf_feedforward = attitude_control->get_bf_feedforward();
@ -358,7 +362,12 @@ void ModeSystemId::run()
} }
Vector2f accel; 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 // run pos controller
pos_control->update_xy_controller(); pos_control->update_xy_controller();