Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 08:22:43 +08:00
Code Issues Actions 26 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
endif
$(TARGET).CFLAGS += -DTRAFFIC_INFO
#
# frequencies
#
conf/firmwares/subsystems/fixedwing/navigation.makefile
+1 -3
View File
@@ -7,8 +7,6 @@
$(TARGET).CFLAGS += -DNAV
$(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/traffic_info.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).srcs += $(SRC_FIRMWARE)/navigation.c
#$(TARGET).srcs += subsystems/navigation/common_nav.c
$(TARGET).srcs += subsystems/navigation/waypoints.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">
<doc>
<description>
Follow a certain AC_ID trough remote GPS.
Follow a certain AC_ID using traffic_info.
Only for rotorcraft firmware.
</description>
<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_Z" value="0" description="the z offset in ENU (meters) from the plane"/>
</doc>
<depends>traffic_info</depends>
<header>
<file name="follow.h"/>
</header>
<init fun="follow_init()"/>
<datalink message="REMOTE_GPS" fun="ParseRemoteGps()"/>
<periodic fun="follow_wp()" freq="5" autorun="FALSE"/>
<makefile>
<file name="follow.c"/>
</makefile>
conf/modules/formation_flight.xml
+3 -2
View File
@@ -16,12 +16,13 @@
</dl_settings>
</dl_settings>
</settings>
<depends>traffic_info</depends>
<header>
<file name="formation.h"/>
</header>
<init fun="formation_init()"/>
<datalink message="FORMATION_STATUS" fun="ParseFormationStatus()"/>
<datalink message="FORMATION_SLOT" fun="ParseFormationSlot()"/>
<datalink message="FORMATION_STATUS" fun="parseFormationStatus()"/>
<datalink message="FORMATION_SLOT" fun="parseFormationSlot()"/>
<makefile>
<file name="formation.c"/>
</makefile>
conf/modules/tcas.xml
+3 -1
View File
@@ -4,13 +4,15 @@
<doc>
<description>TCAS collision avoidance</description>
</doc>
<depends>traffic_info</depends>
<header>
<file name="tcas.h"/>
</header>
<init fun="tcas_init()"/>
<periodic fun="tcas_periodic_task_1Hz()" freq="1"/>
<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>
<file name="tcas.c"/>
<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 "autopilot.h"
#include "subsystems/gps.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/settings.h"
#include "firmwares/fixedwing/nav.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 "firmwares/fixedwing/nav.h"
#include "generated/flight_plan.h"
#ifdef TRAFFIC_INFO
#include "subsystems/navigation/traffic_info.h"
#endif
// datalink & telemetry
#if DATALINK || SITL
@@ -255,12 +252,6 @@ void init_ap(void)
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
/* set initial trim values.
* 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 */
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 (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
@@ -532,7 +523,7 @@ void navigation_task(void)
}
#ifdef TCAS
CallTCAS();
callTCAS();
#endif
#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/autopilot.h"
#include "inter_mcu.h"
#include "subsystems/navigation/traffic_info.h"
#include "subsystems/gps.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 {}
#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.);
NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt + SECURITY_HEIGHT), 0.);
float alpha = M_PI / 2 - ac->course;
NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.);
float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
float ca = cosf(alpha), sa = sinf(alpha);
float x = ac->east - _distance * ca;
float y = ac->north - _distance * sa;
float x = ac->x - distance * ca;
float y = ac->y - distance * sa;
fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
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();
#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 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
#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;
}
@@ -446,9 +447,9 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
INT32_COURSE_NORMALIZE(nav_heading);
RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp,
&(waypoints[wp].enu_i.x),
&(waypoints[wp].enu_i.y),
&(waypoints[wp].enu_i.z)));
&(waypoints[wp].enu_i.x),
&(waypoints[wp].enu_i.y),
&(waypoints[wp].enu_i.z)));
}
bool nav_detect_ground(void)
@@ -675,3 +676,24 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius)
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
*
* 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.
*/
/*
* Copyright (C) 2008-2011 The Paparazzi Team
*
* 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 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);
#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 */
sw/airborne/math/pprz_geodetic.h
+4 -5
View File
@@ -70,26 +70,25 @@ extern "C" {
(_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).y = (_utm1).north - (_utm2).north; \
(_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).y = (_utm1).east - (_utm2).east; \
(_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).north = (_utm0).north + (_pos).y; \
(_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).north = (_utm0).north + (_pos).x; \
(_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 "generated/flight_plan.h"
#include "state.h"
#include "subsystems/navigation/traffic_info.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
* 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
* -90° is to the front.
* the antenna look to the right with 0˚ given, 90˚ is to the back and
* -90˚ is to the front.
*
*
*
* plane front
*
* 90
* 90˚
^
* I
* 135 I 45°
* 135˚ I 45˚
* \ I /
* \I/
* 180-------I------- 0°
* 180˚-------I------- 0˚
* /I\
* / I \
* -135 I -45°
* -135˚ I -45˚
* I
* -90
* plane back
@@ -140,9 +139,9 @@ void airborne_ant_point_periodic(void)
*
*/
/* fPan = 0 -> antenna looks along the wing
90 -> antenna looks in flight direction
-90 -> antenna looks backwards
/* fPan = 0˚ -> antenna looks along the wing
90˚ -> antenna looks in flight direction
-90˚ -> antenna looks backwards
*/
/* fixed to the plane*/
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 "generated/flight_plan.h"
#include "state.h"
#include "subsystems/navigation/traffic_info.h"
#ifdef POINT_CAM
#include "point.h"
#endif // POINT_CAM
@@ -303,13 +302,17 @@ void cam_waypoint_target(void)
cam_target();
}
#ifdef TRAFFIC_INFO
#include "modules/multi/traffic_info.h"
void cam_ac_target(void)
{
#ifdef TRAFFIC_INFO
struct ac_info_ * ac = get_ac_info(cam_target_ac);
cam_target_x = ac->east;
cam_target_y = ac->north;
cam_target_alt = ac->alt;
struct EnuCoor_f ac_pos *ac = acInfoGetPositionEnu_f(cam_target_ac);
cam_target_x = ac->x;
cam_target_y = ac->y;
cam_target_alt = acInfoGetPositionUtm_f()->alt;
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 "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
#define FOLLOW_OFFSET_X 0.0
#endif
@@ -47,31 +46,29 @@
#define FOLLOW_OFFSET_Z 0.0
#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);
}
void follow_change_wp(unsigned char *buffer)
{
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
struct EnuCoor_i enu = *stateGetPositionEnu_i();
enu.x += ac->x + POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
enu.y += ac->y + POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
enu.z += ac->z + POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
// Move the waypoint
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
extern void follow_init(void);
extern void follow_change_wp(unsigned char *buffer);
#define ParseRemoteGps() { \
if (DL_REMOTE_GPS_ac_id(dl_buffer) == FOLLOW_AC_ID) { \
follow_change_wp(dl_buffer); \
} \
}
extern void follow_wp(void);
#endif // FOLLOW
sw/airborne/modules/multi/formation.c
+49 -56
View File
@@ -4,21 +4,17 @@
#define FORMATION_C
#include <math.h>
#include "multi/formation.h"
#include "std.h"
#include "state.h"
#include "subsystems/datalink/downlink.h"
#include "multi/formation.h"
#include "state.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/nav.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_speed, form_speed_n, form_speed_e;
@@ -77,18 +73,18 @@ int formation_init(void)
form_prox = FORM_PROX;
form_mode = FORM_MODE;
old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
old_alt = GROUND_ALT + SECURITY_HEIGHT;
old_alt = SECURITY_ALT;
return false;
}
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);
formation[the_acs_id[_id]].status = IDLE;
formation[the_acs_id[_id]].east = slot_e;
formation[the_acs_id[_id]].north = slot_n;
formation[the_acs_id[_id]].alt = slot_a;
formation[ti_acs_id[_id]].status = IDLE;
formation[ti_acs_id[_id]].east = slot_e;
formation[ti_acs_id[_id]].north = slot_n;
formation[ti_acs_id[_id]].alt = slot_a;
return false;
}
@@ -120,15 +116,14 @@ int stop_formation(void)
v_ctl_auto_throttle_cruise_throttle = old_cruise;
old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
nav_altitude = old_alt;
old_alt = GROUND_ALT + SECURITY_HEIGHT;
old_alt = SECURITY_ALT;
return false;
}
int formation_flight(void)
{
static uint8_t _1Hz = 0;
static uint8_t _1Hz = 0;
uint8_t nb = 0, i;
float hspeed_dir = stateGetHorizontalSpeedDir_f();
float ch = cosf(hspeed_dir);
@@ -141,30 +136,27 @@ int formation_flight(void)
form_speed_e = form_speed * sh;
if (AC_ID == leader_id) {
stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
stateGetPositionEnu_f()->x += formation[ti_acs_id[AC_ID]].east;
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
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);
if (++_1Hz >= 4) {
_1Hz = 0;
DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
&formation[the_acs_id[AC_ID]].east,
&formation[the_acs_id[AC_ID]].north,
&formation[the_acs_id[AC_ID]].alt);
&formation[ti_acs_id[AC_ID]].east,
&formation[ti_acs_id[AC_ID]].north,
&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
struct ac_info_ * leader = get_ac_info(leader_id);
if (formation[the_acs_id[leader_id]].status == UNSET ||
formation[the_acs_id[leader_id]].status == IDLE) {
struct EnuCoor_f *leader_pos = acInfoGetPositionEnu_f(leader_id);
if (formation[ti_acs_id[leader_id]].status == UNSET ||
formation[ti_acs_id[leader_id]].status == IDLE) {
// leader not ready or not in formation
return false;
}
@@ -173,8 +165,8 @@ int formation_flight(void)
struct slot_ form[NB_ACS];
float cr = 0., sr = 1.;
if (form_mode == FORM_MODE_COURSE) {
cr = cosf(leader->course);
sr = sinf(leader->course);
cr = cosf(acInfoGetCourse(leader_id));
sr = sinf(acInfoGetCourse(leader_id));
}
for (i = 0; i < NB_ACS; ++i) {
if (formation[i].status == UNSET) { continue; }
@@ -183,25 +175,26 @@ int formation_flight(void)
form[i].alt = formation[i].alt;
}
struct EnuCoor_f *my_pos = stateGetPositionEnu_f();
// compute control forces
for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
if (ti_acs[i].ac_id == AC_ID) { continue; }
struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ti_acs[i].ac_id);
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 AC not responding for too long
formation[i].status = LOST;
continue;
} 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;
if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
- (form[i].east - form[the_acs_id[AC_ID]].east);
form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
- (form[i].north - form[the_acs_id[AC_ID]].north);
form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
form_speed += ac->gspeed;
if (ac->z > 0 && fabs(my_pos->z - ac->z) < form_prox) {
form_e += (ac->x + ac_speed->x * delta_t - my_pos->x) - (form[i].east - form[ti_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_a += (ac->z + ac_speed->z * delta_t - my_pos->z) - (form[i].alt - form[ti_acs_id[AC_ID]].alt);
form_speed += acInfoGetGspeed(ti_acs[i].ac_id);
//form_speed_e += ac->gspeed * sinf(ac->course);
//form_speed_n += ac->gspeed * cosf(ac->course);
++nb;
@@ -224,21 +217,21 @@ int formation_flight(void)
if (AC_ID == leader_id) {
alt = nav_altitude;
} 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;
flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);
alt += formation[ti_acs_id[AC_ID]].alt + coef_form_alt * form_a;
flight_altitude = Max(alt, SECURITY_ALT);
// carrot
if (AC_ID != leader_id) {
float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
desired_x = leader->east + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx;
desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
float dx = form[ti_acs_id[AC_ID]].east - form[ti_acs_id[leader_id]].east;
float dy = form[ti_acs_id[AC_ID]].north - form[ti_acs_id[leader_id]].north;
desired_x = leader_pos->x + NOMINAL_AIRSPEED * form_carrot * sinf(acInfoGetCourse(leader_id)) + dx;
desired_y = leader_pos->y + NOMINAL_AIRSPEED * form_carrot * cosf(acInfoGetCourse(leader_id)) + dy;
// fly to desired
fly_to_xy(desired_x, desired_y);
desired_x = leader->east + dx;
desired_y = leader->north + dy;
desired_x = leader_pos->x + dx;
desired_y = leader_pos->y + dy;
// lateral correction
//float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
//float diff_course = leader->course - hspeed_dir;
@@ -261,8 +254,8 @@ int formation_flight(void)
void formation_pre_call(void)
{
if (leader_id == AC_ID) {
stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north;
stateGetPositionEnu_f()->x -= formation[ti_acs_id[AC_ID]].east;
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
#define FORMATION_H
#include "firmwares/fixedwing/nav.h"
#include "subsystems/navigation/traffic_info.h"
#include "subsystems/datalink/datalink.h" // dl_buffer
#include "generated/airframe.h" // AC_ID
#include "modules/multi/traffic_info.h"
#define FORM_MODE_GLOBAL 0
#define FORM_MODE_COURSE 1
@@ -27,45 +28,45 @@ struct slot_ {
float alt;
};
extern struct slot_ formation[NB_ACS];
extern struct slot_ formation[];
extern int formation_init(void);
extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
#define UpdateSlot(_id, _se, _sn, _sa) { \
formation[the_acs_id[_id]].east = _se; \
formation[the_acs_id[_id]].north = _sn; \
formation[the_acs_id[_id]].alt = _sa; \
}
static inline void updateSlot(uint8_t id, float se, float sn, float sa)
{
formation[ti_acs_id[id]].east = se;
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() { \
uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer); \
uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer); \
uint8_t status = DL_FORMATION_STATUS_status(dl_buffer); \
if (ac_id == AC_ID) leader_id = leader; \
else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); } \
else { UpdateFormationStatus(ac_id,UNSET); } \
}
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 status = DL_FORMATION_STATUS_status(dl_buffer);
if (ac_id == AC_ID) { leader_id = leader; }
else if (leader == leader_id) { updateFormationStatus(ac_id, status); }
else { updateFormationStatus(ac_id, UNSET); }
}
#define ParseFormationSlot() { \
uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer); \
uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer); \
float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer); \
float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer); \
float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer); \
UpdateSlot(ac_id, slot_east, slot_north, slot_alt); \
if (ac_id == leader_id) form_mode = mode; \
}
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);
float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer);
float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer);
float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer);
updateSlot(ac_id, slot_east, slot_north, slot_alt);
if (ac_id == leader_id) { form_mode = mode; }
}
extern int start_formation(void);
extern int stop_formation(void);
extern int formation_flight(void);
extern void formation_pre_call(void);
#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.north = 0.;
potential_force.alt = 0.;
potential_force.speed = 0.;
potential_force.climb = 0.;
force_pos_gain = FORCE_POS_GAIN;
force_speed_gain = FORCE_SPEED_GAIN;
@@ -67,25 +69,24 @@ int potential_task(void)
// compute control forces
int8_t nb = 0;
for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
if (ti_acs[i].ac_id == AC_ID) { continue; }
struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ti_acs[i].ac_id);
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 (delta_t > CARROT) { continue; }
else {
float sha = sinf(ac->course);
float cha = cosf(ac->course);
float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x;
float de = ac->x + ac_speed->x * delta_t - stateGetPositionEnu_f()->x;
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; }
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; }
float dist = sqrtf(de * de + dn * dn + da * da);
if (dist == 0.) { continue; }
float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float dve = stateGetHorizontalSpeedNorm_f() * sh - ac_speed->x;
float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac_speed->y;
float dva = stateGetSpeedEnu_f()->z - ac_speed->z;
float scal = dve * de + dvn * dn + dva * da;
if (scal < 0.) { continue; } // No risk of collision
float d3 = dist * dist * dist;
@@ -96,9 +97,9 @@ int potential_task(void)
}
}
if (nb == 0) { return true; }
//potential_force.east /= nb;
//potential_force.north /= nb;
//potential_force.alt /= nb;
potential_force.east /= nb;
potential_force.north /= nb;
potential_force.alt /= nb;
// set commands
NavVerticalAutoThrottleMode(0.);
@@ -128,4 +129,3 @@ int potential_task(void)
return true;
}
sw/airborne/modules/multi/potential.h
+1 -1
View File
@@ -8,7 +8,7 @@
#define POTENTIAL_H
#include "firmwares/fixedwing/nav.h"
#include "subsystems/navigation/traffic_info.h"
#include "modules/multi/traffic_info.h"
struct force_ {
float east;
sw/airborne/modules/multi/tcas.c
+54 -35
View File
@@ -26,13 +26,10 @@
*/
#include "multi/tcas.h"
#include "generated/airframe.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "subsystems/gps.h"
#include "generated/flight_plan.h"
#include "generated/flight_plan.h" // SECURITY_ALT
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
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
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 */
#define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 )
void tcas_init(void)
{
tcas_alt_setpoint = GROUND_ALT + SECURITY_HEIGHT;
tcas_alt_setpoint = SECURITY_ALT;
tcas_tau_ta = TCAS_TAU_TA;
tcas_tau_ra = TCAS_TAU_RA;
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)
{
struct ac_info_ * ac = get_ac_info(id);
float dz = ac->alt - stateGetPositionUtm_f()->alt;
struct EnuCoor_f *ac = acInfoGetPositionEnu_f(id);
float dz = ac->z - stateGetPositionEnu_f()->z;
if (dz > tcas_alim / 2) { return RA_DESCEND; }
else if (dz < -tcas_alim / 2) { return RA_CLIMB; }
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)
{
// no TCAS under security_height
if (stateGetPositionUtm_f()->alt < GROUND_ALT + SECURITY_HEIGHT) {
if (stateGetPositionUtm_f()->alt < SECURITY_ALT) {
uint8_t i;
for (i = 0; i < NB_ACS; i++) { tcas_acs_status[i].status = TCAS_NO_ALARM; }
return;
@@ -114,19 +129,19 @@ void tcas_periodic_task_1Hz(void)
float vx = stateGetHorizontalSpeedNorm_f() * sinf(stateGetHorizontalSpeedDir_f());
float vy = stateGetHorizontalSpeedNorm_f() * cosf(stateGetHorizontalSpeedDir_f());
for (i = 2; i < NB_ACS; i++) {
if (the_acs[i].ac_id == 0) { continue; } // no AC data
uint32_t dt = gps.tow - the_acs[i].itow;
if (ti_acs[i].ac_id == 0) { continue; } // no AC data
uint32_t dt = gps.tow - ti_acs[i].itow;
if (dt > 3 * TCAS_DT_MAX) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
continue;
}
if (dt > TCAS_DT_MAX) { continue; } // lost com but keep current status
float dx = the_acs[i].east - stateGetPositionEnu_f()->x;
float dy = the_acs[i].north - stateGetPositionEnu_f()->y;
float dz = the_acs[i].alt - stateGetPositionUtm_f()->alt;
float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float dx = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->x - stateGetPositionEnu_f()->x;
float dy = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->y - stateGetPositionEnu_f()->y;
float dz = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->z - stateGetPositionEnu_f()->z;
float dvx = vx - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->x;
float dvy = vy - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->y;
float dvz = stateGetSpeedEnu_f()->z - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->z;
float scal = dvx * dx + dvy * dy + dvz * dz;
float ddh = dx * dx + dy * dy;
float ddv = dz * dz;
@@ -140,60 +155,62 @@ void tcas_periodic_task_1Hz(void)
if (tau >= TCAS_HUGE_TAU && !inside) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
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;
case TCAS_TA:
if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // TA -> RA
// Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
//test_dir = tcas_test_direction(ti_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(ti_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
break;
}
if (tau > tcas_tau_ta && !inside) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
}
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;
case TCAS_NO_ALARM:
if (tau < tcas_tau_ta || inside) {
tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
// 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) {
tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
// Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
//test_dir = tcas_test_direction(ti_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(ti_acs[i].ac_id),&test_dir);
}
break;
default:
break;
}
// store closest AC
if (tau < tau_min) {
tau_min = tau;
ac_id_close = the_acs[i].ac_id;
ac_id_close = ti_acs[i].ac_id;
}
}
// 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
}
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
if (tcas_status == TCAS_RA) {
tcas_ac_RA = ac_id_close;
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 == tcas_resolve) { // same direction, lowest id go down
if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; }
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
if (ac_resolve != RA_LEVEL) { // message received
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 { // no message
if (tcas_resolve == RA_CLIMB && the_acs[the_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
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 && ti_acs[ti_acs_id[tcas_ac_RA]].climb < -1.0) { tcas_resolve = RA_CLIMB; } // revert resolve
}
}
// Downlink alert
@@ -219,22 +236,24 @@ void tcas_periodic_task_1Hz(void)
void tcas_periodic_task_4Hz(void)
{
// set alt setpoint
if (stateGetPositionUtm_f()->alt > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
if (stateGetPositionUtm_f()->alt > SECURITY_ALT && tcas_status == TCAS_RA) {
struct EnuCoor_f *ac = acInfoGetPositionEnu_f(tcas_ac_RA);
switch (tcas_resolve) {
case RA_CLIMB :
tcas_alt_setpoint = Max(nav_altitude, ac->alt + tcas_alim);
tcas_alt_setpoint = Max(nav_altitude, ac->z + tcas_alim);
break;
case RA_DESCEND :
tcas_alt_setpoint = Min(nav_altitude, ac->alt - tcas_alim);
tcas_alt_setpoint = Min(nav_altitude, ac->z - tcas_alim);
break;
case RA_LEVEL :
case RA_NONE :
tcas_alt_setpoint = nav_altitude;
break;
default:
break;
}
// Bound alt
tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint);
tcas_alt_setpoint = Max(SECURITY_ALT, tcas_alt_setpoint);
} else {
tcas_alt_setpoint = nav_altitude;
tcas_resolve = RA_NONE;
sw/airborne/modules/multi/tcas.h
+8 -9
View File
@@ -29,7 +29,10 @@
#define TCAS_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_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_4Hz(void);
#define CallTCAS() { if (tcas_status == TCAS_RA) v_ctl_altitude_setpoint = tcas_alt_setpoint; }
extern void callTCAS(void);
#define ParseTcasResolve() { \
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[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(dl_buffer); \
} \
}
extern void parseTcasResolve(void);
extern void parseTcasRA(void);
#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 "pprzlink/messages.h"
#include "pprzlink/dl_protocol.h"
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
#include "subsystems/radio_control.h"
@@ -45,16 +44,10 @@
#include "subsystems/gps.h"
#endif
#ifdef TRAFFIC_INFO
#include "subsystems/navigation/traffic_info.h"
#endif
#ifdef RADIO_CONTROL_DATALINK_LED
#include "led.h"
#endif
#define MOfCm(_x) (((float)(_x))/100.)
#if USE_NPS
bool datalink_enabled = true;
#endif
@@ -67,156 +60,77 @@ void dl_parse_msg(void)
/* parse telemetry messages coming from other AC */
if (sender_id != 0) {
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: {
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 */
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
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;
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
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) {
case DL_RC_3CH :
#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;
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
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
#if USE_GPS
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
// GPS parse data
gps_inject_data(
DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer)
);
}
break;
// GPS parse data
gps_inject_data(
DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer)
);
}
break;
#endif // USE_GPS
#ifdef TRAFFIC_INFO
case DL_ACINFO: {
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);
default:
break;
}
break;
#endif
default:
break;
}
/* Parse firmware specific datalink */
firmware_parse_msg();
@@ -225,7 +139,7 @@ void dl_parse_msg(void)
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)
{
}
}
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
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 pos = LL.utm_of WGS84 a.pos in
let ac_info = ["ac_id", PprzLink.String ac;
"utm_east", cm_of_m_32 pos.utm_x;
"utm_north", cm_of_m_32 pos.utm_y;
"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" ac_info;
if a.vehicle_type = FixedWing then
let pos = LL.utm_of WGS84 a.pos in
let ac_info = ["ac_id", PprzLink.String ac;
"utm_east", cm_of_m_32 pos.utm_x;
"utm_north", cm_of_m_32 pos.utm_y;
"utm_zone", PprzLink.Int pos.utm_zone;
"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" 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;
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 DeciDegOfRad(x) ((x) * (1800./ M_PI))
#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 Max(x,y) (x > y ? x : y)
@@ -109,114 +113,115 @@ typedef uint8_t unit_t;
#define BoundWrapped(_x, _min, _max) { \
if ((_max) > (_min)) \
Bound(_x, _min, _max) \
else \
BoundInverted(_x, _min, _max) \
}
else \
BoundInverted(_x, _min, _max) \
}
#define BoundAbs(_x, _max) Bound(_x, -(_max), (_max))
#define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) )
#define ChopAbs(x, max) Chop(x, -(max), (max))
#define DeadBand(_x, _v) { \
if (_x > (_v)) \
_x = _x -(_v); \
else if (_x < -(_v)) \
_x = _x +(_v); \
else \
_x = 0; \
#define DeadBand(_x, _v) { \
if (_x > (_v)) \
_x = _x -(_v); \
else if (_x < -(_v)) \
_x = _x +(_v); \
else \
_x = 0; \
}
#define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b))
#define RunOnceEvery(_prescaler, _code) { \
static uint16_t prescaler = 0; \
prescaler++; \
if (prescaler >= _prescaler) { \
prescaler = 0; \
_code; \
} \
#define RunOnceEvery(_prescaler, _code) { \
static uint16_t prescaler = 0; \
prescaler++; \
if (prescaler >= _prescaler) { \
prescaler = 0; \
_code; \
} \
}
#define RunXTimesEvery(_jumpstart, _prescaler, _interval, _xtimes, _code) { \
static uint16_t prescaler = _jumpstart; \
static uint16_t xtimes = 0; \
prescaler++; \
if (prescaler >= _prescaler + _interval*xtimes && xtimes < _xtimes) { \
_code; \
xtimes++; \
} \
if (xtimes >= _xtimes) { \
xtimes = 0; \
prescaler = 0; \
} \
}
#define RunXTimesEvery(_jumpstart, _prescaler, _interval, _xtimes, _code) { \
static uint16_t prescaler = _jumpstart; \
static uint16_t xtimes = 0; \
prescaler++; \
if (prescaler >= _prescaler + _interval*xtimes && xtimes < _xtimes) { \
_code; \
xtimes++; \
} \
if (xtimes >= _xtimes) { \
xtimes = 0; \
prescaler = 0; \
} \
}
#define PeriodicPrescaleBy5( _code_0, _code_1, _code_2, _code_3, _code_4) { \
static uint8_t _50hz = 0; \
_50hz++; \
if (_50hz >= 5) _50hz = 0; \
switch (_50hz) { \
case 0: \
_code_0; \
break; \
case 1: \
_code_1; \
break; \
case 2: \
_code_2; \
break; \
case 3: \
_code_3; \
break; \
case 4: \
_code_4; \
break; \
} \
static uint8_t _50hz = 0; \
_50hz++; \
if (_50hz >= 5) _50hz = 0; \
switch (_50hz) { \
case 0: \
_code_0; \
break; \
case 1: \
_code_1; \
break; \
case 2: \
_code_2; \
break; \
case 3: \
_code_3; \
break; \
case 4: \
_code_4; \
break; \
} \
}
#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; \
_cnt++; \
if (_cnt >= 10) _cnt = 0; \
switch (_cnt) { \
case 0: \
_code_0; \
break; \
case 1: \
_code_1; \
break; \
case 2: \
_code_2; \
break; \
case 3: \
_code_3; \
break; \
case 4: \
_code_4; \
break; \
case 5: \
_code_5; \
break; \
case 6: \
_code_6; \
break; \
case 7: \
_code_7; \
break; \
case 8: \
_code_8; \
break; \
case 9: \
default: \
_code_9; \
break; \
} \
static uint8_t _cnt = 0; \
_cnt++; \
if (_cnt >= 10) _cnt = 0; \
switch (_cnt) { \
case 0: \
_code_0; \
break; \
case 1: \
_code_1; \
break; \
case 2: \
_code_2; \
break; \
case 3: \
_code_3; \
break; \
case 4: \
_code_4; \
break; \
case 5: \
_code_5; \
break; \
case 6: \
_code_6; \
break; \
case 7: \
_code_7; \
break; \
case 8: \
_code_8; \
break; \
case 9: \
default: \
_code_9; \
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;
while (!(a[i] == 0 && b[i] == 0)) {
if (a[i] != b[i]) return FALSE;
if (a[i] != b[i]) { return FALSE; }
i++;
}
return TRUE;