PX4 Firmware
PX4 Autopilot Software http://px4.io
ReplayEkf2.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016-2019 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 <drivers/drv_hrt.h>
35 #include <px4_platform_common/defines.h>
36 #include <px4_platform_common/posix.h>
37 
38 // for ekf2 replay
39 #include <uORB/topics/airspeed.h>
52 
53 #include "ReplayEkf2.hpp"
54 
55 namespace px4
56 {
57 
58 bool
59 ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
60 {
61  if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
62  ekf2_timestamps_s ekf2_timestamps;
63  memcpy(&ekf2_timestamps, data, sub.orb_meta->o_size);
64 
65  if (!publishEkf2Topics(ekf2_timestamps, replay_file)) {
66  return false;
67  }
68 
69  px4_pollfd_struct_t fds[1];
70  fds[0].fd = _vehicle_attitude_sub;
71  fds[0].events = POLLIN;
72 
73  // wait for a response from the estimator
74  int pret = px4_poll(fds, 1, 1000);
75 
76  // introduce some breaks to make sure the logger can keep up
77  if (++_topic_counter == 50) {
78  px4_usleep(1000);
79  _topic_counter = 0;
80  }
81 
82  if (pret == 0) {
83  PX4_WARN("poll timeout");
84 
85  } else if (pret < 0) {
86  PX4_ERR("poll failed (%i)", pret);
87 
88  } else {
89  if (fds[0].revents & POLLIN) {
91  // need to to an orb_copy so that poll will not return immediately
92  orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att);
93  }
94  }
95 
96  return true;
97 
98  } else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)
99  || sub.orb_meta == ORB_ID(vehicle_gps_position)) {
100  return publishTopic(sub, data);
101  } // else: do not publish
102 
103  return false;
104 }
105 
106 void
108 {
109  if (sub.orb_meta == ORB_ID(sensor_combined)) {
110  _sensor_combined_msg_id = msg_id;
111 
112  } else if (sub.orb_meta == ORB_ID(airspeed)) {
113  _airspeed_msg_id = msg_id;
114 
115  } else if (sub.orb_meta == ORB_ID(distance_sensor)) {
116  _distance_sensor_msg_id = msg_id;
117 
118  } else if (sub.orb_meta == ORB_ID(vehicle_gps_position)) {
119  if (sub.multi_id == 0) {
120  _gps_msg_id = msg_id;
121  }
122 
123  } else if (sub.orb_meta == ORB_ID(optical_flow)) {
124  _optical_flow_msg_id = msg_id;
125 
126  } else if (sub.orb_meta == ORB_ID(vehicle_air_data)) {
127  _vehicle_air_data_msg_id = msg_id;
128 
129  } else if (sub.orb_meta == ORB_ID(vehicle_magnetometer)) {
131 
132  } else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) {
134  }
135 
136  // the main loop should only handle publication of the following topics, the sensor topics are
137  // handled separately in publishEkf2Topics()
138  sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status)
139  && sub.orb_meta != ORB_ID(vehicle_land_detected) &&
140  (sub.orb_meta != ORB_ID(vehicle_gps_position) || sub.multi_id == 0);
141 }
142 
143 bool
144 ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)
145 {
146  auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) {
147  if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) {
148  // timestamp_relative is already given in 0.1 ms
149  uint64_t t = timestamp_relative + ekf2_timestamps.timestamp / 100; // in 0.1 ms
150  findTimestampAndPublish(t, msg_id, replay_file);
151  }
152  };
153 
154  handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id);
155  handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id);
156  handle_sensor_publication(ekf2_timestamps.gps_timestamp_rel, _gps_msg_id);
157  handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id);
158  handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
159  handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
160  handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id);
161 
162  // sensor_combined: publish last because ekf2 is polling on this
163  if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
165  // subscription not found yet or sensor_combined not contained in log
166  return false;
167 
168  } else if (!_subscriptions[_sensor_combined_msg_id]->orb_meta) {
169  return false; // read past end of file
170 
171  } else {
172  // we should publish a topic, just publish the same again
174  publishTopic(*_subscriptions[_sensor_combined_msg_id], _read_buffer.data());
175  }
176  }
177 
178  return true;
179 }
180 
181 bool
182 ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file)
183 {
184  if (msg_id == msg_id_invalid) {
185  // could happen if a topic is not logged
186  return false;
187  }
188 
189  Subscription &sub = *_subscriptions[msg_id];
190 
191  while (sub.next_timestamp / 100 < timestamp && sub.orb_meta) {
192  nextDataMessage(replay_file, sub, msg_id);
193  }
194 
195  if (!sub.orb_meta) { // no messages anymore
196  return false;
197  }
198 
199  if (sub.next_timestamp / 100 != timestamp) {
200  // this can happen in beginning of the log or on a dropout
201  PX4_DEBUG("No timestamp match found for topic %s (%i, %i)", sub.orb_meta->o_name, (int)sub.next_timestamp / 100,
202  timestamp);
203  ++sub.error_counter;
204  return false;
205  }
206 
207  readTopicDataToBuffer(sub, replay_file);
208  publishTopic(sub, _read_buffer.data());
209  return true;
210 }
211 
212 void
214 {
215  _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
216 }
217 
218 void
220 {
221  // print statistics
222  auto print_sensor_statistics = [this](uint16_t msg_id, const char *name) {
223  if (msg_id != msg_id_invalid) {
224  Subscription &sub = *_subscriptions[msg_id];
225 
226  if (sub.publication_counter > 0 || sub.error_counter > 0) {
227  PX4_INFO("%s: %i, %i", name, sub.publication_counter, sub.error_counter);
228  }
229  }
230  };
231 
232  PX4_INFO("");
233  PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):");
234 
235  print_sensor_statistics(_airspeed_msg_id, "airspeed");
236  print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
237  print_sensor_statistics(_gps_msg_id, "vehicle_gps_position");
238  print_sensor_statistics(_optical_flow_msg_id, "optical_flow");
239  print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
240  print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
241  print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
242  print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");
243 
246 }
247 
248 uint64_t
249 ReplayEkf2::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
250 {
251  // no need for usleep
252  return next_file_time;
253 }
254 
255 } // namespace px4
uint16_t _vehicle_visual_odometry_msg_id
Definition: ReplayEkf2.hpp:90
bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file)
find the next message for a subscription that matches a given timestamp and publish it ...
Definition: ReplayEkf2.cpp:182
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
void readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_file)
read a topic from the file (offset given by the subscription) into _read_buffer
Definition: Replay.cpp:862
bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id)
Find next data message for this subscription, starting with the stored file offset.
Definition: Replay.cpp:557
bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)
Definition: ReplayEkf2.cpp:144
int16_t vehicle_magnetometer_timestamp_rel
std::vector< uint8_t > _read_buffer
Definition: Replay.hpp:210
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
High-resolution timer with callouts and timekeeping.
int16_t distance_sensor_timestamp_rel
const char * o_name
unique object name
Definition: uORB.h:51
int16_t airspeed_timestamp_rel
uint16_t _vehicle_magnetometer_msg_id
Definition: ReplayEkf2.hpp:89
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
std::vector< Subscription * > _subscriptions
Definition: Replay.hpp:209
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
bool ignored
if true, it will not be considered for publication in the main loop
Definition: Replay.hpp:135
void onEnterMainLoop() override
called when entering the main replay loop
Definition: ReplayEkf2.cpp:213
uint8_t * data
Definition: dataman.cpp:149
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
const orb_metadata * orb_meta
if nullptr, this subscription is invalid
Definition: Replay.hpp:130
bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override
handle ekf2 topic publication in ekf2 replay mode
Definition: ReplayEkf2.cpp:59
void onExitMainLoop() override
called when exiting the main replay loop
Definition: ReplayEkf2.cpp:219
uint16_t _gps_msg_id
Definition: ReplayEkf2.hpp:85
static constexpr uint16_t msg_id_invalid
Definition: ReplayEkf2.hpp:81
const char * name
Definition: tests_main.c:58
uint16_t _vehicle_air_data_msg_id
Definition: ReplayEkf2.hpp:88
uint16_t _airspeed_msg_id
Definition: ReplayEkf2.hpp:83
int _vehicle_attitude_sub
Definition: ReplayEkf2.hpp:79
int16_t optical_flow_timestamp_rel
bool publishTopic(Subscription &sub, void *data)
publish an orb topic
Definition: Replay.cpp:894
uint16_t _optical_flow_msg_id
Definition: ReplayEkf2.hpp:86
uint64_t next_timestamp
timestamp of the file
Definition: Replay.hpp:138
uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset) override
handle delay until topic can be published.
Definition: ReplayEkf2.cpp:249
int16_t visual_odometry_timestamp_rel
Definition: bst.cpp:62
void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override
called when a new subscription is added
Definition: ReplayEkf2.cpp:107
const uint16_t o_size
object size
Definition: uORB.h:52
int16_t vehicle_air_data_timestamp_rel
uint16_t _sensor_combined_msg_id
Definition: ReplayEkf2.hpp:87
uint16_t _distance_sensor_msg_id
Definition: ReplayEkf2.hpp:84