PX4 Firmware
PX4 Autopilot Software http://px4.io
send_event.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 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 #include "send_event.h"
36 
37 #include <math.h>
38 
39 #include <px4_platform_common/getopt.h>
40 #include <px4_platform_common/log.h>
41 #include <drivers/drv_hrt.h>
42 
43 namespace events
44 {
45 
46 struct work_s SendEvent::_work = {};
47 
48 // Run it at 30 Hz.
49 const unsigned SEND_EVENT_INTERVAL_US = 33000;
50 
51 
52 int SendEvent::task_spawn(int argc, char *argv[])
53 {
54  int ret = work_queue(LPWORK, &_work, (worker_t)&SendEvent::initialize_trampoline, nullptr, 0);
55 
56  if (ret < 0) {
57  return ret;
58  }
59 
60  ret = wait_until_running();
61 
62  if (ret < 0) {
63  return ret;
64  }
65 
66  _task_id = task_id_is_work_queue;
67 
68  return 0;
69 }
70 
71 SendEvent::SendEvent() : ModuleParams(nullptr)
72 {
73  if (_param_ev_tsk_stat_dis.get()) {
75  }
76 
77  if (_param_ev_tsk_rc_loss.get()) {
79  }
80 }
81 
83 {
84  delete _status_display;
85  delete _rc_loss_alarm;
86 }
87 
89 {
90  if (is_running()) {
91  return 0;
92  }
93 
94  // Subscribe to the topics.
96 
97  // Kick off the cycling. We can call it directly because we're already in the work queue context.
98  cycle();
99 
100  return 0;
101 }
102 
104 {
105  SendEvent *send_event = new SendEvent();
106 
107  if (!send_event) {
108  PX4_ERR("alloc failed");
109  return;
110  }
111 
112  send_event->start();
113  _object.store(send_event);
114 }
115 
117 {
118  SendEvent *obj = static_cast<SendEvent *>(arg);
119 
120  obj->cycle();
121 }
122 
124 {
125  if (should_exit()) {
127  exit_and_cleanup();
128  return;
129  }
130 
132 
134 
135  if (_status_display != nullptr) {
137  }
138 
139  if (_rc_loss_alarm != nullptr) {
141  }
142 
143  work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this,
144  USEC2TICK(SEND_EVENT_INTERVAL_US));
145 }
146 
148 {
150  return;
151  }
152 
153  struct vehicle_command_s cmd;
154 
155  orb_copy(ORB_ID(vehicle_command), _subscriber_handler.get_vehicle_command_sub(), &cmd);
156 
157  bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;
158 
159  switch (cmd.command) {
160  case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
161  if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
162  gyro = true;
163  got_temperature_calibration_command = true;
164  }
165 
166  if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
167  accel = true;
168  got_temperature_calibration_command = true;
169  }
170 
171  if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
172  baro = true;
173  got_temperature_calibration_command = true;
174  }
175 
176  if (got_temperature_calibration_command) {
177  if (run_temperature_calibration(accel, baro, gyro) == 0) {
178  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
179 
180  } else {
181  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
182  }
183  }
184 
185  break;
186  }
187 
188 }
189 
190 void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
191 {
192  /* publish ACK */
193  vehicle_command_ack_s command_ack{};
194  command_ack.timestamp = hrt_absolute_time();
195  command_ack.command = cmd.command;
196  command_ack.result = (uint8_t)result;
197  command_ack.target_system = cmd.source_system;
198  command_ack.target_component = cmd.source_component;
199 
200  uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
201  command_ack_pub.publish(command_ack);
202 }
203 
204 int SendEvent::custom_command(int argc, char *argv[])
205 {
206  if (!strcmp(argv[0], "temperature_calibration")) {
207 
208  if (!is_running()) {
209  PX4_ERR("background task not running");
210  return -1;
211  }
212 
213  bool gyro_calib = false, accel_calib = false, baro_calib = false;
214  bool calib_all = true;
215  int myoptind = 1;
216  int ch;
217  const char *myoptarg = nullptr;
218 
219  while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) {
220  switch (ch) {
221  case 'a':
222  accel_calib = true;
223  calib_all = false;
224  break;
225 
226  case 'b':
227  baro_calib = true;
228  calib_all = false;
229  break;
230 
231  case 'g':
232  gyro_calib = true;
233  calib_all = false;
234  break;
235 
236  default:
237  print_usage("unrecognized flag");
238  return 1;
239  }
240  }
241 
242  vehicle_command_s vcmd{};
243  vcmd.timestamp = hrt_absolute_time();
244  vcmd.param1 = (float)((gyro_calib
245  || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
246  vcmd.param2 = NAN;
247  vcmd.param3 = NAN;
248  vcmd.param4 = NAN;
249  vcmd.param5 = ((accel_calib
250  || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN);
251  vcmd.param6 = (double)NAN;
252  vcmd.param7 = (float)((baro_calib
253  || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
254  vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
255 
256  uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
257  vcmd_pub.publish(vcmd);
258 
259  } else {
260  print_usage("unrecognized command");
261  }
262 
263  return 0;
264 }
265 
266 int SendEvent::print_usage(const char *reason)
267 {
268  if (reason) {
269  printf("%s\n\n", reason);
270  }
271 
272  PRINT_MODULE_DESCRIPTION(
273  R"DESCR_STR(
274 ### Description
275 Background process running periodically on the LP work queue to perform housekeeping tasks.
276 It is currently only responsible for temperature calibration and tone alarm on RC Loss.
277 
278 The tasks can be started via CLI or uORB topics (vehicle_command from MAVLink, etc.).
279 )DESCR_STR");
280 
281  PRINT_MODULE_USAGE_NAME("send_event", "system");
282  PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
283  PRINT_MODULE_USAGE_COMMAND_DESCR("temperature_calibration", "Run temperature calibration process");
284  PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true);
285  PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true);
286  PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true);
287  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
288 
289  return 0;
290 }
291 
292 int send_event_main(int argc, char *argv[])
293 {
294  return SendEvent::main(argc, argv);
295 }
296 
297 } /* namespace events */
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
class manages the RC loss audible alarm, LED status display, and thermal calibration.
Definition: send_event.cpp:46
void answer_command(const vehicle_command_s &cmd, unsigned result)
Returns an ACK to a vehicle_command.
Definition: send_event.cpp:190
static int task_spawn(int argc, char *argv[])
Spawns and initializes the class in the same context as the work queue and starts the background list...
Definition: send_event.cpp:52
int send_event_main(int argc, char *argv[])
Definition: send_event.cpp:292
int main(int argc, char **argv)
Definition: main.cpp:3
rc_loss::RC_Loss_Alarm * _rc_loss_alarm
Definition: send_event.h:134
static bool is_running()
Definition: dataman.cpp:415
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int run_temperature_calibration(bool accel, bool baro, bool gyro)
start temperature calibration in a new task for one or multiple sensors
Definition: task.cpp:356
void process_commands()
Checks for new commands and processes them.
Definition: send_event.cpp:147
int start()
Starts background task listening for commands.
Definition: send_event.cpp:88
void cycle()
Calls process_commands() and schedules the next cycle.
Definition: send_event.cpp:123
static int print_usage(const char *reason=nullptr)
Prints usage options to the console.
Definition: send_event.cpp:266
status::StatusDisplay * _status_display
Definition: send_event.h:131
const unsigned SEND_EVENT_INTERVAL_US
Definition: send_event.cpp:49
static int custom_command(int argc, char *argv[])
Recognizes custom startup commands, called from the main() function entry.
Definition: send_event.cpp:204
void process()
regularily called to handle state updates
queue struct.
SubscriberHandler _subscriber_handler
Definition: send_event.h:128
static void cycle_trampoline(void *arg)
Process cycle trampoline for the work queue.
Definition: send_event.cpp:116
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void process()
regularily called to handle state updates
static void initialize_trampoline(void *arg)
Trampoline for initialisation.
Definition: send_event.cpp:103