fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 22:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

- Added JC params to jetdrive control example app.

Browse Source 
- fixed offboard control mode rates
This commit is contained in:
Diogo Machado
2014-02-28 16:24:15 +00:00
parent be4af0ab3b
commit 0789189c05
2 changed files with 36 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/jetdrive_control/jetdrive_control_main.cpp
+22 -13
View File
@@ -95,19 +95,19 @@ JetdriveControl::JetdriveControl() :
I.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_p = param_find("JC_ROLL_P");
_params_handles.roll_rate_p = param_find("JC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("JC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("JC_ROLLRATE_D");
_params_handles.pitch_p = param_find("JC_PITCH_P");
_params_handles.pitch_rate_p = param_find("JC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("JC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("JC_PITCHRATE_D");
_params_handles.yaw_p = param_find("JC_YAW_P");
_params_handles.yaw_rate_p = param_find("JC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("JC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("JC_YAWRATE_D");
_params_handles.yaw_ff = param_find("JC_YAW_FF");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
@@ -463,6 +463,11 @@ JetdriveControl::control_attitude_rates(float dt)
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
// _att_control will be loaded to controls
//
// _att_control = (JC_XXXRATE_P * rates_err) + (JC_XXXRATE_D * (_rates_prev - rate) / dt) + _rates_int;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
_rates_prev = rates;
@@ -597,6 +602,10 @@ JetdriveControl::task_main()
/* attitude controller disabled */
// TODO poll 'attitude_rates_setpoint' topic
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
// _rates_sp.zero();
// _thrust_sp = 0.0f;
}
src/examples/jetdrive_control/jetdrive_control_params.c
+14 -13
View File
@@ -41,16 +41,17 @@
#include <systemlib/param/param.h>
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
PARAM_DEFINE_FLOAT(JC_ROLL_P, 6.0f);
PARAM_DEFINE_FLOAT(JC_ROLLRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(JC_ROLLRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(JC_ROLLRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(JC_PITCH_P, 6.0f);
PARAM_DEFINE_FLOAT(JC_PITCHRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(JC_PITCHRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(JC_PITCHRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(JC_YAW_P, 2.0f);
PARAM_DEFINE_FLOAT(JC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(JC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(JC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(JC_YAW_FF, 0.5f);