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

Merged release into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-13 11:06:01 +02:00
parent 2fc069bd63 a0176474c7
commit c9fefe236b
47 changed files with 1036 additions and 344 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/rc_calibration.cpp
+24 -10
View File
@@ -59,7 +59,7 @@ static const int ERROR = -1;
int do_trim_calibration(int mavlink_fd)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(200000);
usleep(400000);
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
@@ -71,20 +71,34 @@ int do_trim_calibration(int mavlink_fd)
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
/* load trim values which are active */
float roll_trim_active;
param_get(param_find("TRIM_ROLL"), &roll_trim_active);
float pitch_trim_active;
param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
float yaw_trim_active;
param_get(param_find("TRIM_YAW"), &yaw_trim_active);
/* 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;
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;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = sp.r + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL");
px4_close(sub_man);
return ERROR;
}