dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

consider scale parameters in rc calibration code

This commit is contained in:
tumbili 2016-06-09 17:26:59 +02:00 committed by Andreas Antener
parent f0dd5a103c
commit 71e2a43790
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/commander/rc_calibration.cpp
View File

@ -79,18 +79,26 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
float yaw_trim_active;
param_get(param_find("TRIM_YAW"), &yaw_trim_active);
/* get manual control scale values */
float roll_scale;
param_get(param_find("FW_MAN_R_SC"), &roll_scale);
float pitch_scale;
param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
float yaw_scale;
param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);
/* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user
*/
float p = sp.y + roll_trim_active;
float p = sp.y * roll_scale + roll_trim_active;
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -sp.x + pitch_trim_active;
p = -sp.x * pitch_scale + pitch_trim_active;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = sp.r + yaw_trim_active;
p = sp.r * yaw_scale + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */