Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ctc_target.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Hector Garcia de Marina
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
23 #include <math.h>
24 #include <std.h>
25 #include <stdio.h>
26 
28 #include "subsystems/datalink/datalink.h" // dl_buffer
31 #include "autopilot.h"
32 
34 #ifndef CTC_MAX_AC
35 #define CTC_MAX_AC 4
36 #endif
37 
39 
40 void ctc_target_init(void)
41 {
42  for (int i = 0; i < CTC_MAX_AC; i++) {
43  tableNei[i][0] = -1;
44  }
45 }
46 
48 {
49  struct EnuCoor_f *v = stateGetSpeedEnu_f();
50  struct EnuCoor_f *p = stateGetPositionEnu_f();
51 
52  float vx = v->x;
53  float vy = v->y;
54  float px = p->x;
55  float py = p->y;
56 
57  struct pprzlink_msg msg;
58 
59  for (int i = 0; i < CTC_MAX_AC; i++)
60  if (tableNei[i][0] != -1) {
61  msg.trans = &(DefaultChannel).trans_tx;
62  msg.dev = &(DefaultDevice).device;
63  msg.sender_id = AC_ID;
64  msg.receiver_id = tableNei[i][0];
65  msg.component_id = 0;
66  pprzlink_msg_send_CTC_INFO_FROM_TARGET(&msg, &px, &py, &vx, &vy);
67  }
68 }
69 
71 {
72  uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(dl_buffer);
73  if (ac_id == AC_ID) {
74  uint8_t nei_id = DL_CTC_REG_TABLE_nei_id(dl_buffer);
75  for (int i = 0; i < CTC_MAX_AC; i++)
76  if (tableNei[i][0] == -1) {
77  tableNei[i][0] = (int16_t)nei_id;
78  return;
79  }
80  }
81 }
82 
84 {
85  uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(dl_buffer);
86  if (ac_id == AC_ID)
87  for (int i = 0; i < CTC_MAX_AC; i++) {
88  tableNei[i][0] = -1;
89  }
90 }
91 
uint8_t ac_id
Definition: sim_ap.c:45
Collective Tracking Control (module for the target to be tracked)
Periodic telemetry system header (includes downlink utility and generated code).
#define CTC_MAX_AC
Definition: ctc_target.c:35
vector in East North Up coordinates Units: meters
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
void ctc_target_send_info_to_nei(void)
Definition: ctc_target.c:47
void parse_ctc_target_CleanTable(void)
Definition: ctc_target.c:83
float x
in meters
signed short int16_t
Definition: types.h:17
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
void ctc_target_init(void)
Definition: ctc_target.c:40
unsigned char uint8_t
Definition: types.h:14
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
int16_t tableNei[CTC_MAX_AC][6]
Definition: ctc_target.c:38
static float p[2][2]
float y
in meters
void parse_ctc_target_RegTable(void)
Definition: ctc_target.c:70