PX4 Firmware
PX4 Autopilot Software http://px4.io
messages.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012 PX4 Development Team. All rights reserved.
4  * Author: @author Simon Wilks <sjwilks@gmail.com>
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file messages.c
37  *
38  */
39 
40 #include "messages.h"
41 
42 #include <math.h>
43 #include <stdio.h>
44 #include <string.h>
45 #include <lib/ecl/geo/geo.h>
46 #include <unistd.h>
47 #include <px4_platform_common/defines.h>
48 #include <uORB/topics/airspeed.h>
50 #include <uORB/topics/esc_status.h>
54 
55 #include <drivers/drv_hrt.h>
56 
57 /* The board is very roughly 5 deg warmer than the surrounding air */
58 #define BOARD_TEMP_OFFSET_DEG 5
59 
60 static int _battery_sub = -1;
61 static int _gps_sub = -1;
62 static int _home_sub = -1;
63 static int _airdata_sub = -1;
64 static int _airspeed_sub = -1;
65 static int _esc_sub = -1;
66 
67 static orb_advert_t _esc_pub = nullptr;
68 
69 static bool _home_position_set = false;
70 static double _home_lat = 0.0d;
71 static double _home_lon = 0.0d;
72 
73 void
75 {
77  _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
78  _home_sub = orb_subscribe(ORB_ID(home_position));
79  _airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data));
80  _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
81  _esc_sub = orb_subscribe(ORB_ID(esc_status));
82 }
83 
84 void
86 {
87 }
88 
89 void
90 build_gam_request(uint8_t *buffer, size_t *size)
91 {
92  struct gam_module_poll_msg msg;
93  *size = sizeof(msg);
94  memset(&msg, 0, *size);
95 
97  msg.id = GAM_SENSOR_ID;
98 
99  memcpy(buffer, &msg, *size);
100 }
101 
102 void
103 publish_gam_message(const uint8_t *buffer)
104 {
105  struct gam_module_msg msg;
106  size_t size = sizeof(msg);
107  memset(&msg, 0, size);
108  memcpy(&msg, buffer, size);
109  struct esc_status_s esc;
110  memset(&esc, 0, sizeof(esc));
111 
112  // Publish it.
114  esc.esc_count = 1;
115  esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM;
116 
117  esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
118  esc.esc[0].esc_temperature = msg.temperature1 - 20;
119  esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
120  esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
121 
122  /* announce the esc if needed, just publish else */
123  if (_esc_pub != nullptr) {
124  orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
125 
126  } else {
127  _esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
128  }
129 }
130 
131 void
132 build_eam_response(uint8_t *buffer, size_t *size)
133 {
134  /* get a local copy of the current sensor values */
135  vehicle_air_data_s airdata = {};
136  orb_copy(ORB_ID(vehicle_air_data), _airdata_sub, &airdata);
137 
138  /* get a local copy of the battery data */
139  struct battery_status_s battery;
140  memset(&battery, 0, sizeof(battery));
142 
143  struct eam_module_msg msg;
144  *size = sizeof(msg);
145  memset(&msg, 0, *size);
146 
147  msg.start = START_BYTE;
150 
151  msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20);
153 
154  msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
155 
156  uint16_t alt = (uint16_t)(airdata.baro_alt_meter + 500);
157  msg.altitude_L = (uint8_t)alt & 0xff;
158  msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
159 
160  /* get a local copy of the airspeed data */
161  struct airspeed_s airspeed;
162  memset(&airspeed, 0, sizeof(airspeed));
163  orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
164 
165  uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
166  msg.speed_L = (uint8_t)speed & 0xff;
167  msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
168 
169  msg.stop = STOP_BYTE;
170  memcpy(buffer, &msg, *size);
171 }
172 
173 void
174 build_gam_response(uint8_t *buffer, size_t *size)
175 {
176  /* get a local copy of the ESC Status values */
177  struct esc_status_s esc;
178  memset(&esc, 0, sizeof(esc));
179  orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
180 
181  struct gam_module_msg msg;
182  *size = sizeof(msg);
183  memset(&msg, 0, *size);
184 
185  msg.start = START_BYTE;
188 
189  msg.temperature1 = esc.esc[0].esc_temperature + 20;
190  msg.temperature2 = 20; // 0 deg. C.
191 
192  const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
193  msg.main_voltage_L = (uint8_t)voltage & 0xff;
194  msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
195 
196  const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
197  msg.current_L = (uint8_t)current & 0xff;
198  msg.current_H = (uint8_t)(current >> 8) & 0xff;
199 
200  const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
201  msg.rpm_L = (uint8_t)rpm & 0xff;
202  msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
203 
204  msg.stop = STOP_BYTE;
205  memcpy(buffer, &msg, *size);
206 }
207 
208 void
209 build_gps_response(uint8_t *buffer, size_t *size)
210 {
211  /* get a local copy of the battery data */
212  struct vehicle_gps_position_s gps;
213  memset(&gps, 0, sizeof(gps));
214  orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
215 
216  struct gps_module_msg msg;
217  *size = sizeof(msg);
218  memset(&msg, 0, *size);
219 
220  msg.start = START_BYTE;
221  msg.sensor_id = GPS_SENSOR_ID;
223 
224  msg.gps_num_sat = gps.satellites_used;
225 
226  /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
227  msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
228  msg.gps_fix = (uint8_t)(gps.fix_type + 48);
229 
230  /* No point collecting more data if we don't have a 3D fix yet */
231  if (gps.fix_type > 2) {
232  /* Current flight direction */
233  msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
234 
235  /* GPS speed */
236  uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
237  msg.gps_speed_L = (uint8_t)speed & 0xff;
238  msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
239 
240  /* Get latitude in degrees, minutes and seconds */
241  double lat = ((double)(gps.lat)) * 1e-7d;
242 
243  /* Set the N or S specifier */
244  msg.latitude_ns = 0;
245 
246  if (lat < 0) {
247  msg.latitude_ns = 1;
248  lat = abs(lat);
249  }
250 
251  int deg;
252  int min;
253  int sec;
254  convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
255 
256  uint16_t lat_min = (uint16_t)(deg * 100 + min);
257  msg.latitude_min_L = (uint8_t)lat_min & 0xff;
258  msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
259  uint16_t lat_sec = (uint16_t)(sec);
260  msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
261  msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
262 
263  /* Get longitude in degrees, minutes and seconds */
264  double lon = ((double)(gps.lon)) * 1e-7d;
265 
266  /* Set the E or W specifier */
267  msg.longitude_ew = 0;
268 
269  if (lon < 0) {
270  msg.longitude_ew = 1;
271  lon = abs(lon);
272  }
273 
274  convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
275 
276  uint16_t lon_min = (uint16_t)(deg * 100 + min);
277  msg.longitude_min_L = (uint8_t)lon_min & 0xff;
278  msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
279  uint16_t lon_sec = (uint16_t)(sec);
280  msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
281  msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
282 
283  /* Altitude */
284  uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
285  msg.altitude_L = (uint8_t)alt & 0xff;
286  msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
287 
288  /* Get any (and probably only ever one) _home_sub position report */
289  bool updated;
290  orb_check(_home_sub, &updated);
291 
292  if (updated) {
293  /* get a local copy of the home position data */
294  struct home_position_s home;
295  memset(&home, 0, sizeof(home));
296  orb_copy(ORB_ID(home_position), _home_sub, &home);
297 
298  _home_lat = home.lat;
299  _home_lon = home.lon;
300  _home_position_set = true;
301  }
302 
303  /* Distance from home */
304  if (_home_position_set) {
305  uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
306 
307  msg.distance_L = (uint8_t)dist & 0xff;
308  msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
309 
310  /* Direction back to home */
311  uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
312  msg.home_direction = (uint8_t)bearing >> 1;
313  }
314  }
315 
316  msg.stop = STOP_BYTE;
317  memcpy(buffer, &msg, *size);
318 }
319 
320 void
321 convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
322 {
323  *deg = (int)val;
324 
325  double delta = val - *deg;
326  const double min_d = delta * 60.0d;
327  *min = (int)min_d;
328  delta = min_d - *min;
329  *sec = (int)(delta * 10000.0d);
330 }
struct esc_report_s esc[8]
Definition: esc_status.h:67
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
static int _airdata_sub
Definition: messages.cpp:63
uint8_t latitude_min_L
231 0xE7 = 0x12E7 = 4839
Definition: messages.h:200
uint8_t longitude_min_L
150 157 = 0x9D = 0x039D = 0925
Definition: messages.h:206
uint8_t main_voltage_L
Main power voltage using 0.1V steps.
Definition: messages.h:163
#define START_BYTE
Definition: messages.h:63
Definition of geo / math functions to perform geodesic calculations.
uint8_t sensor_text_id
Definition: messages.h:135
uint8_t stop
Stop byte.
Definition: messages.h:176
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the distance to the next waypoint in meters.
Definition: geo.cpp:270
uint8_t distance_H
036 35 = /distance high byte
Definition: messages.h:212
uint8_t main_voltage_H
Definition: messages.h:164
#define BINARY_MODE_REQUEST_ID
Definition: messages.h:62
uint8_t stop
Stop byte.
Definition: messages.h:122
static int _battery_sub
Definition: messages.cpp:60
uint8_t speed_H
Definition: messages.h:121
uint8_t esc_connectiontype
Definition: esc_status.h:64
float esc_current
Definition: esc_report.h:57
void build_gam_response(uint8_t *buffer, size_t *size)
Definition: messages.cpp:174
uint8_t home_direction
HomeDirection (direction from starting point to Model position) (1 byte)
Definition: messages.h:220
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
uint64_t timestamp
Definition: esc_status.h:61
int32_t esc_rpm
Definition: esc_report.h:55
static double _home_lon
Definition: messages.cpp:71
uint8_t gps_speed_H
0 = /GPS speed high byte
Definition: messages.h:197
High-resolution timer with callouts and timekeeping.
uint8_t temperature1
Temperature sensor 1.
Definition: messages.h:103
uint8_t latitude_sec_H
016 3 = 0x03
Definition: messages.h:203
#define STOP_BYTE
Definition: messages.h:64
void convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
Definition: messages.cpp:321
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
#define GPS_SENSOR_TEXT_ID
Definition: messages.h:182
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
void build_gam_request(uint8_t *buffer, size_t *size)
Definition: messages.cpp:90
uint8_t gam_sensor_id
GAM sensor id.
Definition: messages.h:133
void init_sub_messages(void)
Definition: messages.cpp:74
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uint8_t gps_fix
00 ASCII Free Character [6], we use it for GPS FIX
Definition: messages.h:233
static double _home_lat
Definition: messages.cpp:70
#define GAM_SENSOR_ID
Definition: messages.h:128
uint8_t altitude_L
Attitude (meters) lower 8-bits.
Definition: messages.h:105
uint8_t start
Start byte.
Definition: messages.h:132
#define EAM_SENSOR_TEXT_ID
Definition: messages.h:75
uint8_t esc_temperature
Definition: esc_report.h:58
uint8_t sensor_text_id
Definition: messages.h:82
float esc_voltage
Definition: esc_report.h:56
#define BOARD_TEMP_OFFSET_DEG
Definition: messages.cpp:58
#define GAM_SENSOR_TEXT_ID
Definition: messages.h:129
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
uint8_t rpm_H
Definition: messages.h:155
void init_pub_messages(void)
Definition: messages.cpp:85
uint8_t temperature1
Temperature 1.
Definition: messages.h:148
uint8_t latitude_ns
000 = N = 48°39’988
Definition: messages.h:199
float indicated_airspeed_m_s
Definition: airspeed.h:54
uint8_t latitude_sec_L
171 220 = 0xDC = 0x03DC =0988
Definition: messages.h:202
uint8_t gps_num_sat
GPS.Satellites (number of satellites) (1 byte)
Definition: messages.h:218
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the bearing to the next waypoint in radians.
Definition: geo.cpp:320
uint8_t latitude_min_H
018 18 = 0x12
Definition: messages.h:201
uint8_t temperature2
Temperature 2.
Definition: messages.h:149
uint8_t temperature2
Definition: messages.h:104
#define GPS_SENSOR_ID
Definition: messages.h:181
uint8_t main_voltage_L
Main power voltage lower 8-bits in steps of 0.1V.
Definition: messages.h:109
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
uint8_t start
Start byte.
Definition: messages.h:79
uint8_t altitude_H
Definition: messages.h:106
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
uint8_t altitude_L
243 244 = /Altitude low byte 500 = 0m
Definition: messages.h:213
uint8_t altitude_H
001 1 = /Altitude high byte
Definition: messages.h:214
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
uint8_t esc_count
Definition: esc_status.h:63
static int _home_sub
Definition: messages.cpp:62
void build_eam_response(uint8_t *buffer, size_t *size)
Definition: messages.cpp:132
uint8_t distance_L
027 123 = /distance low byte 6 = 6 m
Definition: messages.h:211
uint8_t longitude_ew
000 = E= 9° 25’9360
Definition: messages.h:205
static bool _home_position_set
Definition: messages.cpp:69
uint8_t longitude_min_H
003 3 = 0x03
Definition: messages.h:207
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
#define EAM_SENSOR_ID
Definition: messages.h:74
uint8_t sensor_text_id
GPS Sensor text mode ID.
Definition: messages.h:192
uint8_t flight_direction
119 = Flightdir.
Definition: messages.h:195
static int _gps_sub
Definition: messages.cpp:61
uint8_t longitude_sec_L
056 144 = 0x90 0x2490 = 9360
Definition: messages.h:208
uint8_t eam_sensor_id
EAM sensor.
Definition: messages.h:80
uint8_t rpm_L
RPM in 10 RPM steps.
Definition: messages.h:154
uint8_t gps_speed_L
8 = /GPS speed low byte 8km/h
Definition: messages.h:196
static int _esc_sub
Definition: messages.cpp:65
uint8_t current_H
Definition: messages.h:162
The GPS sensor message Struct based on: https://code.google.com/p/diy-hott-gps/downloads.
Definition: messages.h:188
uint8_t current_L
Current in 0.1A steps.
Definition: messages.h:161
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
uint8_t start
Start byte.
Definition: messages.h:189
void build_gps_response(uint8_t *buffer, size_t *size)
Definition: messages.cpp:209
uint8_t sensor_id
GPS sensor ID.
Definition: messages.h:190
uint8_t speed_L
Airspeed in km/h in steps of 1 km/h.
Definition: messages.h:120
void publish_gam_message(const uint8_t *buffer)
Definition: messages.cpp:103
uint8_t longitude_sec_H
004 36 = 0x24
Definition: messages.h:209
uint8_t gps_fix_char
GPS.FixChar.
Definition: messages.h:219
uint8_t stop
Stop byte.
Definition: messages.h:235
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int _airspeed_sub
Definition: messages.cpp:64
static orb_advert_t _esc_pub
Definition: messages.cpp:67