mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 04:45:37 +08:00
[ground] Add heading to ublox2ivy (#2919)
This commit is contained in:
Freek van Tienen
GitHub
+1
-1
Submodule sw/ext/pprzlink updated: 19f9be4656...afb47d803c
@@ -33,6 +33,7 @@
|
||||
#include <netinet/in.h>
|
||||
#include <pthread.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <glib.h>
|
||||
@@ -41,6 +42,7 @@
|
||||
#include "ublox2ivy.h"
|
||||
|
||||
#define UDP_BUFFER_SIZE 1024
|
||||
#define NAV_RELPOSNED_VERSION 0x01
|
||||
|
||||
/* Endpoints */
|
||||
struct endpoint_udp_t {
|
||||
@@ -79,7 +81,8 @@ static bool verbose = false;
|
||||
static struct endpoint_t gps_ep;
|
||||
static struct gps_ubx_t gps_ubx;
|
||||
static struct gps_rtcm_t gps_rtcm;
|
||||
static uint8_t ac_id = 1;
|
||||
static uint8_t ac_id = 0;
|
||||
static float ground_heading = NAN;
|
||||
|
||||
void packet_handler(void *ep, uint8_t *data, uint16_t len);
|
||||
|
||||
@@ -598,7 +601,7 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
|
||||
IvySendMsg("ground FLIGHT_PARAM GCS %f %f %f %f %f %f %f %f %f %f %f %d %f",
|
||||
0.0, // roll,
|
||||
0.0, // pitch,
|
||||
0.0, // heading
|
||||
ground_heading, // heading
|
||||
lat,
|
||||
lon,
|
||||
gSpeed,
|
||||
@@ -610,7 +613,7 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
|
||||
iTOW, // itow
|
||||
0.0); // airspeed
|
||||
|
||||
IvySendMsg("ground TARGET_POS %d %d %d %d %d %f %f %f",
|
||||
IvySendMsg("ground TARGET_POS %d %d %d %d %d %f %f %f %f",
|
||||
ac_id,
|
||||
ac_id,
|
||||
(int)(lat * 1e7),
|
||||
@@ -618,7 +621,29 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
|
||||
(int)(alt * 1000),
|
||||
gSpeed,
|
||||
-velD,
|
||||
headMot);
|
||||
headMot,
|
||||
ground_heading);
|
||||
break;
|
||||
}
|
||||
case UBX_NAV_RELPOSNED_ID: {
|
||||
/* Get the heading of the RTK base station */
|
||||
uint8_t version = UBX_NAV_RELPOSNED_version(gps_ubx.msg_buf);
|
||||
if(version > NAV_RELPOSNED_VERSION)
|
||||
break;
|
||||
|
||||
uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
|
||||
uint8_t relPosValid = RTCMgetbitu(&flags, 5, 1);
|
||||
float relpos_heading = UBX_NAV_RELPOSNED_relPosHeading(gps_ubx.msg_buf) * 1e-5f;
|
||||
float relpos_dist = UBX_NAV_RELPOSNED_relPosLength(gps_ubx.msg_buf) * 1e-2f;
|
||||
|
||||
if(verbose) printf("Got relpos %d %f %f\r\n", flags, relpos_heading, relpos_dist);
|
||||
|
||||
if(relPosValid) {
|
||||
ground_heading = relpos_heading;
|
||||
} else {
|
||||
ground_heading = NAN;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
@@ -659,6 +684,7 @@ int main(int argc, char** argv) {
|
||||
|
||||
/* Arguments options and usage information */
|
||||
static struct option long_options[] = {
|
||||
{"ac_id", required_argument, NULL, 'i'},
|
||||
{"endpoint", required_argument, NULL, 'e'},
|
||||
{"help", no_argument, NULL, 'h'},
|
||||
{"verbose", no_argument, NULL, 'v'},
|
||||
@@ -667,14 +693,19 @@ int main(int argc, char** argv) {
|
||||
static const char* usage =
|
||||
"Usage: %s [options]\n"
|
||||
" Options :\n"
|
||||
" -i --ac_id [aircraft_id] Aircraft id\n"
|
||||
" -e --endpoint [endpoint_str] Endpoint address of the GPS\n"
|
||||
" -h --help Display this help\n"
|
||||
" -v --verbose Print verbose information\n";
|
||||
|
||||
int c;
|
||||
int option_index = 0;
|
||||
while((c = getopt_long(argc, argv, "e:hv", long_options, &option_index)) != -1) {
|
||||
while((c = getopt_long(argc, argv, "i:e:hv", long_options, &option_index)) != -1) {
|
||||
switch (c) {
|
||||
case 'i':
|
||||
ac_id = atoi(optarg);
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
// Parse the endpoint argument UDP
|
||||
if(!strncmp(optarg, "udp", 3)) {
|
||||
|
||||
@@ -79,6 +79,30 @@
|
||||
#define UBX_NAV_PVT_magDec(_ubx_payload) (int16_t)(*((uint8_t*)_ubx_payload+88)|(int16_t)(*((uint8_t*)_ubx_payload+1+88))<<8)
|
||||
#define UBX_NAV_PVT_magAcc(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+90)|(uint16_t)(*((uint8_t*)_ubx_payload+1+90))<<8)
|
||||
|
||||
/* UBX RELPOSNED message */
|
||||
#define UBX_NAV_RELPOSNED_ID 0x3C
|
||||
#define UBX_NAV_RELPOSNED_version(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
|
||||
#define UBX_NAV_RELPOSNED_reserved1(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))
|
||||
#define UBX_NAV_RELPOSNED_refStationId(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+2)|(uint16_t)(*((uint8_t*)_ubx_payload+1+2))<<8)
|
||||
#define UBX_NAV_RELPOSNED_iTOW(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+4)|(uint32_t)(*((uint8_t*)_ubx_payload+1+4))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
|
||||
#define UBX_NAV_RELPOSNED_relPosN(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+8)|(int32_t)(*((uint8_t*)_ubx_payload+1+8))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
|
||||
#define UBX_NAV_RELPOSNED_relPosE(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+12)|(int32_t)(*((uint8_t*)_ubx_payload+1+12))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
|
||||
#define UBX_NAV_RELPOSNED_relPosD(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+16)|(int32_t)(*((uint8_t*)_ubx_payload+1+16))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
|
||||
#define UBX_NAV_RELPOSNED_relPosLength(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+20)|(int32_t)(*((uint8_t*)_ubx_payload+1+20))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
|
||||
#define UBX_NAV_RELPOSNED_relPosHeading(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+24)|(int32_t)(*((uint8_t*)_ubx_payload+1+24))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
|
||||
#define UBX_NAV_RELPOSNED_reserved2(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+28)|(uint32_t)(*((uint8_t*)_ubx_payload+1+28))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
|
||||
#define UBX_NAV_RELPOSNED_relPosHPN(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+32))
|
||||
#define UBX_NAV_RELPOSNED_relPosHPE(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+33))
|
||||
#define UBX_NAV_RELPOSNED_relPosHPD(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+34))
|
||||
#define UBX_NAV_RELPOSNED_relPosHPLength(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+35))
|
||||
#define UBX_NAV_RELPOSNED_accN(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+36)|(uint32_t)(*((uint8_t*)_ubx_payload+1+36))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+36))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+36))<<24)
|
||||
#define UBX_NAV_RELPOSNED_accE(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+40)|(uint32_t)(*((uint8_t*)_ubx_payload+1+40))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+40))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+40))<<24)
|
||||
#define UBX_NAV_RELPOSNED_accD(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+44)|(uint32_t)(*((uint8_t*)_ubx_payload+1+44))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+44))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+44))<<24)
|
||||
#define UBX_NAV_RELPOSNED_accLength(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+48)|(uint32_t)(*((uint8_t*)_ubx_payload+1+48))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+48))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+48))<<24)
|
||||
#define UBX_NAV_RELPOSNED_accHeading(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+52)|(uint32_t)(*((uint8_t*)_ubx_payload+1+52))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+52))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+52))<<24)
|
||||
#define UBX_NAV_RELPOSNED_reserved3(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+56)|(uint32_t)(*((uint8_t*)_ubx_payload+1+56))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+56))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+56))<<24)
|
||||
#define UBX_NAV_RELPOSNED_flags(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+60)|(uint32_t)(*((uint8_t*)_ubx_payload+1+60))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+60))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+60))<<24)
|
||||
|
||||
struct gps_ubx_t {
|
||||
bool msg_available;
|
||||
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned));
|
||||
|
||||
@@ -144,7 +144,7 @@ static void update_gps(struct gps_data_t *gpsdata,
|
||||
|
||||
|
||||
if(strcmp(ac, "NONE") != 0) {
|
||||
IvySendMsg("%s TARGET_POS %s %s %d %d %d %f %f %f", "0", "0", ac, (int)(gpsdata->fix.latitude * 1e7), (int)(gpsdata->fix.longitude * 1e7), (int)(fix_altitude* 1000), fix_speed, fix_climb, fix_track);
|
||||
IvySendMsg("%s TARGET_POS %s %s %d %d %d %f %f %f %f", "0", "0", ac, (int)(gpsdata->fix.latitude * 1e7), (int)(gpsdata->fix.longitude * 1e7), (int)(fix_altitude* 1000), fix_speed, fix_climb, fix_track, NAN);
|
||||
if (verbose)
|
||||
printf("sending TARGET_POS for aircraft %s\n", ac);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user