diff --git a/conf/modules/distributed_circular_formation.xml b/conf/modules/distributed_circular_formation.xml new file mode 100644 index 0000000000..9c98bb6955 --- /dev/null +++ b/conf/modules/distributed_circular_formation.xml @@ -0,0 +1,46 @@ + + + + + Distributed algorithm for circular formations with air-to-air communications. + For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field + +
+ + + + + +
+ + + + + + + + + + + + + + + + gvf_module.xml + +
+ +
+ + + + + + + + + + + + diff --git a/sw/airborne/modules/dcf/dcf.c b/sw/airborne/modules/dcf/dcf.c new file mode 100644 index 0000000000..a39ec480d3 --- /dev/null +++ b/sw/airborne/modules/dcf/dcf.c @@ -0,0 +1,169 @@ +/* + * Copyright (C) 2017 Hector Garcia de Marina + * + * 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 + * . + * + */ + +#include +#include + +#include "modules/dcf/dcf.h" +#include "subsystems/datalink/datalink.h" // dl_buffer +#include "subsystems/datalink/telemetry.h" +#include "subsystems/navigation/common_nav.h" +#include "autopilot.h" +#include "std.h" + +#if PERIODIC_TELEMETRY +static void send_dcf(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_DCF(trans, dev, AC_ID, 4 * DCF_MAX_NEIGHBORS, &(dcf_tables.tableNei[0][0]), DCF_MAX_NEIGHBORS, + dcf_tables.error_sigma); +} +#endif // PERIODIC TELEMETRY + +// Control +/*! Default gain k for the algorithm */ +#ifndef DCF_GAIN_K +#define DCF_GAIN_K 10 +#endif +/*! Default radius for the circumference */ +#ifndef DCF_RADIUS +#define DCF_RADIUS 80 +#endif +/*! Default timeout for the neighbors' information */ +#ifndef DCF_TIMEOUT +#define DCF_TIMEOUT 1500 +#endif +/*! Default broadcasting time */ +#ifndef DCF_BROADTIME +#define DCF_BROADTIME 200 +#endif + +struct dcf_con dcf_control = {DCF_GAIN_K, DCF_RADIUS, DCF_TIMEOUT, 0, DCF_BROADTIME}; +struct dcf_tab dcf_tables; + +uint32_t last_transmision = 0; + +void dcf_init(void) +{ + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) { + dcf_tables.tableNei[i][0] = -1; + dcf_tables.error_sigma[i] = 0; + } + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DCF, send_dcf); +#endif +} + +bool distributed_circular(uint8_t wp) +{ + float xc = waypoints[wp].x; + float yc = waypoints[wp].y; + struct EnuCoor_f *p = stateGetPositionEnu_f(); + float x = p->x; + float y = p->y; + float u = 0; + + dcf_control.theta = atan2f(y - yc, x - xc); + + uint32_t now = get_sys_time_msec(); + + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) { + if (dcf_tables.tableNei[i][0] != -1) { + uint32_t timeout = now - dcf_tables.last_theta[i]; + if (timeout > dcf_control.timeout) { + dcf_tables.tableNei[i][3] = dcf_control.timeout; + } else { + dcf_tables.tableNei[i][3] = (uint16_t)timeout; + + float e = dcf_control.theta - (dcf_tables.tableNei[i][1] + (dcf_tables.tableNei[i][2])) * M_PI / 1800.0; + NormRadAngle(e); + u += e; + dcf_tables.error_sigma[i] = (uint16_t)(e * 1800.0 / M_PI); + } + } + } + + u *= -gvf_control.s * dcf_control.k; + + gvf_ellipse_XY(xc, yc, dcf_control.radius + u, dcf_control.radius + u, 0); + + if ((now - last_transmision > dcf_control.broadtime) && (autopilot_get_mode() == AP_MODE_AUTO2)) { + send_theta_to_nei(); + last_transmision = now; + } + + return true; +} + +void send_theta_to_nei(void) +{ + struct pprzlink_msg msg; + + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) + if (dcf_tables.tableNei[i][0] != -1) { + msg.trans = &(DefaultChannel).trans_tx; + msg.dev = &(DefaultDevice).device; + msg.sender_id = AC_ID; + msg.receiver_id = dcf_tables.tableNei[i][0]; + msg.component_id = 0; + pprzlink_msg_send_DCF_THETA(&msg, &(dcf_control.theta)); + } +} + +void parseRegTable(void) +{ + uint8_t ac_id = DL_DCF_REG_TABLE_ac_id(dl_buffer); + if (ac_id == AC_ID) { + uint8_t nei_id = DL_DCF_REG_TABLE_nei_id(dl_buffer); + int16_t desired_sigma = DL_DCF_REG_TABLE_desired_sigma(dl_buffer); + + if (nei_id == 0) { + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) { + dcf_tables.tableNei[i][0] = -1; + } + } else { + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) + if (dcf_tables.tableNei[i][0] == (int16_t)nei_id) { + dcf_tables.tableNei[i][0] = (int16_t)nei_id; + dcf_tables.tableNei[i][2] = desired_sigma; + return; + } + + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) + if (dcf_tables.tableNei[i][0] == -1) { + dcf_tables.tableNei[i][0] = (int16_t)nei_id; + dcf_tables.tableNei[i][2] = desired_sigma; + return; + } + } + } +} + +void parseThetaTable(void) +{ + int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(dl_buffer)); + for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) + if (dcf_tables.tableNei[i][0] == sender_id) { + dcf_tables.last_theta[i] = get_sys_time_msec(); + dcf_tables.tableNei[i][1] = (int16_t)((DL_DCF_THETA_theta(dl_buffer)) * 1800 / M_PI); + break; + } +} diff --git a/sw/airborne/modules/dcf/dcf.h b/sw/airborne/modules/dcf/dcf.h new file mode 100644 index 0000000000..7ff7810c5e --- /dev/null +++ b/sw/airborne/modules/dcf/dcf.h @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2017 Hector Garcia de Marina + * + * 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 + * . + * + */ + +/** @file dcf.h + * + * Distributed circular formation algorithm + */ + +#ifndef DCF_H +#define DCF_H + +#include "std.h" + +/*! Default number of neighbors per aircraft */ +#ifndef DCF_MAX_NEIGHBORS +#define DCF_MAX_NEIGHBORS 4 +#endif + +struct dcf_con { + float k; + float radius; + uint16_t timeout; + float theta; + uint16_t broadtime; +}; + +extern struct dcf_con dcf_control; + +struct dcf_tab { + int16_t tableNei[DCF_MAX_NEIGHBORS][4]; + int16_t error_sigma[DCF_MAX_NEIGHBORS]; + uint32_t last_theta[DCF_MAX_NEIGHBORS]; +}; + +extern struct dcf_tab dcf_tables; + +extern void dcf_init(void); +extern bool distributed_circular(uint8_t wp); +extern void send_theta_to_nei(void); + +extern void parseRegTable(void); +extern void parseThetaTable(void); + +#endif // DCF_H diff --git a/sw/ground_segment/python/gvf/dcfInitTables.py b/sw/ground_segment/python/gvf/dcfInitTables.py new file mode 100644 index 0000000000..75eff018d9 --- /dev/null +++ b/sw/ground_segment/python/gvf/dcfInitTables.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +from __future__ import print_function + +import time +import sys +import wx +import numpy as np +import sys +from os import path, getenv +PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) +PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) +sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_HOME + "/var/lib/python") +from pprzlink.ivy import IvyMessagesInterface +from pprzlink.message import PprzMessage +from settings_xml_parse import PaparazziACSettings + +list_ids = [] +interface = IvyMessagesInterface("DCF") + +if len(sys.argv) != 4: + print("Usage: dcfInitTables topology.txt desired_sigma.txt ids.txt") + interface.shutdown() + exit() + +B = np.loadtxt(sys.argv[1]) +desired_sigmas = np.loadtxt(sys.argv[2])*np.pi/180.0 +ids = np.loadtxt(sys.argv[3]) + +list_ids = np.ndarray.tolist(ids) + +if np.size(ids) != np.size(B,0): + print("The ammount of aircrafts in the topology and ids do not match") + interface.shutdown() + exit() +time.sleep(2) + +if len(list_ids) == 2: + B.shape = (2,1) + +for count, column in enumerate(B.T): + index = np.nonzero(column) + i = index[0] + + msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE") + msg_clean_a['ac_id'] = int(list_ids[i[0]]) + msg_clean_a['nei_id'] = 0 + msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE") + msg_clean_b['ac_id'] = int(list_ids[i[1]]) + msg_clean_b['nei_id'] = 0 + + interface.send(msg_clean_a) + interface.send(msg_clean_b) + +for count, column in enumerate(B.T): + index = np.nonzero(column) + i = index[0] + + msga = PprzMessage("datalink", "DCF_REG_TABLE") + msga['ac_id'] = int(list_ids[i[0]]) + msga['nei_id'] = int(list_ids[i[1]]) + if len(list_ids) == 2: + msga['desired_sigma'] = int(desired_sigmas) + else: + msga['desired_sigma'] = int(desired_sigmas[count]) + interface.send(msga) + + msgb = PprzMessage("datalink", "DCF_REG_TABLE") + msgb['ac_id'] = int(list_ids[i[1]]) + msgb['nei_id'] = int(list_ids[i[0]]) + if len(list_ids) == 2: + msgb['desired_sigma'] = int(desired_sigmas) + else: + msgb['desired_sigma'] = int(desired_sigmas[count]) + interface.send(msgb) + + print(msga) + print(msgb) + +time.sleep(2) +interface.shutdown()