PX4 Firmware
PX4 Autopilot Software http://px4.io
camera_feedback.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 /**
35  * @file camera_feedback.cpp
36  *
37  * Online and offline geotagging from camera feedback
38  *
39  * @author Mohammed Kabir <kabir@uasys.io>
40  */
41 
42 #include "camera_feedback.hpp"
43 
44 namespace camera_feedback
45 {
47 }
48 
50  _task_should_exit(false),
51  _main_task(-1),
52  _trigger_sub(-1),
53  _gpos_sub(-1),
54  _att_sub(-1),
55  _capture_pub(nullptr),
56  _camera_capture_feedback(false)
57 {
58 
59  // Parameters
60  _p_camera_capture_feedback = param_find("CAM_CAP_FBACK");
61 
63 
64 }
65 
67 {
68 
69  if (_main_task != -1) {
70 
71  /* task wakes up every 100ms or so at the longest */
72  _task_should_exit = true;
73 
74  /* wait for a second for the task to quit at our request */
75  unsigned i = 0;
76 
77  do {
78  /* wait 20ms */
79  px4_usleep(20000);
80 
81  /* if we have given up, kill it */
82  if (++i > 50) {
83  px4_task_delete(_main_task);
84  break;
85  }
86  } while (_main_task != -1);
87  }
88 
90 }
91 
92 int
94 {
95 
96  /* start the task */
97  _main_task = px4_task_spawn_cmd("camera_feedback",
98  SCHED_DEFAULT,
99  SCHED_PRIORITY_DEFAULT + 15,
100  1600,
102  nullptr);
103 
104  if (_main_task < 0) {
105  warn("task start failed");
106  return -errno;
107  }
108 
109  return OK;
110 
111 }
112 
113 void
115 {
116  if (camera_feedback::g_camera_feedback != nullptr) {
118  }
119 }
120 
121 
122 void
124 {
126 
127  // Polling sources
128  struct camera_trigger_s trig = {};
129 
130  px4_pollfd_struct_t fds[1] = {};
131  fds[0].fd = _trigger_sub;
132  fds[0].events = POLLIN;
133 
134  // Geotagging subscriptions
135  _gpos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
136  _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
137  struct vehicle_global_position_s gpos = {};
138  struct vehicle_attitude_s att = {};
139 
140  bool updated = false;
141 
142  while (!_task_should_exit) {
143 
144  /* wait for up to 20ms for data */
145  int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20);
146 
147  if (pret < 0) {
148  PX4_WARN("poll error %d, %d", pret, errno);
149  continue;
150  }
151 
152  /* trigger subscription updated */
153  if (fds[0].revents & POLLIN) {
154 
156 
157  /* update geotagging subscriptions */
158  orb_check(_gpos_sub, &updated);
159 
160  if (updated) {
161  orb_copy(ORB_ID(vehicle_global_position), _gpos_sub, &gpos);
162  }
163 
164  orb_check(_att_sub, &updated);
165 
166  if (updated) {
167  orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
168  }
169 
170  if (trig.timestamp == 0 ||
171  gpos.timestamp == 0 ||
172  att.timestamp == 0) {
173  // reject until we have valid data
174  continue;
175  }
176 
177  struct camera_capture_s capture = {};
178 
179  // Fill timestamps
180  capture.timestamp = trig.timestamp;
181 
182  capture.timestamp_utc = trig.timestamp_utc;
183 
184  // Fill image sequence
185  capture.seq = trig.seq;
186 
187  // Fill position data
188  capture.lat = gpos.lat;
189 
190  capture.lon = gpos.lon;
191 
192  capture.alt = gpos.alt;
193 
194  capture.ground_distance = gpos.terrain_alt_valid ? (gpos.alt - gpos.terrain_alt) : -1.0f;
195 
196  // Fill attitude data
197  // TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
198  capture.q[0] = att.q[0];
199 
200  capture.q[1] = att.q[1];
201 
202  capture.q[2] = att.q[2];
203 
204  capture.q[3] = att.q[3];
205 
206  // Indicate whether capture feedback from camera is available
207  // What is case 0 for capture.result?
209  capture.result = -1;
210 
211  } else {
212  capture.result = 1;
213  }
214 
215  int instance_id;
216 
218 
219  }
220 
221  }
222 
226 
227  _main_task = -1;
228 
229 }
230 
231 int
233 {
235  return 0;
236 }
237 
238 static int usage()
239 {
240  PX4_INFO("usage: camera_feedback {start|stop}\n");
241  return 1;
242 }
243 
244 extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[]);
245 
246 int camera_feedback_main(int argc, char *argv[])
247 {
248  if (argc < 2) {
249  return usage();
250  }
251 
252  if (!strcmp(argv[1], "start")) {
253 
254  if (camera_feedback::g_camera_feedback != nullptr) {
255  PX4_WARN("already running");
256  return 0;
257  }
258 
260 
261  if (camera_feedback::g_camera_feedback == nullptr) {
262  PX4_WARN("alloc failed");
263  return 1;
264  }
265 
267  return 0;
268  }
269 
270  if (camera_feedback::g_camera_feedback == nullptr) {
271  PX4_WARN("not running");
272  return 1;
273 
274  } else if (!strcmp(argv[1], "stop")) {
276 
277  } else {
278  return usage();
279  }
280 
281  return 0;
282 }
int start()
Start the task.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
void stop()
Stop the task.
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
Definition: I2C.hpp:51
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
bool _task_should_exit
if true, task should exit
uint64_t timestamp_utc
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
orb_advert_t _capture_pub
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
param_t _p_camera_capture_feedback
CameraFeedback()
Constructor.
int32_t _camera_capture_feedback
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
uint64_t timestamp_utc
static int usage()
__EXPORT int camera_feedback_main(int argc, char *argv[])
~CameraFeedback()
Destructor, also kills task.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
CameraFeedback * g_camera_feedback
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
int _main_task
handle for task
#define OK
Definition: uavcan_main.cpp:71
static int task_main_trampoline(int argc, char *argv[])
Shim for calling task_main from task_create.
#define warn(...)
Definition: err.h:94
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177