Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[module] Distributed Formation Control

Browse Source 
integration of #211
This commit is contained in:
Hector Garcia de Marina
2017-12-14 22:16:26 +01:00
committed by Gautier Hattenberger
parent 86101eca4d
commit b932515610
4 changed files with 358 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/distributed_circular_formation.xml
+46
View File
@@ -0,0 +1,46 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="distributed_circular_formation" dir="dcf">
<doc>
<description>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
</description>
<section name="Parameters" prefix="DCF_">
<define name="MAX_NEIGHBORS" value="4" description="Maximum number of accepted neighbors for an aircraft"/>
<define name="GAIN_K" value="10" description="Control gain for the algorithm. It sets the possible maximum and minimum radius of the circle to be tracked"/>
<define name="RADIUS" value="80" description="Radius for the desired steady-state circle"/>
<define name="TIMEOUT" value="1500" description="After this time (in ms) if we do not have any msg from a neighborh, we ignore it"/>
<define name="BROAD_TIME" value="200" description="Time in ms for transmiting theta to your neighbors"/>
</section>
</doc>
<settings name="DCF">
<dl_settings>
<dl_settings NAME="DCF">
<dl_settings NAME="Control">
<dl_setting MAX="20" MIN="0" STEP="0.2" VAR="dcf_control.k" shortname = "Gain" param="DCF_GAIN_K"/>
<dl_setting MAX="200" MIN="0" STEP="1" VAR="dcf_control.radius" shortname = "Radius" param="DCF_RADIUS"/>
<dl_setting MAX="5000" MIN="0" STEP="1" VAR="dcf_control.timeout" shortname = "Timeout" param="DCF_TIMEOUT"/>
<dl_setting MAX="1000" MIN="0" STEP="1" VAR="dcf_control.broadtime" shortname = "Broadcasting" param="DCF_BROAD_TIME"/>
</dl_settings>
</dl_settings>
</dl_settings>
</settings>
<depends>gvf_module.xml</depends>
<header>
<file name="dcf.h"/>
</header>
<init fun="dcf_init()"/>
<datalink message="DCF_REG_TABLE" fun="parseRegTable()"/>
<datalink message="DCF_THETA" fun="parseThetaTable()"/>
<makefile firmware="fixedwing">
<file name="dcf.c"/>
</makefile>
</module>
sw/airborne/modules/dcf/dcf.c
+169
View File
@@ -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
* <http://www.gnu.org/licenses/>.
*
*/
#include <math.h>
#include <std.h>
#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;
}
}
sw/airborne/modules/dcf/dcf.h
+62
View File
@@ -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
* <http://www.gnu.org/licenses/>.
*
*/
/** @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
sw/ground_segment/python/gvf/dcfInitTables.py
+81
View File
@@ -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()