diff --git a/conf/airframes/antenna.xml b/conf/airframes/antenna.xml index 6bca1a3c9c..137fba8401 100644 --- a/conf/airframes/antenna.xml +++ b/conf/airframes/antenna.xml @@ -1,5 +1,15 @@ + + +
+
+ ARCHI=avr @@ -36,6 +46,7 @@ ant.srcs += avr/ant_tracker.c ant.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1 ant.srcs += datalink.c +ant.srcs += traffic_info.c #ant.CFLAGS += ant.srcs += avr/ant_v2x.c avr/ant_spi.c diff --git a/sw/airborne/avr/ant_tracker.c b/sw/airborne/avr/ant_tracker.c index bbae2b7da7..28020368ae 100644 --- a/sw/airborne/avr/ant_tracker.c +++ b/sw/airborne/avr/ant_tracker.c @@ -10,11 +10,23 @@ int32_t nav_utm_east0; int32_t nav_utm_north0; uint8_t nav_utm_zone0; +void ant_tracker_init( void ) { + // nav_utm_east0 = ; + // nav_utm_north0 = ; + // nav_utm_zone0 = ; + ant_track_id = 12; + ant_track_mode = ANT_TRACK_AUTO; + ant_track_azim = 0.; + ant_track_elev = 0.; +} -void ant_tracker_update( void ) { +void ant_tracker_periodic( void ) { if (ant_track_mode == ANT_TRACK_AUTO) { ant_track_azim += 0.5; if (ant_track_azim > 360.) ant_track_azim = 0.; + ant_track_elev += 0.1; + if (ant_track_elev > 90.) + ant_track_elev = 0.; } } diff --git a/sw/airborne/avr/ant_tracker.h b/sw/airborne/avr/ant_tracker.h index 4005570482..3ec10aa022 100644 --- a/sw/airborne/avr/ant_tracker.h +++ b/sw/airborne/avr/ant_tracker.h @@ -18,12 +18,12 @@ extern uint8_t ant_track_id; { \ ant_track_mode = i; \ if(ant_track_mode) \ - LED_ON(1); \ + LED_ON(2); \ else \ - LED_OFF(1); \ + LED_OFF(2); \ } -extern void ant_tracker_update( void ); - +extern void ant_tracker_init( void ); +extern void ant_tracker_periodic( void ); #endif /* ANT_TRACKER_H */ diff --git a/sw/airborne/main_antenna.c b/sw/airborne/main_antenna.c index 5709ce1456..bef7aabd29 100644 --- a/sw/airborne/main_antenna.c +++ b/sw/airborne/main_antenna.c @@ -21,12 +21,13 @@ static inline void main_init( void ); static inline void main_periodic_task( void ); static inline void main_event_task( void); +static inline void main_dl_parse_msg( void ); uint8_t track_mode; int main( void ) { main_init(); - LED_ON(1); + LED_OFF(1); LED_OFF(2); while (1) { if (sys_time_periodic()) @@ -43,6 +44,7 @@ static inline void main_init( void ) { uart1_init_tx(); uart1_init_rx(); adc_init(); + ant_tracker_init(); int_enable(); } @@ -59,7 +61,6 @@ static inline void main_event_task( void ) { } if (PprzBuffer()) { - LED_TOGGLE(1); ReadPprzBuffer(); if (pprz_msg_received) { pprz_parse_payload(); @@ -67,23 +68,54 @@ static inline void main_event_task( void ) { } } if (dl_msg_available) { - dl_parse_msg(); + main_dl_parse_msg(); dl_msg_available = FALSE; } - } static inline void main_periodic_task( void ) { static uint8_t cnt; cnt++; if (!(cnt%16)) { - LED_TOGGLE(2); + // LED_TOGGLE(2); DOWNLINK_SEND_ANTENNA_STATUS(&ant_track_azim, &ant_track_elev, &ant_track_id, &ant_track_mode); } if (!(cnt%4)) { - ant_tracker_update(); + ant_tracker_periodic(); } } +#include "settings.h" +#include "traffic_info.h" +#define NAV_UTM_EAST0 360285 +#define NAV_UTM_NORTH0 4813595 +#define NAV_UTM_ZONE0 31 +#define IdOfMsg(x) (x[1]) +#define MOfCm(_x) (((float)_x)/100.) +static inline void main_dl_parse_msg(void) { + uint8_t msg_id = IdOfMsg(dl_buffer); + if (msg_id == DL_SETTING) { + uint8_t i = DL_SETTING_index(dl_buffer); + float var = DL_SETTING_value(dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(&i, &var); + } + else if (msg_id == DL_ACINFO) { + LED_TOGGLE(1); + uint8_t id = DL_ACINFO_ac_id(dl_buffer); + float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); + float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); + float a = MOfCm(DL_ACINFO_alt(dl_buffer)); + float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); + float s = MOfCm(DL_ACINFO_speed(dl_buffer)); + SetAcInfo(id, ux, uy, c, a, s); + } + else if (msg_id == DL_WIND_INFO) { + + } + else { + // LED_TOGGLE(1); + } +} diff --git a/sw/airborne/traffic_info.h b/sw/airborne/traffic_info.h index 45c4fc5ec2..35f36a5ced 100644 --- a/sw/airborne/traffic_info.h +++ b/sw/airborne/traffic_info.h @@ -29,7 +29,7 @@ #ifndef TI_H #define TI_H -#define NB_ACS 8 +#define NB_ACS 24 struct ac_info_ { float east; /* m */