PX4 Firmware
PX4 Autopilot Software http://px4.io
rc_calibration.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2013 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file rc_calibration.cpp
36  * Remote Control calibration routine
37  */
38 
39 #include <px4_platform_common/posix.h>
40 #include <px4_platform_common/time.h>
41 #include <px4_platform_common/defines.h>
42 
43 #include "rc_calibration.h"
44 #include "commander_helper.h"
45 
46 #include <uORB/Subscription.hpp>
49 #include <systemlib/mavlink_log.h>
50 #include <parameters/param.h>
51 #include <systemlib/err.h>
52 
54 {
55  uORB::Subscription sub_man{ORB_ID(manual_control_setpoint)};
56  px4_usleep(400000);
58  bool changed = sub_man.updated();
59 
60  if (!changed) {
61  mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
62  return PX4_ERROR;
63  }
64 
65  sub_man.copy(&sp);
66 
67  /* load trim values which are active */
68  float roll_trim_active;
69  param_get(param_find("TRIM_ROLL"), &roll_trim_active);
70  float pitch_trim_active;
71  param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
72  float yaw_trim_active;
73  param_get(param_find("TRIM_YAW"), &yaw_trim_active);
74 
75  /* get manual control scale values */
76  float roll_scale;
77  param_get(param_find("FW_MAN_R_SC"), &roll_scale);
78  float pitch_scale;
79  param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
80  float yaw_scale;
81  param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);
82 
83  /* set parameters: the new trim values are the combination of active trim values
84  and the values coming from the remote control of the user
85  */
86  float p = sp.y * roll_scale + roll_trim_active;
87  int p1r = param_set(param_find("TRIM_ROLL"), &p);
88  /*
89  we explicitly swap sign here because the trim is added to the actuator controls
90  which are moving in an inverse sense to manual pitch inputs
91  */
92  p = -sp.x * pitch_scale + pitch_trim_active;
93  int p2r = param_set(param_find("TRIM_PITCH"), &p);
94  p = sp.r * yaw_scale + yaw_trim_active;
95  int p3r = param_set(param_find("TRIM_YAW"), &p);
96 
97  if (p1r != 0 || p2r != 0 || p3r != 0) {
98  mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
99  return PX4_ERROR;
100  }
101 
102  mavlink_log_info(mavlink_log_pub, "trim cal done");
103 
104  return PX4_OK;
105 }
static orb_advert_t * mavlink_log_pub
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
Definition: parameters.cpp:814
Global flash based parameter store.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Commander helper functions definitions.
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
Remote Control calibration routine.