diff --git a/sw/airborne/booz/gps/booz_gps_skytraq.c b/sw/airborne/booz/gps/booz_gps_skytraq.c index 97051c0e3e..6fa2f1ccc8 100644 --- a/sw/airborne/booz/gps/booz_gps_skytraq.c +++ b/sw/airborne/booz/gps/booz_gps_skytraq.c @@ -43,25 +43,25 @@ void booz_gps_impl_init(void) { booz_gps_skytraq.status = UNINIT; - + DEBUG_SERVO1_INIT(); } void booz_gps_skytraq_read_message(void) { - + DEBUG_S1_ON(); if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { - booz_gps_state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(booz_gps_skytraq.msg_buf); - booz_gps_state.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(booz_gps_skytraq.msg_buf); - booz_gps_state.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(booz_gps_skytraq.msg_buf); - booz_gps_state.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(booz_gps_skytraq.msg_buf); - booz_gps_state.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(booz_gps_skytraq.msg_buf); - booz_gps_state.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(booz_gps_skytraq.msg_buf); - booz_gps_state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(booz_gps_skytraq.msg_buf); - booz_gps_state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(booz_gps_skytraq.msg_buf); + booz_gps_state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(booz_gps_skytraq.msg_buf); + booz_gps_state.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(booz_gps_skytraq.msg_buf); + booz_gps_state.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(booz_gps_skytraq.msg_buf); + booz_gps_state.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(booz_gps_skytraq.msg_buf); + booz_gps_state.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(booz_gps_skytraq.msg_buf); + booz_gps_state.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(booz_gps_skytraq.msg_buf); + booz_gps_state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(booz_gps_skytraq.msg_buf); + booz_gps_state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(booz_gps_skytraq.msg_buf); booz_gps_state.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(booz_gps_skytraq.msg_buf); booz_gps_state.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(booz_gps_skytraq.msg_buf); // pacc; @@ -81,7 +81,7 @@ void booz_gps_skytraq_read_message(void) { } #endif } - + DEBUG_S1_OFF(); } @@ -110,7 +110,7 @@ void booz_gps_skytraq_parse(uint8_t c) { if (booz_gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) { booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG; goto error; - } + } break; case GOT_LEN2: booz_gps_skytraq.msg_id = c; @@ -153,4 +153,3 @@ void booz_gps_skytraq_parse(uint8_t c) { booz_gps_skytraq.status = UNINIT; return; } - diff --git a/sw/airborne/booz/gps/booz_gps_skytraq.h b/sw/airborne/booz/gps/booz_gps_skytraq.h index 0d9794208e..f3fe7703f3 100644 --- a/sw/airborne/booz/gps/booz_gps_skytraq.h +++ b/sw/airborne/booz/gps/booz_gps_skytraq.h @@ -13,7 +13,7 @@ #define SKYTRAQ_SYNC4 0x0A -#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8 +#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8 #define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2)) #define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2)) @@ -74,7 +74,7 @@ extern void booz_gps_skytraq_parse(uint8_t c); if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) \ booz_gps_state.lost_counter = 0; \ - _sol_available_callback(); \ + _sol_available_callback(); \ } \ booz_gps_skytraq.msg_available = FALSE; \ } \ @@ -85,4 +85,3 @@ extern void booz_gps_skytraq_parse(uint8_t c); booz_gps_skytraq_parse(GpsLink(Getch())); \ } #endif /* BOOZ_GPS_SKYTRAQ_H */ - diff --git a/sw/airborne/booz/gps/booz_gps_ubx.c b/sw/airborne/booz/gps/booz_gps_ubx.c index 6511525bd4..b8f26e1142 100644 --- a/sw/airborne/booz/gps/booz_gps_ubx.c +++ b/sw/airborne/booz/gps/booz_gps_ubx.c @@ -56,7 +56,7 @@ void booz_gps_impl_init(void) { void booz_gps_ubx_read_message(void) { - + if (booz_gps_ubx.msg_class == UBX_NAV_ID) { if (booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) { booz_gps_state.fix = UBX_NAV_SOL_GPSfix(booz_gps_ubx.msg_buf); diff --git a/sw/airborne/booz/gps/booz_gps_ubx.h b/sw/airborne/booz/gps/booz_gps_ubx.h index d5b77c6940..28d306a0e7 100644 --- a/sw/airborne/booz/gps/booz_gps_ubx.h +++ b/sw/airborne/booz/gps/booz_gps_ubx.h @@ -28,10 +28,10 @@ extern void booz_gps_ubx_parse(uint8_t c); if (booz_gps_ubx.msg_available) { \ booz_gps_ubx_read_message(); \ if (booz_gps_ubx.msg_class == UBX_NAV_ID && \ - booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ + booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) \ booz_gps_state.lost_counter = 0; \ - _sol_available_callback(); \ + _sol_available_callback(); \ } \ booz_gps_ubx.msg_available = FALSE; \ } \