Paparazzi UAS  v5.15_devel-164-g81d4ceb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
e_identification_fr.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Fabien Bonneval
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 #include "state.h"
28 #include "mcu_periph/uart.h"
29 #include "subsystems/gps.h"
30 #include "generated/airframe.h"
31 #include "generated/flight_plan.h"
32 
33 #if defined(ROTORCRAFT_FIRMWARE)
35 #endif
36 
37 #define MAX_BUF_LEN 74
38 
39 #define LEN_ID_FR 30
40 #define LEN_ID_SERIAL 24
41 
42 static struct uart_periph *dev = &(E_ID_DEV);
43 static char id_fr[LEN_ID_FR];
44 
45 static int put_ID(uint8_t *buf)
46 {
47  buf[0] = E_ID_ID_FR;
48  buf[1] = strlen(id_fr);
49  memcpy(buf + 2, id_fr, LEN_ID_FR);
50  return LEN_ID_FR + 2;
51 }
52 
53 static int put_lat(uint8_t *buf)
54 {
55  int32_t lat = stateGetPositionLla_i()->lat / 1e2;
56  buf[0] = E_ID_LAT;
57  buf[1] = sizeof(lat);
58  memcpy(buf + 2, &lat, sizeof(lat));
59  return sizeof(lat) + 2;
60 }
61 
62 static int put_lon(uint8_t *buf)
63 {
64  int32_t lon = stateGetPositionLla_i()->lon / 1e2;
65  buf[0] = E_ID_LON;
66  buf[1] = sizeof(lon);
67  memcpy(buf + 2, &lon, sizeof(lon));
68  return sizeof(lon) + 2;
69 }
70 
71 static int put_alt(uint8_t *buf)
72 {
73  int16_t alt = gps.hmsl / 1e3;
74  buf[0] = E_ID_HMSL;
75  buf[1] = sizeof(alt);
76  memcpy(buf + 2, &alt, sizeof(alt));
77  return sizeof(alt) + 2;
78 }
79 
80 static int put_horizontal_speed(uint8_t *buf)
81 {
83  buf[0] = E_ID_H_SPEED;
84  buf[1] = sizeof(speed);
85  memcpy(buf + 2, &speed, sizeof(speed));
86  return sizeof(speed) + 2;
87 }
88 
89 static int put_route(uint8_t *buf)
90 {
92  uint16_t route;
93  if (route_normalized >= 0) {
94  route = (uint16_t) route_normalized;
95  } else {
96  route = (uint16_t)(360 + route_normalized);
97  }
98  buf[0] = E_ID_ROUTE;
99  buf[1] = sizeof(route);
100  memcpy(buf + 2, &route, sizeof(route));
101  return sizeof(route) + 2;
102 }
103 
104 static int put_lat_lon_home(uint8_t *buf)
105 {
106  struct LlaCoor_i *ptr_home_lla_i;
107 #if defined(FIXEDWING_FIRMWARE)
108  struct LlaCoor_i home_lla_i;
109  struct LlaCoor_f home_lla_f;
110  struct UtmCoor_f home_utm_f;
111 
112  home_utm_f.east = nav_utm_east0 + waypoints[WP_HOME].x;
113  home_utm_f.north = nav_utm_north0 + waypoints[WP_HOME].y;
114  home_utm_f.zone = nav_utm_zone0;
115  home_utm_f.alt = 0; // don't care about the altitude
116 
117  lla_of_utm_f(&home_lla_f, &home_utm_f);
118  LLA_BFP_OF_REAL(home_lla_i, home_lla_f);
119  ptr_home_lla_i = &home_lla_i;
120 
121 
122 #elif defined(ROTORCRAFT_FIRMWARE)
123  ptr_home_lla_i = waypoint_get_lla(WP_HOME);
124 #else
125 #error Not a fixedwing or a rotorcraft, not yet supported
126 #endif
127 
128  int32_t lat_home = ptr_home_lla_i->lat / 1e2;
129  int32_t lon_home = ptr_home_lla_i->lon / 1e2;
130 
131  int offset = 0;
132  buf[offset++] = E_ID_LAT_TO;
133  buf[offset++] = sizeof(lat_home);
134  memcpy(buf + offset, &lat_home, sizeof(lat_home));
135  offset += sizeof(lat_home);
136 
137  buf[offset++] = E_ID_LON_TO;
138  buf[offset++] = sizeof(lon_home);
139  memcpy(buf + offset, &lon_home, sizeof(lon_home));
140  offset += sizeof(lon_home);
141 
142  return offset;
143 }
144 
146 {
147  for (int i = 0; i < LEN_ID_FR; i++) {
148  id_fr[i] = '0';
149  }
150 
151  memcpy(id_fr, ID_MANUFACTURER, 3);
152  memcpy(id_fr + 3, ID_MODEL, 3);
153  int id_len = Min(strlen(ID_SERIAL), LEN_ID_SERIAL);
154  memcpy(id_fr + LEN_ID_SERIAL + 6 - id_len, ID_SERIAL, id_len);
155 }
156 
157 
159 {
160 
161  if (gps.fix) {
162  uint8_t buf[MAX_BUF_LEN];
163  buf[0] = 0X99; //PPRZ_STX
164  buf[1] = 0; //filled later with message length
165  uint8_t offset = 2;
166 
167  offset += put_ID(buf + offset);
168  offset += put_lat(buf + offset);
169  offset += put_lon(buf + offset);
170  offset += put_alt(buf + offset);
171  offset += put_lat_lon_home(buf + offset);
172  offset += put_route(buf + offset);
173  offset += put_horizontal_speed(buf + offset);
174 
175  buf[1] = offset + 2; //include from STX to checksum_b
176 
177  uint8_t ck_a = 0;
178  uint8_t ck_b = 0;
179  for (int i = 1; i < offset; i++) {
180  ck_a += buf[i];
181  ck_b += ck_a;
182  }
183  buf[offset++] = ck_a;
184  buf[offset++] = ck_b;
185 
186  uart_put_buffer(dev, 0, buf, offset);
187  }
188 }
189 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define Min(x, y)
Definition: esc_dshot.c:85
float x
Definition: common_nav.h:40
float east
in meters
float north
in meters
#define INT32_DEG_OF_RAD(_rad)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
#define MAX_BUF_LEN
static struct uart_periph * dev
static int put_route(uint8_t *buf)
void uart_put_buffer(struct uart_periph *p, long fd, const uint8_t *data, uint16_t len)
Uart transmit buffer implementation.
Definition: uart_arch.c:1021
static int put_lon(uint8_t *buf)
uint8_t nav_utm_zone0
Definition: common_nav.c:44
uint8_t ck_a
Definition: w5100.c:91
position in UTM coordinates Units: meters
static int put_lat_lon_home(uint8_t *buf)
vector in Latitude, Longitude and Altitude
vector in Latitude, Longitude and Altitude
float y
Definition: common_nav.h:41
#define LEN_ID_SERIAL
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
int32_t nav_utm_north0
Definition: common_nav.c:43
static const float offset[]
uint8_t ck_b
Definition: w5100.c:91
UART peripheral.
Definition: uart.h:70
Device independent GPS code (interface)
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
signed short int16_t
Definition: types.h:17
int32_t lon
in degrees*1e7
static int put_horizontal_speed(uint8_t *buf)
uint8_t zone
UTM zone number.
signed long int32_t
Definition: types.h:19
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
#define LEN_ID_FR
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t nav_utm_east0
Definition: common_nav.c:42
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
static int put_ID(uint8_t *buf)
#define LLA_BFP_OF_REAL(_o, _i)
static char id_fr[LEN_ID_FR]
struct LlaCoor_i * waypoint_get_lla(uint8_t wp_id)
Get LLA coordinates of waypoint.
Definition: waypoints.c:310
void e_identification_fr_periodic()
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:107
struct GpsState gps
global GPS state
Definition: gps.c:69
void e_identification_fr_init()
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
static int put_lat(uint8_t *buf)
static int put_alt(uint8_t *buf)