PX4 Firmware
PX4 Autopilot Software http://px4.io
esc_calibration.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015-2017 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 esc_calibration.cpp
36  *
37  * Definition of esc calibration
38  *
39  * @author Roman Bapst <roman@px4.io>
40  */
41 
42 #include "esc_calibration.h"
43 #include "calibration_messages.h"
44 #include "calibration_routines.h"
45 
46 #include <drivers/drv_hrt.h>
47 #include <drivers/drv_pwm_output.h>
48 #include <px4_platform_common/defines.h>
49 #include <px4_platform_common/posix.h>
50 #include <px4_platform_common/time.h>
51 #include <systemlib/mavlink_log.h>
52 #include <uORB/Subscription.hpp>
54 
55 using namespace time_literals;
56 
58 {
59  int return_code = PX4_OK;
60 
61 #if defined(__PX4_POSIX_OCPOC) || defined(__PX4_POSIX_BBBLUE)
62  hrt_abstime timeout_start = 0;
63  hrt_abstime timeout_wait = 60_s;
64  armed->in_esc_calibration_mode = true;
65  calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc");
66  timeout_start = hrt_absolute_time();
67 
68  while (true) {
69  if (hrt_absolute_time() - timeout_start > timeout_wait) {
70  break;
71 
72  } else {
73  px4_usleep(50000);
74  }
75  }
76 
77  armed->in_esc_calibration_mode = false;
78  calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "end esc");
79 
80  if (return_code == OK) {
81  calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
82  }
83 
84  return return_code;
85 
86 #else
87  int fd = -1;
88  bool batt_connected = true; // for safety resons assume battery is connected, will be cleared below if not the case
89  hrt_abstime timeout_start = 0;
90 
91  calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
92 
94  const battery_status_s &battery = batt_sub.get();
95 
96  batt_sub.update();
97 
98  if (battery.timestamp == 0) {
99  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable");
100  return_code = PX4_ERROR;
101  goto Out;
102  }
103 
104  // Make sure battery is disconnected
105  // battery is not connected if the connected flag is not set and we have a recent battery measurement
106  batt_sub.update();
107 
108  if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) {
109  batt_connected = false;
110  }
111 
112  if (batt_connected) {
113  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
114  return_code = PX4_ERROR;
115  goto Out;
116  }
117 
118  armed->in_esc_calibration_mode = true;
119 
121 
122  if (fd < 0) {
123  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
124  return_code = PX4_ERROR;
125  goto Out;
126  }
127 
128  /* tell IO/FMU that its ok to disable its safety with the switch */
129  if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) {
130  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
131  return_code = PX4_ERROR;
132  goto Out;
133  }
134 
135  /* tell IO/FMU that the system is armed (it will output values if safety is off) */
136  if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) {
137  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
138  return_code = PX4_ERROR;
139  goto Out;
140  }
141 
142  /* tell IO to switch off safety without using the safety switch */
143  if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
144  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
145  return_code = PX4_ERROR;
146  goto Out;
147  }
148 
149  calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
150 
151  timeout_start = hrt_absolute_time();
152 
153  while (true) {
154  // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
155  // sit high.
156  static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
157  static constexpr hrt_abstime pwm_high_timeout{3_s};
158  hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
159 
160  if (hrt_elapsed_time(&timeout_start) > timeout_wait) {
161  if (!batt_connected) {
162  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
163  return_code = PX4_ERROR;
164  goto Out;
165  }
166 
167  // PWM was high long enough
168  break;
169  }
170 
171  if (!batt_connected) {
172  if (batt_sub.update()) {
173  if (battery.connected) {
174  // Battery is connected, signal to user and start waiting again
175  batt_connected = true;
176  timeout_start = hrt_absolute_time();
177  calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
178  }
179  }
180  }
181 
182  px4_usleep(50_ms);
183  }
184 
185 Out:
186 
187  if (fd != -1) {
188  if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
189  calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
190  }
191 
192  if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
193  calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
194  }
195 
196  if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
197  calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
198  }
199 
200  px4_close(fd);
201  }
202 
203  armed->in_esc_calibration_mode = false;
204 
205  if (return_code == PX4_OK) {
206  calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
207  }
208 
209  return return_code;
210 #endif
211 }
static orb_advert_t * mavlink_log_pub
#define PWM_SERVO_SET_ARM_OK
set the &#39;ARM ok&#39; bit, which activates the safety switch
#define CAL_QGC_DONE_MSG
#define PWM_SERVO_SET_FORCE_SAFETY_ON
force safety switch on (to enable use of safety switch)
#define PWM_SERVO_ARM
arm all servo outputs handle by this driver
Common calibration messages.
#define PWM_SERVO_CLEAR_ARM_OK
clear the &#39;ARM ok&#39; bit, which deactivates the safety switch
High-resolution timer with callouts and timekeeping.
bool in_esc_calibration_mode
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define PWM_OUTPUT0_DEVICE_PATH
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed)
static struct actuator_armed_s armed
Definition: Commander.cpp:139
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
#define CAL_QGC_STARTED_MSG
#define PWM_SERVO_SET_FORCE_SAFETY_OFF
force safety switch off (to disable use of safety switch)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
int px4_open(const char *path, int flags,...)
Definition of esc calibration.
#define OK
Definition: uavcan_main.cpp:71
#define calibration_log_critical(_pub, _text,...)
#define CAL_QGC_FAILED_MSG
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
#define calibration_log_info(_pub, _text,...)
#define PWM_SERVO_DISARM
disarm all servo outputs (stop generating pulses)
int px4_close(int fd)
int px4_ioctl(int fd, int cmd, unsigned long arg)