Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/gps/gps.h"
30 #include "generated/airframe.h"
31 #include "generated/flight_plan.h"
32 
33 #if defined(ROTORCRAFT_FIRMWARE)
34 #include "modules/nav/waypoints.h"
35 #endif
36 
37 #define MAX_BUF_LEN 50
38 
39 
40 static struct uart_periph *dev = &(E_ID_DEV);
42 
43 static int put_lat(uint8_t *buf)
44 {
45  int32_t lat = stateGetPositionLla_i()->lat / 1e2;
47  int32_t belat = __builtin_bswap32(lat);
48  buf[0] = E_ID_LAT;
49  buf[1] = sizeof(belat);
50  memcpy(buf + 2, &belat, sizeof(belat));
51  return sizeof(belat) + 2;
52 }
53 
54 static int put_lon(uint8_t *buf)
55 {
56  int32_t lon = stateGetPositionLla_i()->lon / 1e2;
57  int32_t belon = __builtin_bswap32(lon);
58  buf[0] = E_ID_LON;
59  buf[1] = sizeof(belon);
60  memcpy(buf + 2, &belon, sizeof(belon));
61  return sizeof(belon) + 2;
62 }
63 
64 static int put_alt(uint8_t *buf)
65 {
66  int16_t alt = gps.hmsl / 1e3;
67  int16_t bealt = __builtin_bswap16(alt);
68  buf[0] = E_ID_HMSL;
69  buf[1] = sizeof(bealt);
70  memcpy(buf + 2, &bealt, sizeof(bealt));
71  return sizeof(bealt) + 2;
72 }
73 
74 static int put_horizontal_speed(uint8_t *buf)
75 {
77  buf[0] = E_ID_H_SPEED;
78  buf[1] = sizeof(speed);
79  memcpy(buf + 2, &speed, sizeof(speed));
80  return sizeof(speed) + 2;
81 }
82 
83 static int put_route(uint8_t *buf)
84 {
85  int32_t route_normalized = DegOfRad(stateGetHorizontalSpeedDir_f());
86  uint16_t route;
87  if (route_normalized >= 0) {
88  route = (uint16_t) route_normalized;
89  } else {
90  route = (uint16_t)(360 + route_normalized);
91  }
92  uint16_t beroute = __builtin_bswap16(route);
93  buf[0] = E_ID_ROUTE;
94  buf[1] = sizeof(beroute);
95  memcpy(buf + 2, &beroute, sizeof(beroute));
96  return sizeof(beroute) + 2;
97 }
98 
99 static int put_lat_lon_home(uint8_t *buf)
100 {
101  struct LlaCoor_i *ptr_home_lla_i;
102 #if defined(FIXEDWING_FIRMWARE)
103  struct LlaCoor_i home_lla_i;
104  struct LlaCoor_f home_lla_f;
105  struct UtmCoor_f home_utm_f;
106 
107  home_utm_f.east = nav_utm_east0 + waypoints[WP_HOME].x;
108  home_utm_f.north = nav_utm_north0 + waypoints[WP_HOME].y;
109  home_utm_f.zone = nav_utm_zone0;
110  home_utm_f.alt = 0; // don't care about the altitude
111 
112  lla_of_utm_f(&home_lla_f, &home_utm_f);
113  LLA_BFP_OF_REAL(home_lla_i, home_lla_f);
114  ptr_home_lla_i = &home_lla_i;
115 
116 
117 #elif defined(ROTORCRAFT_FIRMWARE)
118  ptr_home_lla_i = waypoint_get_lla(WP_HOME);
119 #else
120 #error Not a fixedwing or a rotorcraft, not yet supported
121 #endif
122 
123  int32_t lat_home = ptr_home_lla_i->lat / 1e2;
124  int32_t lon_home = ptr_home_lla_i->lon / 1e2;
125  int32_t belat_home = __builtin_bswap32(lat_home);
126  int32_t belon_home = __builtin_bswap32(lon_home);
127 
128  int offset = 0;
129  buf[offset++] = E_ID_LAT_TO;
130  buf[offset++] = sizeof(belat_home);
131  memcpy(buf + offset, &belat_home, sizeof(belat_home));
132  offset += sizeof(belat_home);
133 
134  buf[offset++] = E_ID_LON_TO;
135  buf[offset++] = sizeof(belon_home);
136  memcpy(buf + offset, &belon_home, sizeof(belon_home));
137  offset += sizeof(belon_home);
138 
139  return offset;
140 }
141 
143 {
144 }
145 
146 
148 {
149 #if defined(FIXEDWING_FIRMWARE)
150  if (autopilot.launch) {
151 #elif defined(ROTORCRAFT_FIRMWARE)
152  if (autopilot_in_flight()) {
153 #else
154 #error "firmware unsupported!"
155 #endif
157  }
158 
160  uint8_t buf[MAX_BUF_LEN];
161  buf[0] = 0X99; //PPRZ_STX
162  buf[1] = 0; //filled later with message length
163  uint8_t offset = 2;
164 
165  offset += put_lat(buf + offset);
166  offset += put_lon(buf + offset);
167  offset += put_alt(buf + offset);
168  offset += put_lat_lon_home(buf + offset);
169  offset += put_route(buf + offset);
171 
172  buf[1] = offset + 2; //include from STX to checksum_b
173 
174  uint8_t ck_a = 0;
175  uint8_t ck_b = 0;
176  for (int i = 1; i < offset; i++) {
177  ck_a += buf[i];
178  ck_b += ck_a;
179  }
180  buf[offset++] = ck_a;
181  buf[offset++] = ck_b;
182 
183  uart_put_buffer(dev, 0, buf, offset);
184  }
185 }
186 
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:340
bool launch
request launch
Definition: autopilot.h:71
int32_t nav_utm_east0
Definition: common_nav.c:43
uint8_t nav_utm_zone0
Definition: common_nav.c:45
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
int32_t nav_utm_north0
Definition: common_nav.c:44
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
static const float offset[]
static int put_lon(uint8_t *buf)
void e_identification_fr_init()
#define MAX_BUF_LEN
static int put_lat_lon_home(uint8_t *buf)
static int put_lat(uint8_t *buf)
static int put_horizontal_speed(uint8_t *buf)
static int put_route(uint8_t *buf)
bool e_identification_started
void e_identification_fr_periodic()
static struct uart_periph * dev
static int put_alt(uint8_t *buf)
@ E_ID_LON
@ E_ID_LAT
@ E_ID_ROUTE
@ E_ID_HMSL
@ E_ID_H_SPEED
@ E_ID_LON_TO
@ E_ID_LAT_TO
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:93
#define GpsFixValid()
Definition: gps.h:176
int32_t lat
in degrees*1e7
int32_t lon
in degrees*1e7
#define LLA_BFP_OF_REAL(_o, _i)
vector in Latitude, Longitude and Altitude
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
struct LlaCoor_i * waypoint_get_lla(uint8_t wp_id)
Get LLA coordinates of waypoint.
Definition: waypoints.c:374
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float east
in meters
float north
in meters
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
API to get/set the generic vehicle states.
void WEAK uart_put_buffer(struct uart_periph *p, long fd, const uint8_t *data, uint16_t len)
Definition: uart.c:266
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
UART peripheral.
Definition: uart.h:72
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
uint8_t ck_a
Definition: w5100.c:91
uint8_t ck_b
Definition: w5100.c:91