diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h index 431030eb31..f2b04185b2 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -178,7 +178,7 @@ typedef struct _navdata_gps_t { uint8_t unk_2[16]; struct{ uint8_t sat; - uint8_t unk; + uint8_t cn0; }channels[12]; int32_t gps_plugged; /*!< When the gps is plugged */ uint8_t unk_3[108]; diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c index 60a1e5db78..0326f0dd64 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.c +++ b/sw/airborne/subsystems/gps/gps_ardrone2.c @@ -36,6 +36,7 @@ void gps_impl_init( void ) { } void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { + int i; // Set the lla double struct from the navdata struct LlaCoor_d gps_lla_d; gps_lla_d.lat = RadOfDeg(navdata_gps->lat); @@ -51,6 +52,12 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d); // TODO: parse other stuff + gps.nb_channels = GPS_NB_CHANNELS; + + for(i = 0; i < GPS_NB_CHANNELS; i++) { + gps.svinfos[i].svid = navdata_gps->channels[i].sat; + gps.svinfos[i].cno = navdata_gps->channels[i].cn0; + } // Check if we have a fix TODO: check if 2D or 3D fix? if (navdata_gps->gps_state == 1) diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h index 42e926ff34..2995d0f853 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.h +++ b/sw/airborne/subsystems/gps/gps_ardrone2.h @@ -30,7 +30,7 @@ #include "boards/ardrone/at_com.h" -//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet +#define GPS_NB_CHANNELS 12 extern bool_t gps_ardrone2_available; /*