Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[skytraq] added new gps support

Browse Source
This commit is contained in:
masierra
2016-02-03 09:47:24 -08:00
parent c56fea0899
commit ac9cef2f5e
3 changed files with 86 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/shared/gps_skytraq.makefile
+3 -3
View File
@@ -4,7 +4,7 @@ GPS_LED ?= none
SKYTRAQ_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(SKYTRAQ_GPS_PORT_LOWER)
ap.CFLAGS += -DSKYTRAQ_GPS_LINK=$(SKYTRAQ_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -12,13 +12,13 @@ ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS
sw/airborne/subsystems/gps/gps_skytraq.c
+62 -38
View File
@@ -20,6 +20,7 @@
*/
#include "subsystems/gps.h"
#include "subsystems/gps/gps_skytraq.h"
#include "subsystems/abi.h"
#include "led.h"
@@ -85,7 +86,7 @@ static inline uint16_t bswap16(uint16_t a)
static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos);
void gps_impl_init(void)
void skytraq_gps_impl_init(void)
{
gps_skytraq.status = UNINIT;
@@ -96,85 +97,96 @@ void gps_skytraq_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
gps_skytraq.state.last_msg_ticks = sys_time.nb_sec_rem;
gps_skytraq.state.last_msg_time = sys_time.nb_sec;
gps_skytraq_read_message();
if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
if (gps_skytraq.state.fix == GPS_FIX_3D) {
gps_skytraq.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_skytraq.state.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SKYTRAQ_ID, now_ts, &gps);
}
gps_skytraq.msg_available = FALSE;
}
void skytraq_gps_event(void)
{
struct link_device *dev = &((SKYTRAQ_GPS_LINK).device);
while (dev->char_available(dev->periph)) {
gps_skytraq_parse(dev->get_byte(dev->periph));
if (gps_skytraq.msg_available) {
gps_skytraq_msg();
}
}
}
void gps_skytraq_read_message(void)
{
if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf);
gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf);
gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps_skytraq.state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf);
gps_skytraq.state.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf);
gps_skytraq.state.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf);
SetBit(gps_skytraq.state.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf);
gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf);
gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_skytraq.state.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf);
gps_skytraq.state.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf);
gps_skytraq.state.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf);
SetBit(gps_skytraq.state.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf);
gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf);
gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps_skytraq.state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf);
gps_skytraq.state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf);
gps_skytraq.state.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10;
SetBit(gps_skytraq.state.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps_skytraq.state.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10;
SetBit(gps_skytraq.state.valid_fields, GPS_VALID_HMSL_BIT);
// pacc;
// sacc;
gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf);
gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf);
gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf) * 10;
gps_skytraq.state.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf);
gps_skytraq.state.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf);
gps_skytraq.state.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf) * 10;
switch (SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf)) {
case SKYTRAQ_FIX_3D_DGPS:
case SKYTRAQ_FIX_3D:
gps.fix = GPS_FIX_3D;
gps_skytraq.state.fix = GPS_FIX_3D;
break;
case SKYTRAQ_FIX_2D:
gps.fix = GPS_FIX_2D;
gps_skytraq.state.fix = GPS_FIX_2D;
break;
default:
gps.fix = GPS_FIX_NONE;
gps_skytraq.state.fix = GPS_FIX_NONE;
}
if (gps.fix == GPS_FIX_3D) {
if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) {
if (gps_skytraq.state.fix == GPS_FIX_3D) {
if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps_skytraq.state.ecef_pos)) {
// just grab current ecef_pos as reference.
ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps.ecef_pos);
ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps_skytraq.state.ecef_pos);
}
// convert ecef velocity vector to NED vector.
ned_of_ecef_vect_i(&gps.ned_vel, &gps_skytraq.ref_ltp, &gps.ecef_vel);
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
ned_of_ecef_vect_i(&gps_skytraq.state.ned_vel, &gps_skytraq.ref_ltp, &gps_skytraq.state.ecef_vel);
SetBit(gps_skytraq.state.valid_fields, GPS_VALID_VEL_NED_BIT);
// ground course in radians
gps.course = (atan2f((float)gps.ned_vel.y, (float)gps.ned_vel.x)) * 1e7;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
// GT: gps.cacc = ... ? what should course accuracy be?
gps_skytraq.state.course = (atan2f((float)gps_skytraq.state.ned_vel.y, (float)gps_skytraq.state.ned_vel.x)) * 1e7;
SetBit(gps_skytraq.state.valid_fields, GPS_VALID_COURSE_BIT);
// GT: gps_skytraq.state.cacc = ... ? what should course accuracy be?
// ground speed
gps.gspeed = sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y);
gps.speed_3d = sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y + gps.ned_vel.z * gps.ned_vel.z);
gps_skytraq.state.gspeed = sqrt(gps_skytraq.state.ned_vel.x * gps_skytraq.state.ned_vel.x + gps_skytraq.state.ned_vel.y * gps_skytraq.state.ned_vel.y);
gps_skytraq.state.speed_3d = sqrt(gps_skytraq.state.ned_vel.x * gps_skytraq.state.ned_vel.x + gps_skytraq.state.ned_vel.y * gps_skytraq.state.ned_vel.y + gps_skytraq.state.ned_vel.z * gps_skytraq.state.ned_vel.z);
// vertical speed (climb)
// solved by gps.ned.z?
// solved by gps_skytraq.state.ned.z?
}
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
if (gps_skytraq.state.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
} else {
LED_TOGGLE(GPS_LED);
@@ -273,3 +285,15 @@ static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ec
return FALSE;
}
/*
* register callbacks & structs
*/
void skytraq_gps_register(void)
{
#ifdef GPS_SECONDARY_PIKSI
gps_register_impl(skytraq_gps_impl_init, skytraq_gps_event, GPS_SKYTRAQ_ID, 1);
#else
gps_register_impl(skytraq_gps_impl_init, skytraq_gps_event, GPS_SKYTRAQ_ID, 0);
#endif
}
sw/airborne/subsystems/gps/gps_skytraq.h
+21 -12
View File
@@ -26,6 +26,22 @@
#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8
#if GPS_SECONDARY_SKYTRAQ
#ifndef SKYTRAQ_GPS_LINK
#define SKYTRAQ_GPS_LINK GPS_SECONDARY_PORT
#define SecondaryGpsImpl skytraq
#endif
#else
#ifndef PrimaryGpsImpl
#define PrimaryGpsImpl skytraq
#endif
#endif
#if GPS_PRIMARY_SKYTRAQ
#ifndef SKYTRAQ_GPS_LINK
#define SKYTRAQ_GPS_LINK GPS_PRIMARY_PORT
#endif
#endif
/* last error type */
enum GpsSkytraqError {
GPS_SKYTRAQ_ERR_NONE = 0,
@@ -50,6 +66,8 @@ struct GpsSkytraq {
enum GpsSkytraqError error_last;
struct LtpDef_i ref_ltp;
struct GpsState state;
};
extern struct GpsSkytraq gps_skytraq;
@@ -63,17 +81,8 @@ extern struct GpsSkytraq gps_skytraq;
extern void gps_skytraq_read_message(void);
extern void gps_skytraq_parse(uint8_t c);
extern void gps_skytraq_msg(void);
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
while (dev->char_available(dev->periph)) {
gps_skytraq_parse(dev->get_byte(dev->periph));
if (gps_skytraq.msg_available) {
gps_skytraq_msg();
}
}
}
extern void skytraq_gps_register(void);
void skytraq_gps_impl_init(void);
void skytraq_gps_event(void);
#endif /* GPS_SKYTRAQ_H */