Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[sirf] modified for ABI approach

Browse Source
This commit is contained in:
masierra
2016-02-01 16:05:24 -08:00
parent 052c4660cf
commit 2a2a1a2c3c
3 changed files with 72 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile
+2 -2
View File
@@ -6,7 +6,7 @@ GPS_LED ?= none
SIRF_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(SIRF_GPS_PORT_LOWER)
ap.CFLAGS += -DSIRF_GPS_LINK=$(SIRF_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -14,7 +14,7 @@ ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sirf.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sw/airborne/subsystems/gps/gps_sirf.c
+48 -36
View File
@@ -36,7 +36,7 @@ struct GpsSirf gps_sirf;
void sirf_parse_2(void);
void sirf_parse_41(void);
void gps_impl_init(void)
void sirf_gps_impl_init(void)
{
gps_sirf.msg_available = FALSE;
gps_sirf.pos_available = FALSE;
@@ -48,13 +48,13 @@ void gps_sirf_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_sirf.state.last_msg_ticks = sys_time.nb_sec_rem;
gps_sirf.state.last_msg_time = sys_time.nb_sec;
sirf_parse_msg();
if (gps_sirf.pos_available) {
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_sirf.state.fix == GPS_FIX_3D) {
gps_sirf.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_sirf.state.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SIRF_ID, now_ts, &gps);
}
@@ -115,32 +115,32 @@ void sirf_parse_41(void)
{
struct sirf_msg_41 *p = (struct sirf_msg_41 *)&gps_sirf.msg_buf[4];
gps.tow = Invert4Bytes(p->tow);
gps.hmsl = Invert4Bytes(p->alt_msl) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.num_sv = p->num_sat;
gps.nb_channels = p ->num_sat;
gps_sirf.state.tow = Invert4Bytes(p->tow);
gps_sirf.state.hmsl = Invert4Bytes(p->alt_msl) * 10;
SetBit(gps_sirf.state.valid_fields, GPS_VALID_HMSL_BIT);
gps_sirf.state.num_sv = p->num_sat;
gps_sirf.state.nb_channels = p ->num_sat;
/* read latitude, longitude and altitude from packet */
gps.lla_pos.lat = Invert4Bytes(p->latitude);
gps.lla_pos.lon = Invert4Bytes(p->longitude);
gps.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps_sirf.state.lla_pos.lat = Invert4Bytes(p->latitude);
gps_sirf.state.lla_pos.lon = Invert4Bytes(p->longitude);
gps_sirf.state.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10;
SetBit(gps_sirf.state.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.sacc = (Invert2Bytes(p->ehve) >> 16);
gps.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5);
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5);
gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5);
gps.pacc = Invert4Bytes(p->ehpe);
gps.pdop = p->hdop * 20;
gps_sirf.state.sacc = (Invert2Bytes(p->ehve) >> 16);
gps_sirf.state.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5);
SetBit(gps_sirf.state.valid_fields, GPS_VALID_COURSE_BIT);
gps_sirf.state.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5);
gps_sirf.state.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5);
gps_sirf.state.pacc = Invert4Bytes(p->ehpe);
gps_sirf.state.pdop = p->hdop * 20;
if ((p->nav_type >> 8 & 0x7) >= 0x4) {
gps.fix = GPS_FIX_3D;
gps_sirf.state.fix = GPS_FIX_3D;
} else if ((p->nav_type >> 8 & 0x7) >= 0x1) {
gps.fix = GPS_FIX_2D;
gps_sirf.state.fix = GPS_FIX_2D;
} else {
gps.fix = GPS_FIX_NONE;
gps_sirf.state.fix = GPS_FIX_NONE;
}
@@ -152,24 +152,24 @@ void sirf_parse_2(void)
{
struct sirf_msg_2 *p = (struct sirf_msg_2 *)&gps_sirf.msg_buf[4];
gps.week = Invert2Bytes(p->week);
gps_sirf.state.week = Invert2Bytes(p->week);
gps.ecef_pos.x = Invert4Bytes(p->x_pos) * 100;
gps.ecef_pos.y = Invert4Bytes(p->y_pos) * 100;
gps.ecef_pos.z = Invert4Bytes(p->z_pos) * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps_sirf.state.ecef_pos.x = Invert4Bytes(p->x_pos) * 100;
gps_sirf.state.ecef_pos.y = Invert4Bytes(p->y_pos) * 100;
gps_sirf.state.ecef_pos.z = Invert4Bytes(p->z_pos) * 100;
SetBit(gps_sirf.state.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8;
gps.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8;
gps.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8;
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_sirf.state.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8;
gps_sirf.state.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8;
gps_sirf.state.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8;
SetBit(gps_sirf.state.valid_fields, GPS_VALID_VEL_ECEF_BIT);
if (gps.fix == GPS_FIX_3D) {
if (gps_sirf.state.fix == GPS_FIX_3D) {
ticks++;
#if DEBUG_SIRF
printf("GPS %i %i %i %i\n", ticks, (sys_time.nb_sec - start_time), ticks2, (sys_time.nb_sec - start_time2));
#endif
} else if (sys_time.nb_sec - gps.last_3dfix_time > 10) {
} else if (sys_time.nb_sec - gps_sirf.state.last_3dfix_time > 10) {
start_time = sys_time.nb_sec;
ticks = 0;
}
@@ -201,3 +201,15 @@ void sirf_parse_msg(void)
}
}
/*
* register callbacks & structs
*/
void sirf_gps_register(void)
{
#ifdef GPS_SECONDARY_SIRF
gps_register_impl(sirf_gps_impl_init, sirf_gps_event, GPS_SIRF_ID, 1);
#else
gps_register_impl(sirf_gps_impl_init, sirf_gps_event, GPS_SIRF_ID, 0);
#endif
}
sw/airborne/subsystems/gps/gps_sirf.h
+22 -3
View File
@@ -31,7 +31,23 @@
#include "std.h"
#define GPS_NB_CHANNELS 16
#if GPS_SECONDARY_SIRF
#ifndef SIRF_GPS_LINK
#define SIRF_GPS_LINK GPS_SECONDARY_PORT
#define SecondaryGpsImpl sirf
#endif
#else
#ifndef PrimaryGpsImpl
#define PrimaryGpsImpl sirf
#endif
#endif
#if GPS_PRIMARY_SIRF
#ifndef SIRF_GPS_LINK
#define SIRF_GPS_LINK GPS_PRIMARY_PORT
#endif
#endif
#define SIRF_GPS_NB_CHANNELS 16
#define SIRF_MAXLEN 255
//Read states
@@ -46,6 +62,7 @@ struct GpsSirf {
char msg_buf[SIRF_MAXLEN]; ///< buffer for storing one nmea-line
int msg_len;
int read_state;
struct GpsState state;
};
extern struct GpsSirf gps_sirf;
@@ -133,10 +150,12 @@ struct sirf_msg_41 {
extern void sirf_parse_char(uint8_t c);
extern void sirf_parse_msg(void);
extern void gps_sirf_msg(void);
void sirf_gps_impl_init(void);
void sirf_gps_register(void);
static inline void GpsEvent(void)
static inline void sirf_gps_event(void)
{
struct link_device *dev = &((GPS_LINK).device);
struct link_device *dev = &((SIRF_GPS_LINK).device);
while (dev->char_available(dev->periph)) {
sirf_parse_char(dev->get_byte(dev->periph));