diff --git a/conf/airframes/antenna.xml b/conf/airframes/antenna.xml index 137fba8401..3a6afa2d45 100644 --- a/conf/airframes/antenna.xml +++ b/conf/airframes/antenna.xml @@ -1,14 +1,10 @@ - -
-
+ diff --git a/sw/airborne/avr/ant_tracker.c b/sw/airborne/avr/ant_tracker.c index 28020368ae..3da636adf7 100644 --- a/sw/airborne/avr/ant_tracker.c +++ b/sw/airborne/avr/ant_tracker.c @@ -1,5 +1,6 @@ #include "ant_tracker.h" +#include "traffic_info.h" uint8_t ant_track_mode; float ant_track_azim; @@ -9,12 +10,13 @@ uint8_t ant_track_id; int32_t nav_utm_east0; int32_t nav_utm_north0; uint8_t nav_utm_zone0; +const float ant_track_gnd_alt = 185.; void ant_tracker_init( void ) { // nav_utm_east0 = ; // nav_utm_north0 = ; // nav_utm_zone0 = ; - ant_track_id = 12; + ant_track_id = 5; ant_track_mode = ANT_TRACK_AUTO; ant_track_azim = 0.; ant_track_elev = 0.; @@ -22,11 +24,21 @@ void ant_tracker_init( void ) { void ant_tracker_periodic( void ) { if (ant_track_mode == ANT_TRACK_AUTO) { +#if 0 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.; +#endif + struct ac_info_ * ac = get_ac_info(ant_track_id); + ant_track_azim = atan2(ac->north, ac->east) * 180. / M_PI; + ant_track_azim = 90. - ant_track_azim; + if (ant_track_azim < 0) + ant_track_azim += 360.; + float dist = sqrt(ac->north*ac->north + ac->east*ac->east); + float height = ac->alt - ant_track_elev; + ant_track_elev = atan2( height, dist) * 180. / M_PI; } } diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index 062ae99a64..dc3fdc6708 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -55,6 +55,7 @@ let airborne_device = fun device addr -> match device with "WAVECARD" -> WavecardDevice (W.addr_of_string addr) | "XBEE" -> XBeeDevice + | "PPRZ" -> Uart | _ -> failwith (sprintf "Link: unknown datalink: %s" device) let get_define = fun xml name -> @@ -116,6 +117,7 @@ let airframes = let device = get_define dls "DEVICE_TYPE" and addr = get_define dls "DEVICE_ADDRESS" in let dl = airborne_device device addr in + printf "%s %b\n%!" (ExtXml.attrib a "ac_id") (dl = Uart); (ios (ExtXml.attrib a "ac_id"), dl)::r with Not_found -> r @@ -135,7 +137,7 @@ exception NotSendingToThis let airborne_device = fun ac_id airframes device -> let ac_device = try Some (List.assoc ac_id airframes) with Not_found -> None in match ac_device, device with - None, Pprz -> Uart + (None, Pprz) | (Some Uart, Pprz) -> Uart | (Some (WavecardDevice _ as ac_device), Wavecard) | (Some (XBeeDevice as ac_device), XBee) -> ac_device @@ -611,7 +613,7 @@ let _ = if !uplink then begin (** Listening on Ivy (FIXME: remove the ad hoc messages) *) -(*** ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp device)); ***) + ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp device)); ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" (move_wp device)); ignore (Ground_Pprz.message_bind "DL_SETTING" (setting device)); ignore (Ground_Pprz.message_bind "JUMP_TO_BLOCK" (jump_block device));