Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[refactor] Converted traffic_info from subsystem to module with upgraded interface

Browse Source
This commit is contained in:
kirkscheper
2016-06-17 08:25:39 +02:00
parent 19a0ad30ca
commit 4f8fd9c7d3
31 changed files with 1366 additions and 676 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/autopilot.makefile
-2
View File
@@ -47,8 +47,6 @@ else
$(TARGET).CFLAGS += -DWIND_INFO $(TARGET).CFLAGS += -DWIND_INFO
endif endif
$(TARGET).CFLAGS += -DTRAFFIC_INFO
# #
# frequencies # frequencies
# #
conf/firmwares/subsystems/fixedwing/navigation.makefile
+1 -3
View File
@@ -7,8 +7,6 @@
$(TARGET).CFLAGS += -DNAV $(TARGET).CFLAGS += -DNAV
$(TARGET).srcs += $(SRC_FIRMWARE)/nav.c $(TARGET).srcs += $(SRC_FIRMWARE)/nav.c
#$(TARGET).srcs += subsystems/navigation/common_nav.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c
conf/firmwares/subsystems/rotorcraft/navigation.makefile
+1 -1
View File
@@ -2,6 +2,6 @@
$(TARGET).CFLAGS += -DUSE_NAVIGATION $(TARGET).CFLAGS += -DUSE_NAVIGATION
$(TARGET).srcs += $(SRC_FIRMWARE)/navigation.c $(TARGET).srcs += $(SRC_FIRMWARE)/navigation.c
#$(TARGET).srcs += subsystems/navigation/common_nav.c
$(TARGET).srcs += subsystems/navigation/waypoints.c $(TARGET).srcs += subsystems/navigation/waypoints.c
$(TARGET).srcs += subsystems/navigation/common_flight_plan.c $(TARGET).srcs += subsystems/navigation/common_flight_plan.c
$(TARGET).srcs += subsystems/navigation/traffic_info.c
conf/modules/follow.xml
+3 -5
View File
@@ -3,7 +3,7 @@
<module name="follow" dir="multi"> <module name="follow" dir="multi">
<doc> <doc>
<description> <description>
Follow a certain AC_ID trough remote GPS. Follow a certain AC_ID using traffic_info.
Only for rotorcraft firmware. Only for rotorcraft firmware.
</description> </description>
<define name="FOLLOW_AC_ID" description="the aircraft ID which it has to follow"/> <define name="FOLLOW_AC_ID" description="the aircraft ID which it has to follow"/>
@@ -12,14 +12,12 @@
<define name="FOLLOW_OFFSET_Y" value="0" description="the y offset in ENU (meters) from the plane"/> <define name="FOLLOW_OFFSET_Y" value="0" description="the y offset in ENU (meters) from the plane"/>
<define name="FOLLOW_OFFSET_Z" value="0" description="the z offset in ENU (meters) from the plane"/> <define name="FOLLOW_OFFSET_Z" value="0" description="the z offset in ENU (meters) from the plane"/>
</doc> </doc>
<depends>traffic_info</depends>
<header> <header>
<file name="follow.h"/> <file name="follow.h"/>
</header> </header>
<init fun="follow_init()"/> <init fun="follow_init()"/>
<datalink message="REMOTE_GPS" fun="ParseRemoteGps()"/> <periodic fun="follow_wp()" freq="5" autorun="FALSE"/>
<makefile> <makefile>
<file name="follow.c"/> <file name="follow.c"/>
</makefile> </makefile>
conf/modules/formation_flight.xml
+3 -2
View File
@@ -16,12 +16,13 @@
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
<depends>traffic_info</depends>
<header> <header>
<file name="formation.h"/> <file name="formation.h"/>
</header> </header>
<init fun="formation_init()"/> <init fun="formation_init()"/>
<datalink message="FORMATION_STATUS" fun="ParseFormationStatus()"/> <datalink message="FORMATION_STATUS" fun="parseFormationStatus()"/>
<datalink message="FORMATION_SLOT" fun="ParseFormationSlot()"/> <datalink message="FORMATION_SLOT" fun="parseFormationSlot()"/>
<makefile> <makefile>
<file name="formation.c"/> <file name="formation.c"/>
</makefile> </makefile>
conf/modules/tcas.xml
+3 -1
View File
@@ -4,13 +4,15 @@
<doc> <doc>
<description>TCAS collision avoidance</description> <description>TCAS collision avoidance</description>
</doc> </doc>
<depends>traffic_info</depends>
<header> <header>
<file name="tcas.h"/> <file name="tcas.h"/>
</header> </header>
<init fun="tcas_init()"/> <init fun="tcas_init()"/>
<periodic fun="tcas_periodic_task_1Hz()" freq="1"/> <periodic fun="tcas_periodic_task_1Hz()" freq="1"/>
<periodic fun="tcas_periodic_task_4Hz()" freq="4"/> <periodic fun="tcas_periodic_task_4Hz()" freq="4"/>
<datalink message="TCAS_RESOLVE" fun="ParseTcasResolve()"/> <datalink message="TCAS_RESOLVE" fun="parseTcasResolve()"/>
<datalink message="TCAS_RA" fun="parseTcasRA()"/>
<makefile> <makefile>
<file name="tcas.c"/> <file name="tcas.c"/>
<define name="TCAS"/> <define name="TCAS"/>
conf/modules/traffic_info.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="traffic_info" dir="multi">
<doc>
<description>Keeps track of other aircraft in airspace</description>
</doc>
<header>
<file name="traffic_info.h"/>
</header>
<init fun="traffic_info_init()"/>
<datalink message="ACINFO" fun="parse_acinfo_dl()"/>
<datalink message="ACINFO_LLA" fun="parse_acinfo_dl()"/>
<datalink message="GPS_SMALL" fun="parse_acinfo_dl()"/>
<datalink message="GPS" fun="parse_acinfo_dl()"/>
<datalink message="GPS_LLA" fun="parse_acinfo_dl()"/>
<makefile>
<file name="traffic_info.c"/>
<define name="TRAFFIC_INFO"/>
</makefile>
</module>
sw/airborne/arch/sim/sim_ap.c
-1
View File
@@ -13,7 +13,6 @@
#include "inter_mcu.h" #include "inter_mcu.h"
#include "autopilot.h" #include "autopilot.h"
#include "subsystems/gps.h" #include "subsystems/gps.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/settings.h" #include "generated/settings.h"
#include "firmwares/fixedwing/nav.h" #include "firmwares/fixedwing/nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
sw/airborne/firmwares/fixedwing/main_ap.c
+2 -11
View File
@@ -67,9 +67,6 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#include CTRL_TYPE_H #include CTRL_TYPE_H
#include "firmwares/fixedwing/nav.h" #include "firmwares/fixedwing/nav.h"
#include "generated/flight_plan.h" #include "generated/flight_plan.h"
#ifdef TRAFFIC_INFO
#include "subsystems/navigation/traffic_info.h"
#endif
// datalink & telemetry // datalink & telemetry
#if DATALINK || SITL #if DATALINK || SITL
@@ -255,12 +252,6 @@ void init_ap(void)
IO0SET = _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif #endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
/* set initial trim values. /* set initial trim values.
* these are passed to fbw via inter_mcu. * these are passed to fbw via inter_mcu.
*/ */
@@ -395,7 +386,7 @@ static inline void telecommand_task(void)
/* really_lost is true if we lost RC in MANUAL or AUTO1 */ /* really_lost is true if we lost RC in MANUAL or AUTO1 */
uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) &&
(pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL); (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) { if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) { if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
@@ -532,7 +523,7 @@ void navigation_task(void)
} }
#ifdef TCAS #ifdef TCAS
CallTCAS(); callTCAS();
#endif #endif
#if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode) #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
sw/airborne/firmwares/fixedwing/nav.c
+14 -9
View File
@@ -35,7 +35,6 @@ static unit_t unit __attribute__((unused));
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/autopilot.h" #include "firmwares/fixedwing/autopilot.h"
#include "inter_mcu.h" #include "inter_mcu.h"
#include "subsystems/navigation/traffic_info.h"
#include "subsystems/gps.h" #include "subsystems/gps.h"
#include "generated/flight_plan.h" #include "generated/flight_plan.h"
@@ -284,24 +283,30 @@ static inline bool compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float gli
#define LINE_STOP_FUNCTION {} #define LINE_STOP_FUNCTION {}
#endif #endif
#ifdef TRAFFIC_INFO
#include "modules/multi/traffic_info.h"
void nav_follow(uint8_t ac_id, float distance, float height)
void nav_follow(uint8_t _ac_id, float _distance, float _height)
{ {
struct ac_info_ * ac = get_ac_info(_ac_id); struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id);
NavVerticalAutoThrottleMode(0.); NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt + SECURITY_HEIGHT), 0.); NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.);
float alpha = M_PI / 2 - ac->course; float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
float ca = cosf(alpha), sa = sinf(alpha); float ca = cosf(alpha), sa = sinf(alpha);
float x = ac->east - _distance * ca; float x = ac->x - distance * ca;
float y = ac->north - _distance * sa; float y = ac->y - distance * sa;
fly_to_xy(x, y); fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN #ifdef NAV_FOLLOW_PGAIN
float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa; float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN * s; nav_ground_speed_setpoint = acInfoGetGspeed(ac_id) + NAV_FOLLOW_PGAIN * s;
nav_ground_speed_loop(); nav_ground_speed_loop();
#endif #endif
} }
#else
void nav_follow(uint8_t __attribute__((unused)) _ac_id, float __attribute__((unused)) distance,
float __attribute__((unused)) height) {}
#endif // TRAFFIC_INFO
float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT; float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
float desired_x, desired_y; float desired_x, desired_y;
sw/airborne/firmwares/rotorcraft/navigation.c
+26 -4
View File
@@ -116,7 +116,8 @@ static inline void nav_set_altitude(void);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
void set_exception_flag(uint8_t flag_num) { void set_exception_flag(uint8_t flag_num)
{
exception_flag[flag_num] = 1; exception_flag[flag_num] = 1;
} }
@@ -446,9 +447,9 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
INT32_COURSE_NORMALIZE(nav_heading); INT32_COURSE_NORMALIZE(nav_heading);
RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp,
&(waypoints[wp].enu_i.x), &(waypoints[wp].enu_i.x),
&(waypoints[wp].enu_i.y), &(waypoints[wp].enu_i.y),
&(waypoints[wp].enu_i.z))); &(waypoints[wp].enu_i.z)));
} }
bool nav_detect_ground(void) bool nav_detect_ground(void)
@@ -675,3 +676,24 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius)
return; return;
} }
} }
#ifdef TRAFFIC_INFO
#include "modules/multi/traffic_info.h"
void nav_follow(uint8_t ac_id, uint32_t distance, uint32_t height)
{
/* struct EnuCoor_i* target = acInfoGetPositionEnu_i(ac_id);
float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
float ca = cosf(alpha), sa = sinf(alpha);
target->x += - distance * ca;
target->y += - distance * sa;
target->z = (Max(target->z + height, SECURITY_HEIGHT)); // todo add ground height to check
ENU_OF_TO_NED(navigation_target, *target);*/
}
#else
void nav_follow(uint8_t __attribute__((unused)) _ac_id, uint32_t __attribute__((unused)) distance,
uint32_t __attribute__((unused)) height) {}
#endif
sw/airborne/firmwares/rotorcraft/navigation.h
+24 -20
View File
@@ -1,23 +1,23 @@
/* /*
* Copyright (C) 2008-2011 The Paparazzi Team * Copyright (C) 2008-2011 The Paparazzi Team
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
* paparazzi is free software; you can redistribute it and/or modify * paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option) * the Free Software Foundation; either version 2, or (at your option)
* any later version. * any later version.
* *
* paparazzi is distributed in the hope that it will be useful, * paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
/** /**
* @file firmwares/rotorcraft/navigation.h * @file firmwares/rotorcraft/navigation.h
@@ -242,4 +242,8 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp); extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp);
#define NavFollow(_ac_id, _distance, _height) \
nav_follow(_ac_id, _distance, _height);
extern void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height);
#endif /* NAVIGATION_H */ #endif /* NAVIGATION_H */
sw/airborne/math/pprz_geodetic.h
+4 -5
View File
@@ -70,26 +70,25 @@ extern "C" {
(_u1).zone = (_u2).zone; \ (_u1).zone = (_u2).zone; \
} }
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2) { \
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2) { \
(_pos).x = (_utm1).east - (_utm2).east; \ (_pos).x = (_utm1).east - (_utm2).east; \
(_pos).y = (_utm1).north - (_utm2).north; \ (_pos).y = (_utm1).north - (_utm2).north; \
(_pos).z = (_utm1).alt - (_utm2).alt; \ (_pos).z = (_utm1).alt - (_utm2).alt; \
} }
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2) { \ #define NED_OF_UTM_DIFF(_pos, _utm1, _utm2) { \
(_pos).x = (_utm1).north - (_utm2).north; \ (_pos).x = (_utm1).north - (_utm2).north; \
(_pos).y = (_utm1).east - (_utm2).east; \ (_pos).y = (_utm1).east - (_utm2).east; \
(_pos).z = -(_utm1).alt + (_utm2).alt; \ (_pos).z = -(_utm1).alt + (_utm2).alt; \
} }
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0) { \ #define UTM_OF_ENU_ADD(_utm, _pos, _utm0) { \
(_utm).east = (_utm0).east + (_pos).x; \ (_utm).east = (_utm0).east + (_pos).x; \
(_utm).north = (_utm0).north + (_pos).y; \ (_utm).north = (_utm0).north + (_pos).y; \
(_utm).alt = (_utm0).alt + (_pos).z; \ (_utm).alt = (_utm0).alt + (_pos).z; \
} }
#define UTM_OF_NED_ADD(_utm, _pos, _utm0) { \ #define UTM_OF_NED_ADD(_utm, _pos, _utm0) { \
(_utm).east = (_utm0).east + (_pos).y; \ (_utm).east = (_utm0).east + (_pos).y; \
(_utm).north = (_utm0).north + (_pos).x; \ (_utm).north = (_utm0).north + (_pos).x; \
(_utm).alt = (_utm0).alt - (_pos).z; \ (_utm).alt = (_utm0).alt - (_pos).z; \
sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
+9 -10
View File
@@ -24,7 +24,6 @@
#include "autopilot.h" #include "autopilot.h"
#include "generated/flight_plan.h" #include "generated/flight_plan.h"
#include "state.h" #include "state.h"
#include "subsystems/navigation/traffic_info.h"
#include "airborne_ant_track.h" #include "airborne_ant_track.h"
@@ -116,23 +115,23 @@ void airborne_ant_point_periodic(void)
/* /*
* This is for one axis pan antenna mechanisms. The default is to * This is for one axis pan antenna mechanisms. The default is to
* circle clockwise so view is right. The pan servo neutral makes * circle clockwise so view is right. The pan servo neutral makes
* the antenna look to the right with 0° given, 90° is to the back and * the antenna look to the right with 0˚ given, 90˚ is to the back and
* -90° is to the front. * -90˚ is to the front.
* *
* *
* *
* plane front * plane front
* *
* 90 * 90˚
^ ^
* I * I
* 135 I 45° * 135˚ I 45˚
* \ I / * \ I /
* \I/ * \I/
* 180-------I------- 0° * 180˚-------I------- 0˚
* /I\ * /I\
* / I \ * / I \
* -135 I -45° * -135˚ I -45˚
* I * I
* -90 * -90
* plane back * plane back
@@ -140,9 +139,9 @@ void airborne_ant_point_periodic(void)
* *
*/ */
/* fPan = 0 -> antenna looks along the wing /* fPan = 0˚ -> antenna looks along the wing
90 -> antenna looks in flight direction 90˚ -> antenna looks in flight direction
-90 -> antenna looks backwards -90˚ -> antenna looks backwards
*/ */
/* fixed to the plane*/ /* fixed to the plane*/
airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy))); airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
sw/airborne/modules/cam_control/cam.c
+10 -7
View File
@@ -30,7 +30,6 @@
#include "autopilot.h" #include "autopilot.h"
#include "generated/flight_plan.h" #include "generated/flight_plan.h"
#include "state.h" #include "state.h"
#include "subsystems/navigation/traffic_info.h"
#ifdef POINT_CAM #ifdef POINT_CAM
#include "point.h" #include "point.h"
#endif // POINT_CAM #endif // POINT_CAM
@@ -303,13 +302,17 @@ void cam_waypoint_target(void)
cam_target(); cam_target();
} }
#ifdef TRAFFIC_INFO
#include "modules/multi/traffic_info.h"
void cam_ac_target(void) void cam_ac_target(void)
{ {
#ifdef TRAFFIC_INFO struct EnuCoor_f ac_pos *ac = acInfoGetPositionEnu_f(cam_target_ac);
struct ac_info_ * ac = get_ac_info(cam_target_ac); cam_target_x = ac->x;
cam_target_x = ac->east; cam_target_y = ac->y;
cam_target_y = ac->north; cam_target_alt = acInfoGetPositionUtm_f()->alt;
cam_target_alt = ac->alt;
cam_target(); cam_target();
#endif // TRAFFIC_INFO
} }
#else
void cam_ac_target(void) {}
#endif // TRAFFIC_INFO
sw/airborne/modules/multi/follow.c
+22 -25
View File
@@ -32,9 +32,8 @@
#include "subsystems/navigation/waypoints.h" #include "subsystems/navigation/waypoints.h"
#include "state.h" #include "state.h"
#include "pprzlink/messages.h"
#include "pprzlink/dl_protocol.h"
/* FOLLOW_OFFSET_ X Y and Z are all in ENU frame */
#ifndef FOLLOW_OFFSET_X #ifndef FOLLOW_OFFSET_X
#define FOLLOW_OFFSET_X 0.0 #define FOLLOW_OFFSET_X 0.0
#endif #endif
@@ -47,31 +46,29 @@
#define FOLLOW_OFFSET_Z 0.0 #define FOLLOW_OFFSET_Z 0.0
#endif #endif
void follow_init(void) #ifndef FOLLOW_AC_ID
#error "Please define FOLLOW_AC_ID"
#endif
#ifndef FOLLOW_WAYPOINT_ID
#error "Please define FOLLOW_WAYPOINT_ID"
#endif
void follow_init(void) {}
/*
* follow_wp(void)
* updates the FOLLOW_WAYPOINT_ID to a fixed offset from the last received location
* of other aircraft with id FOLLOW_AC_ID
*/
void follow_wp(void)
{ {
struct EnuCoor_i *ac = acInfoGetPositionEnu_i(FOLLOW_AC_ID);
} struct EnuCoor_i enu = *stateGetPositionEnu_i();
enu.x += ac->x + POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
void follow_change_wp(unsigned char *buffer) enu.y += ac->y + POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
{ enu.z += ac->z + POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
struct EcefCoor_i new_pos;
struct EnuCoor_i enu;
new_pos.x = DL_REMOTE_GPS_ecef_x(buffer);
new_pos.y = DL_REMOTE_GPS_ecef_y(buffer);
new_pos.z = DL_REMOTE_GPS_ecef_z(buffer);
// Translate to ENU
enu_of_ecef_point_i(&enu, &state.ned_origin_i, &new_pos);
INT32_VECT3_SCALE_2(enu, enu, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
// TODO: Add the angle to the north
// Update the offsets
enu.x += POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
enu.y += POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
enu.z += POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
// TODO: Remove the angle to the north
// Move the waypoint // Move the waypoint
waypoint_set_enu_i(FOLLOW_WAYPOINT_ID, &enu); waypoint_set_enu_i(FOLLOW_WAYPOINT_ID, &enu);
sw/airborne/modules/multi/follow.h
+1 -7
View File
@@ -28,12 +28,6 @@
#define FOLLOW_H #define FOLLOW_H
extern void follow_init(void); extern void follow_init(void);
extern void follow_change_wp(unsigned char *buffer); extern void follow_wp(void);
#define ParseRemoteGps() { \
if (DL_REMOTE_GPS_ac_id(dl_buffer) == FOLLOW_AC_ID) { \
follow_change_wp(dl_buffer); \
} \
}
#endif // FOLLOW #endif // FOLLOW
sw/airborne/modules/multi/formation.c
+49 -56
View File
@@ -4,21 +4,17 @@
#define FORMATION_C #define FORMATION_C
#include <math.h> #include "multi/formation.h"
#include "std.h"
#include "state.h"
#include "subsystems/datalink/downlink.h" #include "subsystems/datalink/downlink.h"
#include "multi/formation.h" #include "firmwares/fixedwing/nav.h"
#include "state.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/guidance/guidance_v.h"
#include "autopilot.h"
#include "subsystems/gps.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
#include "pprzlink/dl_protocol.h"
#include <stdio.h> #include "generated/flight_plan.h" // SECURITY_ALT
float form_n, form_e, form_a; float form_n, form_e, form_a;
float form_speed, form_speed_n, form_speed_e; float form_speed, form_speed_n, form_speed_e;
@@ -77,18 +73,18 @@ int formation_init(void)
form_prox = FORM_PROX; form_prox = FORM_PROX;
form_mode = FORM_MODE; form_mode = FORM_MODE;
old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
old_alt = GROUND_ALT + SECURITY_HEIGHT; old_alt = SECURITY_ALT;
return false; return false;
} }
int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a)
{ {
if (_id != AC_ID && the_acs_id[_id] == 0) { return false; } // no info for this AC if (_id != AC_ID && ti_acs_id[_id] == 0) { return false; } // no info for this AC
DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &_id, &form_mode, &slot_e, &slot_n, &slot_a); DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &_id, &form_mode, &slot_e, &slot_n, &slot_a);
formation[the_acs_id[_id]].status = IDLE; formation[ti_acs_id[_id]].status = IDLE;
formation[the_acs_id[_id]].east = slot_e; formation[ti_acs_id[_id]].east = slot_e;
formation[the_acs_id[_id]].north = slot_n; formation[ti_acs_id[_id]].north = slot_n;
formation[the_acs_id[_id]].alt = slot_a; formation[ti_acs_id[_id]].alt = slot_a;
return false; return false;
} }
@@ -120,15 +116,14 @@ int stop_formation(void)
v_ctl_auto_throttle_cruise_throttle = old_cruise; v_ctl_auto_throttle_cruise_throttle = old_cruise;
old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
nav_altitude = old_alt; nav_altitude = old_alt;
old_alt = GROUND_ALT + SECURITY_HEIGHT; old_alt = SECURITY_ALT;
return false; return false;
} }
int formation_flight(void) int formation_flight(void)
{ {
static uint8_t _1Hz = 0;
static uint8_t _1Hz = 0;
uint8_t nb = 0, i; uint8_t nb = 0, i;
float hspeed_dir = stateGetHorizontalSpeedDir_f(); float hspeed_dir = stateGetHorizontalSpeedDir_f();
float ch = cosf(hspeed_dir); float ch = cosf(hspeed_dir);
@@ -141,30 +136,27 @@ int formation_flight(void)
form_speed_e = form_speed * sh; form_speed_e = form_speed * sh;
if (AC_ID == leader_id) { if (AC_ID == leader_id) {
stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east; stateGetPositionEnu_f()->x += formation[ti_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; stateGetPositionEnu_f()->y += formation[ti_acs_id[AC_ID]].north;
} }
// set info for this AC
set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir,
stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow);
// broadcast info // broadcast info
uint8_t ac_id = AC_ID; uint8_t ac_id = AC_ID;
uint8_t status = formation[the_acs_id[AC_ID]].status; uint8_t status = formation[ti_acs_id[AC_ID]].status;
DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status); DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
if (++_1Hz >= 4) { if (++_1Hz >= 4) {
_1Hz = 0; _1Hz = 0;
DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode, DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
&formation[the_acs_id[AC_ID]].east, &formation[ti_acs_id[AC_ID]].east,
&formation[the_acs_id[AC_ID]].north, &formation[ti_acs_id[AC_ID]].north,
&formation[the_acs_id[AC_ID]].alt); &formation[ti_acs_id[AC_ID]].alt);
} }
if (formation[the_acs_id[AC_ID]].status != ACTIVE) { return false; } // AC not ready if (formation[ti_acs_id[AC_ID]].status != ACTIVE) { return false; } // AC not ready
// get leader info // get leader info
struct ac_info_ * leader = get_ac_info(leader_id); struct EnuCoor_f *leader_pos = acInfoGetPositionEnu_f(leader_id);
if (formation[the_acs_id[leader_id]].status == UNSET || if (formation[ti_acs_id[leader_id]].status == UNSET ||
formation[the_acs_id[leader_id]].status == IDLE) { formation[ti_acs_id[leader_id]].status == IDLE) {
// leader not ready or not in formation // leader not ready or not in formation
return false; return false;
} }
@@ -173,8 +165,8 @@ int formation_flight(void)
struct slot_ form[NB_ACS]; struct slot_ form[NB_ACS];
float cr = 0., sr = 1.; float cr = 0., sr = 1.;
if (form_mode == FORM_MODE_COURSE) { if (form_mode == FORM_MODE_COURSE) {
cr = cosf(leader->course); cr = cosf(acInfoGetCourse(leader_id));
sr = sinf(leader->course); sr = sinf(acInfoGetCourse(leader_id));
} }
for (i = 0; i < NB_ACS; ++i) { for (i = 0; i < NB_ACS; ++i) {
if (formation[i].status == UNSET) { continue; } if (formation[i].status == UNSET) { continue; }
@@ -183,25 +175,26 @@ int formation_flight(void)
form[i].alt = formation[i].alt; form[i].alt = formation[i].alt;
} }
struct EnuCoor_f *my_pos = stateGetPositionEnu_f();
// compute control forces // compute control forces
for (i = 0; i < NB_ACS; ++i) { for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; } if (ti_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ti_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); struct EnuCoor_f *ac_speed = acInfoGetVelocityEnu_f(ti_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - acInfoGetItow(ti_acs[i].ac_id)) / 1000., 0.);
if (delta_t > FORM_CARROT) { if (delta_t > FORM_CARROT) {
// if AC not responding for too long // if AC not responding for too long
formation[i].status = LOST; formation[i].status = LOST;
continue; continue;
} else { } else {
// compute control if AC is ACTIVE and around the same altitude (maybe not so usefull) // compute control if AC is ACTIVE and around the same altitude (maybe not so useful)
formation[i].status = ACTIVE; formation[i].status = ACTIVE;
if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) { if (ac->z > 0 && fabs(my_pos->z - ac->z) < form_prox) {
form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x) form_e += (ac->x + ac_speed->x * delta_t - my_pos->x) - (form[i].east - form[ti_acs_id[AC_ID]].east);
- (form[i].east - form[the_acs_id[AC_ID]].east); form_n += (ac->y + ac_speed->y * delta_t - my_pos->y) - (form[i].north - form[ti_acs_id[AC_ID]].north);
form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y) form_a += (ac->z + ac_speed->z * delta_t - my_pos->z) - (form[i].alt - form[ti_acs_id[AC_ID]].alt);
- (form[i].north - form[the_acs_id[AC_ID]].north); form_speed += acInfoGetGspeed(ti_acs[i].ac_id);
form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
form_speed += ac->gspeed;
//form_speed_e += ac->gspeed * sinf(ac->course); //form_speed_e += ac->gspeed * sinf(ac->course);
//form_speed_n += ac->gspeed * cosf(ac->course); //form_speed_n += ac->gspeed * cosf(ac->course);
++nb; ++nb;
@@ -224,21 +217,21 @@ int formation_flight(void)
if (AC_ID == leader_id) { if (AC_ID == leader_id) {
alt = nav_altitude; alt = nav_altitude;
} else { } else {
alt = leader->alt - form[the_acs_id[leader_id]].alt; alt = leader_pos->z - form[ti_acs_id[leader_id]].alt;
} }
alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a; alt += formation[ti_acs_id[AC_ID]].alt + coef_form_alt * form_a;
flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT); flight_altitude = Max(alt, SECURITY_ALT);
// carrot // carrot
if (AC_ID != leader_id) { if (AC_ID != leader_id) {
float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east; float dx = form[ti_acs_id[AC_ID]].east - form[ti_acs_id[leader_id]].east;
float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north; float dy = form[ti_acs_id[AC_ID]].north - form[ti_acs_id[leader_id]].north;
desired_x = leader->east + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx; desired_x = leader_pos->x + NOMINAL_AIRSPEED * form_carrot * sinf(acInfoGetCourse(leader_id)) + dx;
desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy; desired_y = leader_pos->y + NOMINAL_AIRSPEED * form_carrot * cosf(acInfoGetCourse(leader_id)) + dy;
// fly to desired // fly to desired
fly_to_xy(desired_x, desired_y); fly_to_xy(desired_x, desired_y);
desired_x = leader->east + dx; desired_x = leader_pos->x + dx;
desired_y = leader->north + dy; desired_y = leader_pos->y + dy;
// lateral correction // lateral correction
//float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy)); //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
//float diff_course = leader->course - hspeed_dir; //float diff_course = leader->course - hspeed_dir;
@@ -261,8 +254,8 @@ int formation_flight(void)
void formation_pre_call(void) void formation_pre_call(void)
{ {
if (leader_id == AC_ID) { if (leader_id == AC_ID) {
stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east; stateGetPositionEnu_f()->x -= formation[ti_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north; stateGetPositionEnu_f()->y -= formation[ti_acs_id[AC_ID]].north;
} }
} }
sw/airborne/modules/multi/formation.h
+30 -29
View File
@@ -7,8 +7,9 @@
#ifndef FORMATION_H #ifndef FORMATION_H
#define FORMATION_H #define FORMATION_H
#include "firmwares/fixedwing/nav.h" #include "subsystems/datalink/datalink.h" // dl_buffer
#include "subsystems/navigation/traffic_info.h" #include "generated/airframe.h" // AC_ID
#include "modules/multi/traffic_info.h"
#define FORM_MODE_GLOBAL 0 #define FORM_MODE_GLOBAL 0
#define FORM_MODE_COURSE 1 #define FORM_MODE_COURSE 1
@@ -27,45 +28,45 @@ struct slot_ {
float alt; float alt;
}; };
extern struct slot_ formation[NB_ACS]; extern struct slot_ formation[];
extern int formation_init(void); extern int formation_init(void);
extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a); extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
#define UpdateSlot(_id, _se, _sn, _sa) { \ static inline void updateSlot(uint8_t id, float se, float sn, float sa)
formation[the_acs_id[_id]].east = _se; \ {
formation[the_acs_id[_id]].north = _sn; \ formation[ti_acs_id[id]].east = se;
formation[the_acs_id[_id]].alt = _sa; \ formation[ti_acs_id[id]].north = sn;
} formation[ti_acs_id[id]].alt = sa;
}
#define UpdateFormationStatus(_id,_status) { formation[the_acs_id[_id]].status = _status; } static inline void updateFormationStatus(uint8_t id, uint8_t status) { formation[ti_acs_id[id]].status = status; }
#define ParseFormationStatus() { \ static inline void parseFormationStatus(void)
uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer); \ {
uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer); \ uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer);
uint8_t status = DL_FORMATION_STATUS_status(dl_buffer); \ uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer);
if (ac_id == AC_ID) leader_id = leader; \ uint8_t status = DL_FORMATION_STATUS_status(dl_buffer);
else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); } \ if (ac_id == AC_ID) { leader_id = leader; }
else { UpdateFormationStatus(ac_id,UNSET); } \ else if (leader == leader_id) { updateFormationStatus(ac_id, status); }
} else { updateFormationStatus(ac_id, UNSET); }
}
#define ParseFormationSlot() { \ static inline void parseFormationSlot(void)
uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer); \ {
uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer); \ uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer);
float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer); \ uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer);
float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer); \ float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer);
float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer); \ float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer);
UpdateSlot(ac_id, slot_east, slot_north, slot_alt); \ float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer);
if (ac_id == leader_id) form_mode = mode; \ updateSlot(ac_id, slot_east, slot_north, slot_alt);
} if (ac_id == leader_id) { form_mode = mode; }
}
extern int start_formation(void); extern int start_formation(void);
extern int stop_formation(void); extern int stop_formation(void);
extern int formation_flight(void); extern int formation_flight(void);
extern void formation_pre_call(void); extern void formation_pre_call(void);
#endif /* FORMATION_H */ #endif /* FORMATION_H */
sw/airborne/modules/multi/potential.c
+15 -15
View File
@@ -46,6 +46,8 @@ void potential_init(void)
potential_force.east = 0.; potential_force.east = 0.;
potential_force.north = 0.; potential_force.north = 0.;
potential_force.alt = 0.; potential_force.alt = 0.;
potential_force.speed = 0.;
potential_force.climb = 0.;
force_pos_gain = FORCE_POS_GAIN; force_pos_gain = FORCE_POS_GAIN;
force_speed_gain = FORCE_SPEED_GAIN; force_speed_gain = FORCE_SPEED_GAIN;
@@ -67,25 +69,24 @@ int potential_task(void)
// compute control forces // compute control forces
int8_t nb = 0; int8_t nb = 0;
for (i = 0; i < NB_ACS; ++i) { for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; } if (ti_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ti_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); struct EnuCoor_f *ac_speed = acInfoGetVelocityEnu_f(ti_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - acInfoGetItow(ti_acs[i].ac_id)) / 1000., 0.);
// if AC not responding for too long, continue, else compute force // if AC not responding for too long, continue, else compute force
if (delta_t > CARROT) { continue; } if (delta_t > CARROT) { continue; }
else { else {
float sha = sinf(ac->course); float de = ac->x + ac_speed->x * delta_t - stateGetPositionEnu_f()->x;
float cha = cosf(ac->course);
float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x;
if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; } if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; }
float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y; float dn = ac->y + ac_speed->y * delta_t - stateGetPositionEnu_f()->y;
if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; } if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; }
float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt; float da = ac->z + ac_speed->z * delta_t - stateGetPositionEnu_f()->z;
if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; } if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; }
float dist = sqrtf(de * de + dn * dn + da * da); float dist = sqrtf(de * de + dn * dn + da * da);
if (dist == 0.) { continue; } if (dist == 0.) { continue; }
float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha; float dve = stateGetHorizontalSpeedNorm_f() * sh - ac_speed->x;
float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha; float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac_speed->y;
float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb; float dva = stateGetSpeedEnu_f()->z - ac_speed->z;
float scal = dve * de + dvn * dn + dva * da; float scal = dve * de + dvn * dn + dva * da;
if (scal < 0.) { continue; } // No risk of collision if (scal < 0.) { continue; } // No risk of collision
float d3 = dist * dist * dist; float d3 = dist * dist * dist;
@@ -96,9 +97,9 @@ int potential_task(void)
} }
} }
if (nb == 0) { return true; } if (nb == 0) { return true; }
//potential_force.east /= nb; potential_force.east /= nb;
//potential_force.north /= nb; potential_force.north /= nb;
//potential_force.alt /= nb; potential_force.alt /= nb;
// set commands // set commands
NavVerticalAutoThrottleMode(0.); NavVerticalAutoThrottleMode(0.);
@@ -128,4 +129,3 @@ int potential_task(void)
return true; return true;
} }
sw/airborne/modules/multi/potential.h
+1 -1
View File
@@ -8,7 +8,7 @@
#define POTENTIAL_H #define POTENTIAL_H
#include "firmwares/fixedwing/nav.h" #include "firmwares/fixedwing/nav.h"
#include "subsystems/navigation/traffic_info.h" #include "modules/multi/traffic_info.h"
struct force_ { struct force_ {
float east; float east;
sw/airborne/modules/multi/tcas.c
+54 -35
View File
@@ -26,13 +26,10 @@
*/ */
#include "multi/tcas.h" #include "multi/tcas.h"
#include "generated/airframe.h"
#include "state.h" #include "state.h"
#include "firmwares/fixedwing/nav.h" #include "firmwares/fixedwing/nav.h"
#include "subsystems/gps.h" #include "generated/flight_plan.h" // SECURITY_ALT
#include "generated/flight_plan.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h" #include "subsystems/datalink/downlink.h"
float tcas_alt_setpoint; float tcas_alt_setpoint;
@@ -65,12 +62,14 @@ struct tcas_ac_status tcas_acs_status[NB_ACS];
#define TCAS_HUGE_TAU 100*TCAS_TAU_TA #define TCAS_HUGE_TAU 100*TCAS_TAU_TA
void callTCAS(void) { if (tcas_status == TCAS_RA) { v_ctl_altitude_setpoint = tcas_alt_setpoint; } }
/* AC is inside the horizontol dmod area and twice the vertical alim separation */ /* AC is inside the horizontol dmod area and twice the vertical alim separation */
#define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 ) #define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 )
void tcas_init(void) void tcas_init(void)
{ {
tcas_alt_setpoint = GROUND_ALT + SECURITY_HEIGHT; tcas_alt_setpoint = SECURITY_ALT;
tcas_tau_ta = TCAS_TAU_TA; tcas_tau_ta = TCAS_TAU_TA;
tcas_tau_ra = TCAS_TAU_RA; tcas_tau_ra = TCAS_TAU_RA;
tcas_dmod = TCAS_DMOD; tcas_dmod = TCAS_DMOD;
@@ -85,10 +84,26 @@ void tcas_init(void)
} }
} }
void parseTcasResolve(void)
{
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) {
uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer);
tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(dl_buffer);
}
}
void parseTcasRA(void)
{
if (DL_TCAS_RA_ac_id(dl_buffer) == AC_ID && SenderIdOfPprzMsg(dl_buffer) != AC_ID) {
uint8_t ac_id_conflict = SenderIdOfPprzMsg(dl_buffer);
tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
}
}
static inline enum tcas_resolve tcas_test_direction(uint8_t id) static inline enum tcas_resolve tcas_test_direction(uint8_t id)
{ {
struct ac_info_ * ac = get_ac_info(id); struct EnuCoor_f *ac = acInfoGetPositionEnu_f(id);
float dz = ac->alt - stateGetPositionUtm_f()->alt; float dz = ac->z - stateGetPositionEnu_f()->z;
if (dz > tcas_alim / 2) { return RA_DESCEND; } if (dz > tcas_alim / 2) { return RA_DESCEND; }
else if (dz < -tcas_alim / 2) { return RA_CLIMB; } else if (dz < -tcas_alim / 2) { return RA_CLIMB; }
else { // AC with the smallest ID descend else { // AC with the smallest ID descend
@@ -102,7 +117,7 @@ static inline enum tcas_resolve tcas_test_direction(uint8_t id)
void tcas_periodic_task_1Hz(void) void tcas_periodic_task_1Hz(void)
{ {
// no TCAS under security_height // no TCAS under security_height
if (stateGetPositionUtm_f()->alt < GROUND_ALT + SECURITY_HEIGHT) { if (stateGetPositionUtm_f()->alt < SECURITY_ALT) {
uint8_t i; uint8_t i;
for (i = 0; i < NB_ACS; i++) { tcas_acs_status[i].status = TCAS_NO_ALARM; } for (i = 0; i < NB_ACS; i++) { tcas_acs_status[i].status = TCAS_NO_ALARM; }
return; return;
@@ -114,19 +129,19 @@ void tcas_periodic_task_1Hz(void)
float vx = stateGetHorizontalSpeedNorm_f() * sinf(stateGetHorizontalSpeedDir_f()); float vx = stateGetHorizontalSpeedNorm_f() * sinf(stateGetHorizontalSpeedDir_f());
float vy = stateGetHorizontalSpeedNorm_f() * cosf(stateGetHorizontalSpeedDir_f()); float vy = stateGetHorizontalSpeedNorm_f() * cosf(stateGetHorizontalSpeedDir_f());
for (i = 2; i < NB_ACS; i++) { for (i = 2; i < NB_ACS; i++) {
if (the_acs[i].ac_id == 0) { continue; } // no AC data if (ti_acs[i].ac_id == 0) { continue; } // no AC data
uint32_t dt = gps.tow - the_acs[i].itow; uint32_t dt = gps.tow - ti_acs[i].itow;
if (dt > 3 * TCAS_DT_MAX) { if (dt > 3 * TCAS_DT_MAX) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
continue; continue;
} }
if (dt > TCAS_DT_MAX) { continue; } // lost com but keep current status if (dt > TCAS_DT_MAX) { continue; } // lost com but keep current status
float dx = the_acs[i].east - stateGetPositionEnu_f()->x; float dx = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->x - stateGetPositionEnu_f()->x;
float dy = the_acs[i].north - stateGetPositionEnu_f()->y; float dy = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->y - stateGetPositionEnu_f()->y;
float dz = the_acs[i].alt - stateGetPositionUtm_f()->alt; float dz = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->z - stateGetPositionEnu_f()->z;
float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course); float dvx = vx - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->x;
float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course); float dvy = vy - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->y;
float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb; float dvz = stateGetSpeedEnu_f()->z - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->z;
float scal = dvx * dx + dvy * dy + dvz * dz; float scal = dvx * dx + dvy * dy + dvz * dz;
float ddh = dx * dx + dy * dy; float ddh = dx * dx + dy * dy;
float ddv = dz * dz; float ddv = dz * dz;
@@ -140,60 +155,62 @@ void tcas_periodic_task_1Hz(void)
if (tau >= TCAS_HUGE_TAU && !inside) { if (tau >= TCAS_HUGE_TAU && !inside) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
tcas_acs_status[i].resolve = RA_NONE; tcas_acs_status[i].resolve = RA_NONE;
DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(the_acs[i].ac_id)); DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(ti_acs[i].ac_id));
} }
break; break;
case TCAS_TA: case TCAS_TA:
if (tau < tcas_tau_ra || inside) { if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // TA -> RA tcas_acs_status[i].status = TCAS_RA; // TA -> RA
// Downlink alert // Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id); //test_dir = tcas_test_direction(ti_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ??? //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(ti_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
break; break;
} }
if (tau > tcas_tau_ta && !inside) { if (tau > tcas_tau_ta && !inside) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
} }
tcas_acs_status[i].resolve = RA_NONE; tcas_acs_status[i].resolve = RA_NONE;
DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(the_acs[i].ac_id)); DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(ti_acs[i].ac_id));
break; break;
case TCAS_NO_ALARM: case TCAS_NO_ALARM:
if (tau < tcas_tau_ta || inside) { if (tau < tcas_tau_ta || inside) {
tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
// Downlink warning // Downlink warning
DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice, &(the_acs[i].ac_id)); DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice, &(ti_acs[i].ac_id));
} }
if (tau < tcas_tau_ra || inside) { if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ? tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
// Downlink alert // Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id); //test_dir = tcas_test_direction(ti_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir); //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(ti_acs[i].ac_id),&test_dir);
} }
break; break;
default:
break;
} }
// store closest AC // store closest AC
if (tau < tau_min) { if (tau < tau_min) {
tau_min = tau; tau_min = tau;
ac_id_close = the_acs[i].ac_id; ac_id_close = ti_acs[i].ac_id;
} }
} }
// set current conflict mode // set current conflict mode
if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && tcas_acs_status[the_acs_id[tcas_ac_RA]].status == TCAS_RA) { if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && tcas_acs_status[ti_acs_id[tcas_ac_RA]].status == TCAS_RA) {
ac_id_close = tcas_ac_RA; // keep RA until resolved ac_id_close = tcas_ac_RA; // keep RA until resolved
} }
tcas_status = tcas_acs_status[the_acs_id[ac_id_close]].status; tcas_status = tcas_acs_status[ti_acs_id[ac_id_close]].status;
// at least one in conflict, deal with closest one // at least one in conflict, deal with closest one
if (tcas_status == TCAS_RA) { if (tcas_status == TCAS_RA) {
tcas_ac_RA = ac_id_close; tcas_ac_RA = ac_id_close;
tcas_resolve = tcas_test_direction(tcas_ac_RA); tcas_resolve = tcas_test_direction(tcas_ac_RA);
uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve; uint8_t ac_resolve = tcas_acs_status[ti_acs_id[tcas_ac_RA]].resolve;
if (ac_resolve != RA_NONE) { // first resolution, no message received if (ac_resolve != RA_NONE) { // first resolution, no message received
if (ac_resolve == tcas_resolve) { // same direction, lowest id go down if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; } if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; }
else { tcas_resolve = RA_CLIMB; } else { tcas_resolve = RA_CLIMB; }
} }
tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now tcas_acs_status[ti_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
} else { // second resolution or message received } else { // second resolution or message received
if (ac_resolve != RA_LEVEL) { // message received if (ac_resolve != RA_LEVEL) { // message received
if (ac_resolve == tcas_resolve) { // same direction, lowest id go down if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
@@ -201,8 +218,8 @@ void tcas_periodic_task_1Hz(void)
else { tcas_resolve = RA_CLIMB; } else { tcas_resolve = RA_CLIMB; }
} }
} else { // no message } else { // no message
if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) { tcas_resolve = RA_DESCEND; } // revert resolve if (tcas_resolve == RA_CLIMB && ti_acs[ti_acs_id[tcas_ac_RA]].climb > 1.0) { tcas_resolve = RA_DESCEND; } // revert resolve
else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) { tcas_resolve = RA_CLIMB; } // revert resolve else if (tcas_resolve == RA_DESCEND && ti_acs[ti_acs_id[tcas_ac_RA]].climb < -1.0) { tcas_resolve = RA_CLIMB; } // revert resolve
} }
} }
// Downlink alert // Downlink alert
@@ -219,22 +236,24 @@ void tcas_periodic_task_1Hz(void)
void tcas_periodic_task_4Hz(void) void tcas_periodic_task_4Hz(void)
{ {
// set alt setpoint // set alt setpoint
if (stateGetPositionUtm_f()->alt > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) { if (stateGetPositionUtm_f()->alt > SECURITY_ALT && tcas_status == TCAS_RA) {
struct ac_info_ * ac = get_ac_info(tcas_ac_RA); struct EnuCoor_f *ac = acInfoGetPositionEnu_f(tcas_ac_RA);
switch (tcas_resolve) { switch (tcas_resolve) {
case RA_CLIMB : case RA_CLIMB :
tcas_alt_setpoint = Max(nav_altitude, ac->alt + tcas_alim); tcas_alt_setpoint = Max(nav_altitude, ac->z + tcas_alim);
break; break;
case RA_DESCEND : case RA_DESCEND :
tcas_alt_setpoint = Min(nav_altitude, ac->alt - tcas_alim); tcas_alt_setpoint = Min(nav_altitude, ac->z - tcas_alim);
break; break;
case RA_LEVEL : case RA_LEVEL :
case RA_NONE : case RA_NONE :
tcas_alt_setpoint = nav_altitude; tcas_alt_setpoint = nav_altitude;
break; break;
default:
break;
} }
// Bound alt // Bound alt
tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint); tcas_alt_setpoint = Max(SECURITY_ALT, tcas_alt_setpoint);
} else { } else {
tcas_alt_setpoint = nav_altitude; tcas_alt_setpoint = nav_altitude;
tcas_resolve = RA_NONE; tcas_resolve = RA_NONE;
sw/airborne/modules/multi/tcas.h
+8 -9
View File
@@ -29,7 +29,10 @@
#define TCAS_H #define TCAS_H
#include "std.h" #include "std.h"
#include "subsystems/navigation/traffic_info.h" #include "subsystems/datalink/datalink.h" // dl_buffer
#include "pprzlink/messages.h" // TCAS_RA
#include "generated/airframe.h" // AC_INFO
#include "modules/multi/traffic_info.h"
extern float tcas_alt_setpoint; extern float tcas_alt_setpoint;
extern float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim; extern float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim;
@@ -54,13 +57,9 @@ extern void tcas_init(void);
extern void tcas_periodic_task_1Hz(void); extern void tcas_periodic_task_1Hz(void);
extern void tcas_periodic_task_4Hz(void); extern void tcas_periodic_task_4Hz(void);
#define CallTCAS() { if (tcas_status == TCAS_RA) v_ctl_altitude_setpoint = tcas_alt_setpoint; } extern void callTCAS(void);
#define ParseTcasResolve() { \ extern void parseTcasResolve(void);
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) { \ extern void parseTcasRA(void);
uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer); \
tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(dl_buffer); \
} \
}
#endif // TCAS #endif /* TCAS_H */
sw/airborne/modules/multi/traffic_info.c
+447
View File
@@ -0,0 +1,447 @@
/*
* Copyright (C) Pascal Brisset, Antoine Drouin (2008), Kirk Scheper (2016)
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/traffic_info/traffic_info.c"
* @author Kirk Scheper
* Information relative to the other aircrafts.
* Keeps track of other aircraft in airspace
*/
#include "modules/multi/traffic_info.h"
#include "generated/airframe.h" // AC_ID
#include "generated/flight_plan.h" // NAV_MSL0
#include "subsystems/datalink/datalink.h"
#include "pprzlink/dl_protocol.h" // datalink messages
#include "pprzlink/messages.h" // telemetry messages
#include "state.h"
#include "math/pprz_geodetic_utm.h"
#include "math/pprz_geodetic_wgs84.h"
/* number of ac being tracked */
uint8_t ti_acs_idx;
/* index of ac in the list of traffic info aircraft (ti_acs) */
uint8_t ti_acs_id[NB_ACS_ID];
/* container for available traffic info */
struct acInfo ti_acs[NB_ACS];
/* Geoid height (msl) over ellipsoid [mm] */
int32_t geoid_height;
void traffic_info_init(void)
{
memset(ti_acs_id, 0, NB_ACS_ID);
ti_acs_id[0] = 0; // ground station
ti_acs_id[AC_ID] = 1;
ti_acs[ti_acs_id[AC_ID]].ac_id = AC_ID;
ti_acs_idx = 2;
geoid_height = NAV_MSL0;
}
/**
* Update estimate of the geoid height
* Requires an available hsml and/or lla measurement, if not available value isn't updated
*/
static void update_geoid_height(void) {
if(bit_is_set(gps.valid_fields, GPS_VALID_HMSL_BIT) && bit_is_set(gps.valid_fields, GPS_VALID_POS_LLA_BIT))
{
geoid_height = gps.lla_pos.alt - gps.hmsl;
} else if(bit_is_set(gps.valid_fields, GPS_VALID_POS_LLA_BIT))
{
geoid_height = wgs84_ellipsoid_to_geoid_i(gps.lla_pos.lat, gps.lla_pos.lon);
} /* default is just keep last available height */
}
bool parse_acinfo_dl(void)
{
uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
uint8_t msg_id = IdOfPprzMsg(dl_buffer);
/* handle telemetry message */
if (sender_id > 0) {
switch (msg_id) {
case DL_GPS_SMALL: {
uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
// decode compressed values
int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
if (course & 0x400) {
course |= 0xF800; // fix for twos complements
}
course *= 2; // scale course by resolution
int16_t gspeed = (int16_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
if (gspeed & 0x400) {
gspeed |= 0xF800; // fix for twos complements
}
int16_t climb = (int16_t)(multiplex_speed & 0x3FF); // bits 9-0 z climb speed in cm/s
if (climb & 0x200) {
climb |= 0xFC00; // fix for twos complements
}
set_ac_info_lla(sender_id,
DL_GPS_SMALL_lat(dl_buffer),
DL_GPS_SMALL_lon(dl_buffer),
(int32_t)DL_GPS_SMALL_alt(dl_buffer) * 10,
course,
gspeed,
climb,
gps_tow_from_sys_ticks(sys_time.nb_tick));
}
break;
case DL_GPS: {
set_ac_info_utm(sender_id,
DL_GPS_utm_east(dl_buffer),
DL_GPS_utm_north(dl_buffer),
DL_GPS_alt(dl_buffer),
DL_GPS_utm_zone(dl_buffer),
DL_GPS_course(dl_buffer),
DL_GPS_speed(dl_buffer),
DL_GPS_climb(dl_buffer),
DL_GPS_itow(dl_buffer));
}
break;
case DL_GPS_LLA: {
set_ac_info_lla(sender_id,
DL_GPS_LLA_lat(dl_buffer),
DL_GPS_LLA_lon(dl_buffer),
DL_GPS_LLA_alt(dl_buffer),
DL_GPS_LLA_course(dl_buffer),
DL_GPS_LLA_speed(dl_buffer),
DL_GPS_LLA_climb(dl_buffer),
DL_GPS_LLA_itow(dl_buffer));
}
break;
default:
return FALSE;
}
/* handle datalink message */
} else {
switch (msg_id) {
case DL_ACINFO: {
set_ac_info_utm(DL_ACINFO_ac_id(dl_buffer),
DL_ACINFO_utm_east(dl_buffer),
DL_ACINFO_utm_north(dl_buffer),
DL_ACINFO_alt(dl_buffer) * 10,
DL_ACINFO_utm_zone(dl_buffer),
DL_ACINFO_course(dl_buffer),
DL_ACINFO_speed(dl_buffer),
DL_ACINFO_climb(dl_buffer),
DL_ACINFO_itow(dl_buffer));
}
break;
case DL_ACINFO_LLA: {
set_ac_info_lla(DL_ACINFO_LLA_ac_id(dl_buffer),
DL_ACINFO_LLA_lat(dl_buffer),
DL_ACINFO_LLA_lon(dl_buffer),
DL_ACINFO_LLA_alt(dl_buffer) * 10,
DL_ACINFO_LLA_course(dl_buffer),
DL_ACINFO_LLA_speed(dl_buffer),
DL_ACINFO_LLA_climb(dl_buffer),
DL_ACINFO_LLA_itow(dl_buffer));
}
break;
default:
return FALSE;
}
}
return TRUE;
}
void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t alt, uint8_t utm_zone, uint16_t course,
uint16_t gspeed, uint16_t climb, uint32_t itow)
{
if (ti_acs_idx < NB_ACS) {
if (id > 0 && ti_acs_id[id] == 0) { // new aircraft id
ti_acs_id[id] = ti_acs_idx++;
ti_acs[ti_acs_id[id]].ac_id = id;
}
ti_acs[ti_acs_id[id]].status = 0;
uint16_t my_zone = UtmZoneOfLlaLonDeg(gps.lla_pos.lon);
if (utm_zone == my_zone) {
ti_acs[ti_acs_id[id]].utm_pos_i.east = utm_east;
ti_acs[ti_acs_id[id]].utm_pos_i.north = utm_north;
ti_acs[ti_acs_id[id]].utm_pos_i.alt = alt;
ti_acs[ti_acs_id[id]].utm_pos_i.zone = utm_zone;
SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_UTM_I);
} else { // store other uav in utm extended zone
struct UtmCoor_i utm = {.east = utm_east, .north = utm_north, .alt = alt, .zone = utm_zone};
struct LlaCoor_i lla;
lla_of_utm_i(&lla, &utm);
LLA_COPY(ti_acs[ti_acs_id[id]].lla_pos_i, lla);
SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_LLA_I);
utm.zone = my_zone;
utm_of_lla_i(&utm, &lla);
UTM_COPY(ti_acs[ti_acs_id[id]].utm_pos_i, utm);
SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_UTM_I);
}
ti_acs[ti_acs_id[id]].course = RadOfDeciDeg(course);
ti_acs[ti_acs_id[id]].gspeed = MOfCm(gspeed);
ti_acs[ti_acs_id[id]].climb = MOfCm(climb);
SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_VEL_LOCAL_F);
ti_acs[ti_acs_id[id]].itow = itow;
}
}
void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt,
int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow)
{
if (ti_acs_idx < NB_ACS) {
if (id > 0 && ti_acs_id[id] == 0) {
ti_acs_id[id] = ti_acs_idx++;
ti_acs[ti_acs_id[id]].ac_id = id;
}
ti_acs[ti_acs_id[id]].status = 0;
struct LlaCoor_i lla = {.lat = lat, .lon = lon, .alt = alt};
LLA_COPY(ti_acs[ti_acs_id[id]].lla_pos_i, lla);
SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_LLA_I);
ti_acs[ti_acs_id[id]].course = RadOfDeciDeg(course);
ti_acs[ti_acs_id[id]].gspeed = MOfCm(gspeed);
ti_acs[ti_acs_id[id]].climb = MOfCm(climb);
SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_VEL_LOCAL_F);
ti_acs[ti_acs_id[id]].itow = itow;
}
}
/* Conversion funcitons for computing ac position in requested reference frame from
* any other available reference frame
*/
/* compute UTM position of aircraft with ac_id (int) */
void acInfoCalcPositionUtm_i(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
{
return;
}
/* LLA_i -> UTM_i is more accurate than from UTM_f */
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
{
// use my zone as reference, i.e zone extend
ti_acs[ac_nr].utm_pos_i.zone = UtmZoneOfLlaLonDeg(ti_acs[ac_nr].lla_pos_i.lon);
utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
update_geoid_height();
ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
{
UTM_BFP_OF_REAL(ti_acs[ac_nr].utm_pos_i, ti_acs[ac_nr].utm_pos_f);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
{
ti_acs[ac_nr].utm_pos_i.zone = 0;
utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
update_geoid_height();
ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
UTM_BFP_OF_REAL(ti_acs[ac_nr].utm_pos_i, ti_acs[ac_nr].utm_pos_f);
}
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
}
/* compute LLA position of aircraft with ac_id (int) */
void acInfoCalcPositionLla_i(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
{
return;
}
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
{
LLA_BFP_OF_REAL(ti_acs[ac_nr].lla_pos_i, ti_acs[ac_nr].lla_pos_f);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
{
lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
update_geoid_height();
ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
{
lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
update_geoid_height();
ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
LLA_BFP_OF_REAL(ti_acs[ac_nr].lla_pos_i, ti_acs[ac_nr].lla_pos_f);
}
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
}
/* compute ENU position of aircraft with ac_id (int) */
void acInfoCalcPositionEnu_i(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I))
{
return;
}
if (state.ned_initialized_i && (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I)))
{
enu_of_lla_point_i(&ti_acs[ac_nr].enu_pos_i, &state.ned_origin_i, acInfoGetPositionLla_i(ac_id));
} else if (state.ned_initialized_f && (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F)))
{
enu_of_lla_point_f(&ti_acs[ac_nr].enu_pos_f, &state.ned_origin_f, acInfoGetPositionLla_f(ac_id));
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f)
} else {
ti_acs[ac_nr].enu_pos_i = (struct EnuCoor_i) {0,0,0};
}
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
}
/* compute UTM position of aircraft with ac_id (float) */
void acInfoCalcPositionUtm_f(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
{
return;
}
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
{
UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
{
// use my zone as reference, i.e zone extend
ti_acs[ac_nr].utm_pos_i.zone = 0;
utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
update_geoid_height();
ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
{
/* not very accurate with float ~5cm */
utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
update_geoid_height();
ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
}
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
}
/* compute LLA position of aircraft with ac_id (float) */
void acInfoCalcPositionLla_f(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
{
return;
}
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
{
LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
{
lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
update_geoid_height();
ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
{
lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
update_geoid_height();
ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
}
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
}
/* compute ENU position of aircraft with ac_id (float) */
void acInfoCalcPositionEnu_f(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F))
{
return;
}
if (state.ned_initialized_f && (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F)))
{
enu_of_lla_point_f(&ti_acs[ac_nr].enu_pos_f, &state.ned_origin_f, acInfoGetPositionLla_f(ac_id));
} else if (state.ned_initialized_i && (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I)))
{
enu_of_lla_point_i(&ti_acs[ac_nr].enu_pos_i, &state.ned_origin_i, acInfoGetPositionLla_i(ac_id));
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i)
} else {
ti_acs[ac_nr].enu_pos_f = (struct EnuCoor_f) {0,0,0};
}
SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
}
/* compute ENU velocity of aircraft with ac_id (int) */
void acInfoCalcVelocityEnu_i(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
{
return;
}
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
{
SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
} else {
ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
}
SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I);
}
/* compute ENU position of aircraft with ac_id (float) */
void acInfoCalcVelocityEnu_f(uint8_t ac_id)
{
uint8_t ac_nr = ti_acs_id[ac_id];
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
{
return;
}
if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
{
SPEEDS_FLOAT_OF_BFP(ti_acs[ac_nr].enu_vel_f, ti_acs[ac_nr].enu_vel_i);
} else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_LOCAL_F)) {
ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
}
SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
}
sw/airborne/modules/multi/traffic_info.h
+445
View File
@@ -0,0 +1,445 @@
/*
* Copyright (C) Pascal Brisset, Antoine Drouin (2008), Kirk Scheper (2016)
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file "modules/mutli/traffic_info.h"
* @author Kirk Scheper
* Keeps track of other aircraft in airspace
*/
#ifndef TRAFFIC_INFO_H
#define TRAFFIC_INFO_H
#include "mcu_periph/sys_time.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
#include "subsystems/gps.h"
#ifndef NB_ACS_ID
#define NB_ACS_ID 256
#endif
#ifndef NB_ACS
#define NB_ACS 24
#endif
/**
* @defgroup ac_info Data availability representations
* @{
*/
#define AC_INFO_POS_UTM_I 0
#define AC_INFO_POS_LLA_I 1
#define AC_INFO_POS_ENU_I 2
#define AC_INFO_POS_UTM_F 3
#define AC_INFO_POS_LLA_F 4
#define AC_INFO_POS_ENU_F 5
#define AC_INFO_VEL_ENU_I 6
#define AC_INFO_VEL_ENU_F 7
#define AC_INFO_VEL_LOCAL_F 8
struct acInfo {
uint8_t ac_id;
/**
* Holds the status bits for all acinfo position and velocity representations.
* When the corresponding bit is set the representation
* is already computed.
*/
uint16_t status;
/**
* Position in UTM coordinates.
* Units x,y: centimetres.
* Units z: millimetres above MSL
*/
struct UtmCoor_i utm_pos_i;
/**
* Position in Latitude, Longitude and Altitude.
* Units lat,lon: degrees*1e7
* Units alt: milimeters above reference ellipsoid
*/
struct LlaCoor_i lla_pos_i;
/**
* Position in North East Down coordinates.
* Units: m in BFP with #INT32_POS_FRAC
*/
struct EnuCoor_i enu_pos_i;
/**
* Velocity in North East Down coordinates.
* Units: m/s in BFP with #INT32_SPEED_FRAC
*/
struct EnuCoor_i enu_vel_i;
/**
* Position in UTM coordinates.
* Units x,y: meters.
* Units z: meters above MSL
*/
struct UtmCoor_f utm_pos_f;
/**
* Position in Latitude, Longitude and Altitude.
* Units lat,lon: radians
* Units alt: meters above reference ellipsoid
*/
struct LlaCoor_f lla_pos_f;
/**
* Position in North East Down coordinates
* Units: m */
struct EnuCoor_f enu_pos_f;
/**
* @brief speed in North East Down coordinates
* @details Units: m/s */
struct EnuCoor_f enu_vel_f;
float course; ///< rad
float gspeed; ///< m/s
float climb; ///< m/s
uint32_t itow; ///< ms
};
extern uint8_t ti_acs_idx;
extern uint8_t ti_acs_id[];
extern struct acInfo ti_acs[];
extern void traffic_info_init(void);
/**
* Parse all datalink or telemetry messages that contain global position of other acs
* Messages currently handled:
* Telemetry (vehicle -> ground or vehicle -> vehicle): GPS_SMALL, GPS, GPS_LLA
* Datalink (ground -> vehicle): ACINFO, ACINFO_LLA
*/
extern bool parse_acinfo_dl(void);
/************************ Set functions ****************************/
/**
* Set Aircraft info.
* @param[in] id aircraft id, 0 is reserved for GCS, 1 for this aircraft (id=AC_ID)
* @param[in] utm_east UTM east in cm
* @param[in] utm_north UTM north in cm
* @param[in] alt Altitude in mm above MSL
* @param[in] utm_zone UTM zone
* @param[in] course Course in decideg (CW)
* @param[in] gspeed Ground speed in m/s
* @param[in] climb Climb rate in m/s
* @param[in] itow GPS time of week in ms
*/
extern void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t alt, uint8_t utm_zone,
uint16_t course, uint16_t gspeed, uint16_t climb, uint32_t itow);
/**
* Set Aircraft info.
* @param[in] id aircraft id, 0 is reserved for GCS, 1 for this aircraft (id=AC_ID)
* @param[in] lat Latitude in 1e7deg
* @param[in] lon Longitude in 1e7deg
* @param[in] alt Altitude in mm above ellipsoid
* @param[in] course Course in decideg (CW)
* @param[in] gspeed Ground speed in cm/s
* @param[in] climb Climb rate in cm/s
* @param[in] itow GPS time of week in ms
*/
extern void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt,
int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow);
/** Set position from UTM coordinates (int).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_vel velocity in ENU (float)
*/
static inline void acInfoSetPositionUtm_i(uint8_t ac_id, struct UtmCoor_i *utm_pos)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
UTM_COPY(ti_acs[ti_acs_id[ac_id]].utm_pos_i, *utm_pos);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_POS_UTM_I);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set position from LLA coordinates (int).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_vel velocity in ENU (float)
*/
static inline void acInfoSetPositionLla_i(uint8_t ac_id, struct LlaCoor_i *lla_pos)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
LLA_COPY(ti_acs[ti_acs_id[ac_id]].lla_pos_i, *lla_pos);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_POS_LLA_I);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set velocity from ENU coordinates (int).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_pos position in ENU (int)
*/
static inline void acInfoSetPositionEnu_i(uint8_t ac_id, struct EnuCoor_i *enu_pos)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
VECT3_COPY(ti_acs[ti_acs_id[ac_id]].enu_pos_i, *enu_pos);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_POS_ENU_I);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set position from UTM coordinates (float).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_vel velocity in ENU (float)
*/
static inline void acInfoSetPositionUtm_f(uint8_t ac_id, struct UtmCoor_f *utm_pos)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
UTM_COPY(ti_acs[ti_acs_id[ac_id]].utm_pos_f, *utm_pos);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_POS_UTM_F);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set position from LLA coordinates (float).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_vel velocity in ENU (float)
*/
static inline void acInfoSetPositionLla_f(uint8_t ac_id, struct LlaCoor_f *lla_pos)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
LLA_COPY(ti_acs[ti_acs_id[ac_id]].lla_pos_i, *lla_pos);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_POS_LLA_F);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set velocity from ENU coordinates (float).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_pos position in ENU (float)
*/
static inline void acInfoSetPositionEnu_f(uint8_t ac_id, struct EnuCoor_f *enu_pos)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
VECT3_COPY(ti_acs[ti_acs_id[ac_id]].enu_pos_f, *enu_pos);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_POS_ENU_F);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set velocity from ENU coordinates (int).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_vel velocity in ENU (int)
*/
static inline void acInfoSetVelocityEnu_i(uint8_t ac_id, struct EnuCoor_i *enu_vel)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
VECT3_COPY(ti_acs[ti_acs_id[ac_id]].enu_vel_i, *enu_vel);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_VEL_ENU_I);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/** Set velocity from ENU coordinates (float).
* @param[in] aircraft id of aircraft info to set
* @param[in] enu_vel velocity in ENU (float)
*/
static inline void acInfoSetVelocityEnu_f(uint8_t ac_id, struct EnuCoor_f *enu_vel)
{
if (ti_acs_idx < NB_ACS) {
if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
ti_acs_id[ac_id] = ti_acs_idx++;
ti_acs[ti_acs_id[ac_id]].ac_id = ac_id;
}
VECT3_COPY(ti_acs[ti_acs_id[ac_id]].enu_vel_i, *enu_vel);
/* clear bits for all position representations and only set the new one */
ti_acs[ti_acs_id[ac_id]].status = (1 << AC_INFO_VEL_ENU_F);
ti_acs[ti_acs_id[ac_id]].itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
}
}
/*************** Reference frame conversion functions ***************/
extern void acInfoCalcPositionUtm_i(uint8_t ac_id);
extern void acInfoCalcPositionUtm_f(uint8_t ac_id);
extern void acInfoCalcPositionLla_i(uint8_t ac_id);
extern void acInfoCalcPositionLla_f(uint8_t ac_id);
extern void acInfoCalcPositionEnu_i(uint8_t ac_id);
extern void acInfoCalcPositionEnu_f(uint8_t ac_id);
extern void acInfoCalcVelocityEnu_i(uint8_t ac_id);
extern void acInfoCalcVelocityEnu_f(uint8_t ac_id);
/************************ Get functions ****************************/
/** Get position from UTM coordinates (int).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct UtmCoor_i *acInfoGetPositionUtm_i(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_UTM_I)) {
acInfoCalcPositionUtm_i(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].utm_pos_i;
}
/** Get position from LLA coordinates (int).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct LlaCoor_i *acInfoGetPositionLla_i(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_LLA_I)) {
acInfoCalcPositionLla_i(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].lla_pos_i;
}
/** Get position in local ENU coordinates (int).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct EnuCoor_i *acInfoGetPositionEnu_i(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_ENU_I)) {
acInfoCalcPositionEnu_i(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].enu_pos_i;
}
/** Get position from UTM coordinates (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct UtmCoor_f *acInfoGetPositionUtm_f(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_UTM_F)) {
acInfoCalcPositionUtm_f(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].utm_pos_f;
}
/** Get position from LLA coordinates (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct LlaCoor_f *acInfoGetPositionLla_f(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_LLA_F)) {
acInfoCalcPositionLla_f(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].lla_pos_f;
}
/** Get position in local ENU coordinates (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct EnuCoor_f *acInfoGetPositionEnu_f(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_ENU_F)) {
acInfoCalcPositionEnu_f(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].enu_pos_f;
}
/** Get position from ENU coordinates (int).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct EnuCoor_i *acInfoGetVelocityEnu_i(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_VEL_ENU_I)) {
acInfoCalcVelocityEnu_i(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].enu_vel_i;
}
/** Get position from ENU coordinates (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline struct EnuCoor_f *acInfoGetVelocityEnu_f(uint8_t ac_id)
{
if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_VEL_ENU_F)) {
acInfoCalcVelocityEnu_f(ac_id);
}
return &ti_acs[ti_acs_id[ac_id]].enu_vel_f;
}
/** Get vehicle course (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline float acInfoGetCourse(uint8_t ac_id)
{
return ti_acs[ti_acs_id[ac_id]].course;
}
/** Get vehicle ground speed (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline float acInfoGetGspeed(uint8_t ac_id)
{
return ti_acs[ti_acs_id[ac_id]].gspeed;
}
/** Get vehicle climb speed (float).
* @param[in] aircraft id of aircraft info to get
*/
static inline float acInfoGetClimb(uint8_t ac_id)
{
return ti_acs[ti_acs_id[ac_id]].climb;
}
/** Get time of week from latest message (ms).
* @param[in] aircraft id of aircraft info to get
*/
static inline uint32_t acInfoGetItow(uint8_t ac_id)
{
return ti_acs[ti_acs_id[ac_id]].itow;
}
#endif
sw/airborne/subsystems/datalink/datalink.c
+55 -141
View File
@@ -35,7 +35,6 @@
#include "generated/settings.h" #include "generated/settings.h"
#include "pprzlink/messages.h" #include "pprzlink/messages.h"
#include "pprzlink/dl_protocol.h"
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
#include "subsystems/radio_control.h" #include "subsystems/radio_control.h"
@@ -45,16 +44,10 @@
#include "subsystems/gps.h" #include "subsystems/gps.h"
#endif #endif
#ifdef TRAFFIC_INFO
#include "subsystems/navigation/traffic_info.h"
#endif
#ifdef RADIO_CONTROL_DATALINK_LED #ifdef RADIO_CONTROL_DATALINK_LED
#include "led.h" #include "led.h"
#endif #endif
#define MOfCm(_x) (((float)(_x))/100.)
#if USE_NPS #if USE_NPS
bool datalink_enabled = true; bool datalink_enabled = true;
#endif #endif
@@ -67,156 +60,77 @@ void dl_parse_msg(void)
/* parse telemetry messages coming from other AC */ /* parse telemetry messages coming from other AC */
if (sender_id != 0) { if (sender_id != 0) {
switch (msg_id) { switch (msg_id) {
#if 0 // TODO waiting for telemetry macros to be made in pprzlink
#ifdef TRAFFIC_INFO
case DL_GPS_SMALL: {
uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
// Position in ENU coordinates
int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
if (course & 0x400) {
course |= 0xFFFFF800; // fix for twos complements
}
int16_t gspeed = (int16_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
if (gspeed & 0x400) {
gspeed |= 0xFFFFF800; // fix for twos complements
}
int16_t climb = (int16_t)(multiplex_speed >> 2 & 0x7FF); // bits 9-0 z climb speed in cm/s
if (climb & 0x400) {
climb |= 0xFFFFF800; // fix for twos complements
}
set_ac_info(sender_id,
MOfCm(DL_GPS_SMALL_utm_east(dl_buffer)), /*m*/
MOfCm(DL_GPS_SMALL_utm_north(dl_buffer)), /*m*/
RadOfDeg(((float)course) / 10.), /*rad(CW)*/
MOfCm(DL_GPS_SMALL_alt(dl_buffer)), /*m*/
MOfCm(gspeed), /*m/s*/
MOfCm(climb), /*m/s*/
gps_tow_from_sys_ticks(sys_time.nb_tick));
}
break;
case DL_GPS: {
set_ac_info(sender_id,
MOfCm(DL_GPS_utm_east(dl_buffer)), /*m*/
MOfCm(DL_GPS_utm_north(dl_buffer)), /*m*/
RadOfDeg(((float)DL_GPS_course(dl_buffer)) / 10.), /*rad(CW)*/
MOfCm(DL_GPS_alt(dl_buffer)), /*m*/
MOfCm(DL_GPS_speed(dl_buffer)), /*m/s*/
MOfCm(DL_GPS_climb(dl_buffer)), /*m/s*/
(uint32_t)DL_GPS_itow(dl_buffer));
}
break;
case DL_GPS_LLA: {
set_ac_info_lla(sender_id,
DL_GPS_LLA_lat(dl_buffer), /*1e7deg*/
DL_GPS_LLA_lon(dl_buffer), /*1e7deg*/
DL_GPS_LLA_alt(dl_buffer), /*mm*/
DL_GPS_LLA_course(dl_buffer), /*decideg*/
DL_GPS_LLA_speed(dl_buffer), /*cm/s*/
DL_GPS_LLA_climb(dl_buffer), /*cm/s*/
DL_GPS_LLA_itow(dl_buffer)); /*ms*/
}
break;
#endif /* TRAFFIC_INFO */
#ifdef TCAS
case DL_TCAS_RA: {
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
}
}
#endif /* TCAS */
#endif
default: { default: {
break; break;
} }
} }
return; // msg was telemetry not datalink so return } else {
} /* parse telemetry messages coming from ground station */
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
/* parse telemetry messages coming from ground station */ case DL_SETTING : {
switch (msg_id) { if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
case DL_PING: { uint8_t i = DL_SETTING_index(dl_buffer);
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); float var = DL_SETTING_value(dl_buffer);
} DlSetting(i, var);
break; DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_SETTING : { case DL_GET_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; } if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer); uint8_t i = DL_GET_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer); float val = settings_get_value(i);
DlSetting(i, var); DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var); }
} break;
break;
case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
#ifdef RADIO_CONTROL_TYPE_DATALINK #ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH : case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED #ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif #endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer), parse_rc_3ch_datalink(
DL_RC_4CH_throttle(dl_buffer), DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_4CH_roll(dl_buffer), DL_RC_3CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer), DL_RC_3CH_pitch(dl_buffer));
DL_RC_4CH_yaw(dl_buffer)); break;
} case DL_RC_4CH :
break; if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK #endif // RADIO_CONTROL_TYPE_DATALINK
#if USE_GPS #if USE_GPS
case DL_GPS_INJECT : { case DL_GPS_INJECT : {
// Check if the GPS is for this AC // Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; } if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
// GPS parse data // GPS parse data
gps_inject_data( gps_inject_data(
DL_GPS_INJECT_packet_id(dl_buffer), DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer), DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer) DL_GPS_INJECT_data(dl_buffer)
); );
} }
break; break;
#endif // USE_GPS #endif // USE_GPS
#ifdef TRAFFIC_INFO default:
case DL_ACINFO: { break;
if (DL_ACINFO_ac_id(dl_buffer) == AC_ID) { break; }
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));
float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
uint32_t t = DL_ACINFO_itow(dl_buffer);
set_ac_info(id, ux, uy, c, a, s, cl, t);
} }
break;
#endif
default:
break;
} }
/* Parse firmware specific datalink */ /* Parse firmware specific datalink */
firmware_parse_msg(); firmware_parse_msg();
@@ -225,7 +139,7 @@ void dl_parse_msg(void)
modules_parse_datalink(msg_id); modules_parse_datalink(msg_id);
} }
/* default emtpy WEAK implementation for firmwares without an extra firmware_parse_msg */ /* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */
WEAK void firmware_parse_msg(void) WEAK void firmware_parse_msg(void)
{ {
} }
sw/airborne/subsystems/navigation/traffic_info.c
-95
View File
@@ -1,95 +0,0 @@
/*
* Copyright (C) 2005 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/traffic_info.c
*
* Information relative to the other aircrafts.
*
*/
#include "subsystems/navigation/traffic_info.h"
//#include "subsystems/navigation/common_nav.h"
#include "generated/airframe.h" // AC_ID
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
uint8_t acs_idx;
uint8_t the_acs_id[NB_ACS_ID];
struct ac_info_ the_acs[NB_ACS];
void traffic_info_init(void)
{
the_acs_id[0] = 0; // ground station
the_acs_id[AC_ID] = 1;
the_acs[the_acs_id[AC_ID]].ac_id = AC_ID;
acs_idx = 2;
}
struct ac_info_ *get_ac_info(uint8_t _id)
{
return &the_acs[the_acs_id[_id]];
}
void set_ac_info(uint8_t id, float utm_east, float utm_north, float course, float alt,
float gspeed, float climb, uint32_t itow)
{
if (acs_idx < NB_ACS) {
if (id > 0 && the_acs_id[id] == 0) {
the_acs_id[id] = acs_idx++;
the_acs[the_acs_id[id]].ac_id = id;
}
the_acs[the_acs_id[id]].east = utm_east;// - nav_utm_east0;
the_acs[the_acs_id[id]].north = utm_north;// - nav_utm_north0;
the_acs[the_acs_id[id]].course = course;
the_acs[the_acs_id[id]].alt = alt;// +- NAV_MSL0;
the_acs[the_acs_id[id]].gspeed = gspeed;
the_acs[the_acs_id[id]].climb = climb;
the_acs[the_acs_id[id]].itow = itow;
}
}
void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt,
int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow)
{
if (acs_idx < NB_ACS) {
if (id > 0 && the_acs_id[id] == 0) {
the_acs_id[id] = acs_idx++;
the_acs[the_acs_id[id]].ac_id = id;
}
struct LlaCoor_i lla_i = {.lat = lat, .lon = lon, .alt = alt};
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, lla_i);
struct UtmCoor_f utm_f;
utm_of_lla_f(&utm_f, &lla_f);
the_acs[the_acs_id[id]].east = utm_f.east;
the_acs[the_acs_id[id]].north = utm_f.north;
the_acs[the_acs_id[id]].alt = utm_f.alt;
the_acs[the_acs_id[id]].course = RadOfDeg((float)course / 10.);
the_acs[the_acs_id[id]].gspeed = (float)gspeed * 100;
the_acs[the_acs_id[id]].climb = (float)climb * 100;
the_acs[the_acs_id[id]].itow = itow;
}
}
sw/airborne/subsystems/navigation/traffic_info.h
-83
View File
@@ -1,83 +0,0 @@
/*
* Copyright (C) 2005 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/traffic_info.h
*
* Information relative to the other aircrafts.
*
*/
#ifndef TI_H
#define TI_H
#define NB_ACS_ID 256
#define NB_ACS 24
#include <inttypes.h>
struct ac_info_ {
uint8_t ac_id;
float east; ///< m relative to nav_utm_east0
float north; ///< m relative to nav_utm_north0
float course; ///< rad (CW)
float alt; ///< m above Mean Sea Level (geoid)
float gspeed; ///< m/s
float climb; ///< m/s
uint32_t itow;///< ms
};
extern uint8_t acs_idx;
extern uint8_t the_acs_id[NB_ACS_ID];
extern struct ac_info_ the_acs[NB_ACS];
extern void traffic_info_init(void);
extern struct ac_info_ *get_ac_info(uint8_t id);
/**
* Set Aircraft info.
* @param[in] id aircraft id, 0 is reserved for GCS, 1 for this aircraft (id=AC_ID)
* @param[in] utm_east UTM east in m relative to nav_utm_east0
* @param[in] utm_north UTM north in m relative to nav_utm_north0
* @param[in] course Course in rad (CW)
* @param[in] alt Altitude in m above MSL
* @param[in] gspeed Ground speed in m/s
* @param[in] climb Climb rate in m/s
* @param[in] itow GPS time of week in ms
*/
extern void set_ac_info(uint8_t id, float utm_east, float utm_north, float course, float alt,
float gspeed, float climb, uint32_t itow);
/**
* Set Aircraft info.
* @param[in] id aircraft id, 0 is reserved for GCS, 1 for this aircraft (id=AC_ID)
* @param[in] lat Latitude in 1e7deg
* @param[in] lon Longitude in 1e7deg
* @param[in] alt Altitude in mm above MSL
* @param[in] course Course in decideg (CW)
* @param[in] gspeed Ground speed in cm/s
* @param[in] climb Climb rate in cm/s
* @param[in] itow GPS time of week in ms
*/
extern void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt,
int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow);
#endif
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 4b59c679a6...ebe2624e43
sw/ground_segment/tmtc/server.ml
+23 -10
View File
@@ -365,16 +365,29 @@ let send_aircraft_msg = fun ac ->
begin begin
let cm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate (100. *. f))) in let cm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate (100. *. f))) in
let cm_of_m = fun f -> PprzLink.Int (truncate (100. *. f)) in let cm_of_m = fun f -> PprzLink.Int (truncate (100. *. f)) in
let pos = LL.utm_of WGS84 a.pos in if a.vehicle_type = FixedWing then
let ac_info = ["ac_id", PprzLink.String ac; let pos = LL.utm_of WGS84 a.pos in
"utm_east", cm_of_m_32 pos.utm_x; let ac_info = ["ac_id", PprzLink.String ac;
"utm_north", cm_of_m_32 pos.utm_y; "utm_east", cm_of_m_32 pos.utm_x;
"course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg a.course))); "utm_north", cm_of_m_32 pos.utm_y;
"alt", cm_of_m_32 a.alt; "utm_zone", PprzLink.Int pos.utm_zone;
"speed", cm_of_m a.gspeed; "course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg a.course)));
"climb", cm_of_m a.climb; "alt", cm_of_m_32 a.alt;
"itow", PprzLink.Int64 a.itow] in "speed", cm_of_m a.gspeed;
Dl_Pprz.message_send dl_id "ACINFO" ac_info; "climb", cm_of_m a.climb;
"itow", PprzLink.Int64 a.itow] in
Dl_Pprz.message_send dl_id "ACINFO" ac_info;
else
let deg7_of_rad = fun f -> PprzLink.Int32 (Int32.of_float (Geometry_2d.rad2deg (f *. 1e7))) in
let ac_info_lla = ["ac_id", PprzLink.String ac;
"lat", deg7_of_rad a.pos.posn_lat;
"lon", deg7_of_rad a.pos.posn_long;
"course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg a.course)));
"alt", cm_of_m_32 a.alt;
"speed", cm_of_m a.gspeed;
"climb", cm_of_m a.climb;
"itow", PprzLink.Int64 a.itow] in
Dl_Pprz.message_send dl_id "ACINFO_LLA" ac_info_lla;
end; end;
if !Kml.enabled then if !Kml.enabled then
sw/include/std.h
+93 -88
View File
@@ -86,6 +86,10 @@ typedef uint8_t unit_t;
#define DegOfRad(x) ((x) * (180. / M_PI)) #define DegOfRad(x) ((x) * (180. / M_PI))
#define DeciDegOfRad(x) ((x) * (1800./ M_PI)) #define DeciDegOfRad(x) ((x) * (1800./ M_PI))
#define RadOfDeg(x) ((x) * (M_PI/180.)) #define RadOfDeg(x) ((x) * (M_PI/180.))
#define RadOfDeciDeg(x) ((x) * (M_PI/1800.))
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMm(_x) (((float)(_x))/1000.)
#define Min(x,y) (x < y ? x : y) #define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y) #define Max(x,y) (x > y ? x : y)
@@ -109,114 +113,115 @@ typedef uint8_t unit_t;
#define BoundWrapped(_x, _min, _max) { \ #define BoundWrapped(_x, _min, _max) { \
if ((_max) > (_min)) \ if ((_max) > (_min)) \
Bound(_x, _min, _max) \ Bound(_x, _min, _max) \
else \ else \
BoundInverted(_x, _min, _max) \ BoundInverted(_x, _min, _max) \
} }
#define BoundAbs(_x, _max) Bound(_x, -(_max), (_max)) #define BoundAbs(_x, _max) Bound(_x, -(_max), (_max))
#define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) ) #define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) )
#define ChopAbs(x, max) Chop(x, -(max), (max)) #define ChopAbs(x, max) Chop(x, -(max), (max))
#define DeadBand(_x, _v) { \ #define DeadBand(_x, _v) { \
if (_x > (_v)) \ if (_x > (_v)) \
_x = _x -(_v); \ _x = _x -(_v); \
else if (_x < -(_v)) \ else if (_x < -(_v)) \
_x = _x +(_v); \ _x = _x +(_v); \
else \ else \
_x = 0; \ _x = 0; \
} }
#define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b)) #define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b))
#define RunOnceEvery(_prescaler, _code) { \ #define RunOnceEvery(_prescaler, _code) { \
static uint16_t prescaler = 0; \ static uint16_t prescaler = 0; \
prescaler++; \ prescaler++; \
if (prescaler >= _prescaler) { \ if (prescaler >= _prescaler) { \
prescaler = 0; \ prescaler = 0; \
_code; \ _code; \
} \ } \
} }
#define RunXTimesEvery(_jumpstart, _prescaler, _interval, _xtimes, _code) { \ #define RunXTimesEvery(_jumpstart, _prescaler, _interval, _xtimes, _code) { \
static uint16_t prescaler = _jumpstart; \ static uint16_t prescaler = _jumpstart; \
static uint16_t xtimes = 0; \ static uint16_t xtimes = 0; \
prescaler++; \ prescaler++; \
if (prescaler >= _prescaler + _interval*xtimes && xtimes < _xtimes) { \ if (prescaler >= _prescaler + _interval*xtimes && xtimes < _xtimes) { \
_code; \ _code; \
xtimes++; \ xtimes++; \
} \ } \
if (xtimes >= _xtimes) { \ if (xtimes >= _xtimes) { \
xtimes = 0; \ xtimes = 0; \
prescaler = 0; \ prescaler = 0; \
} \ } \
} }
#define PeriodicPrescaleBy5( _code_0, _code_1, _code_2, _code_3, _code_4) { \ #define PeriodicPrescaleBy5( _code_0, _code_1, _code_2, _code_3, _code_4) { \
static uint8_t _50hz = 0; \ static uint8_t _50hz = 0; \
_50hz++; \ _50hz++; \
if (_50hz >= 5) _50hz = 0; \ if (_50hz >= 5) _50hz = 0; \
switch (_50hz) { \ switch (_50hz) { \
case 0: \ case 0: \
_code_0; \ _code_0; \
break; \ break; \
case 1: \ case 1: \
_code_1; \ _code_1; \
break; \ break; \
case 2: \ case 2: \
_code_2; \ _code_2; \
break; \ break; \
case 3: \ case 3: \
_code_3; \ _code_3; \
break; \ break; \
case 4: \ case 4: \
_code_4; \ _code_4; \
break; \ break; \
} \ } \
} }
#define PeriodicPrescaleBy10( _code_0, _code_1, _code_2, _code_3, _code_4, _code_5, _code_6, _code_7, _code_8, _code_9) { \ #define PeriodicPrescaleBy10( _code_0, _code_1, _code_2, _code_3, _code_4, _code_5, _code_6, _code_7, _code_8, _code_9) { \
static uint8_t _cnt = 0; \ static uint8_t _cnt = 0; \
_cnt++; \ _cnt++; \
if (_cnt >= 10) _cnt = 0; \ if (_cnt >= 10) _cnt = 0; \
switch (_cnt) { \ switch (_cnt) { \
case 0: \ case 0: \
_code_0; \ _code_0; \
break; \ break; \
case 1: \ case 1: \
_code_1; \ _code_1; \
break; \ break; \
case 2: \ case 2: \
_code_2; \ _code_2; \
break; \ break; \
case 3: \ case 3: \
_code_3; \ _code_3; \
break; \ break; \
case 4: \ case 4: \
_code_4; \ _code_4; \
break; \ break; \
case 5: \ case 5: \
_code_5; \ _code_5; \
break; \ break; \
case 6: \ case 6: \
_code_6; \ _code_6; \
break; \ break; \
case 7: \ case 7: \
_code_7; \ _code_7; \
break; \ break; \
case 8: \ case 8: \
_code_8; \ _code_8; \
break; \ break; \
case 9: \ case 9: \
default: \ default: \
_code_9; \ _code_9; \
break; \ break; \
} \ } \
} }
static inline bool str_equal(const char * a, const char * b) { static inline bool str_equal(const char *a, const char *b)
{
int i = 0; int i = 0;
while (!(a[i] == 0 && b[i] == 0)) { while (!(a[i] == 0 && b[i] == 0)) {
if (a[i] != b[i]) return FALSE; if (a[i] != b[i]) { return FALSE; }
i++; i++;
} }
return TRUE; return TRUE;