| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,169 @@ | ||
| /* | ||
| * Copyright (C) 2021 Guido de Croon <g.c.h.e.decroon@tudelft.nl> | ||
| * | ||
| * 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/ins/ins_flow.h | ||
| * | ||
| * "Inertial" navigation system. | ||
| */ | ||
|
|
||
| #ifndef INS_FLOW_H | ||
| #define INS_FLOW_H | ||
|
|
||
| #ifdef __cplusplus | ||
| extern "C" { | ||
| #endif | ||
|
|
||
| #define CONSTANT_ALT_FILTER 1 | ||
| #define OF_DRAG 1 | ||
| // Only for constant alt for now! | ||
| #define OF_TWO_DIM 1 | ||
| // Only for changing alt | ||
| #define OF_THRUST_BIAS 0 | ||
| // Whether to use gyros: | ||
| #define OF_USE_GYROS 1 | ||
| // Predicting gyros with roll command and optic flow (normally 0): | ||
| #define PREDICT_GYROS 0 | ||
|
|
||
| #if CONSTANT_ALT_FILTER == 1 | ||
|
|
||
| #if OF_TWO_DIM == 0 | ||
|
|
||
|
|
||
| #define OF_V_IND 0 | ||
| #define OF_ANGLE_IND 1 | ||
| #define OF_Z_IND 2 | ||
| #define OF_ANGLE_DOT_IND 3 | ||
| #if OF_USE_GYROS == 1 | ||
| #define N_MEAS_OF_KF 3 | ||
| #else | ||
| // gyros not used at all | ||
| #define N_MEAS_OF_KF 2 | ||
| #endif | ||
|
|
||
| #define OF_THETA_IND -1 | ||
| #define OF_VX_IND -1 | ||
|
|
||
| #if OF_THRUST_BIAS == 0 | ||
| #define N_STATES_OF_KF 4 | ||
| #define OF_THRUST_BIAS_IND -1 | ||
| #else | ||
| // does this work with thrust bias? | ||
| #define N_STATES_OF_KF 5 | ||
| #define OF_THRUST_BIAS_IND 4 | ||
| #endif | ||
|
|
||
| #else | ||
| #define N_STATES_OF_KF 6 | ||
| #define OF_V_IND 0 | ||
| #define OF_ANGLE_IND 1 | ||
| #define OF_Z_IND 2 | ||
| #define OF_THETA_IND 3 | ||
| #define OF_VX_IND 4 | ||
| #define OF_ANGLE_DOT_IND 5 | ||
| // TODO: also a theta dot ind? | ||
|
|
||
|
|
||
| #define OF_THRUST_BIAS_IND -1 | ||
|
|
||
| // the third measurement here is the other lateral flow: | ||
| #if OF_USE_GYROS == 1 | ||
| // gyros used in the prediction and measurement | ||
| #define N_MEAS_OF_KF 4 | ||
| #else | ||
| // gyros not used at all | ||
| #define N_MEAS_OF_KF 3 | ||
| #endif | ||
| #endif | ||
|
|
||
| #define OF_Z_DOT_IND -1 | ||
| #else | ||
| #if OF_THRUST_BIAS == 0 | ||
| #define N_STATES_OF_KF 5 | ||
| #define OF_THRUST_BIAS_IND -1 | ||
| #else | ||
| #define N_STATES_OF_KF 6 | ||
| #define OF_THRUST_BIAS_IND 5 | ||
| #endif | ||
|
|
||
| #define OF_V_IND 0 | ||
| #define OF_ANGLE_IND 1 | ||
| #define OF_ANGLE_DOT_IND 2 | ||
| #define OF_Z_IND 3 | ||
| #define OF_Z_DOT_IND 4 | ||
|
|
||
| #define OF_THETA_IND -1 | ||
| #define OF_VX_IND -1 | ||
|
|
||
|
|
||
| #if OF_USE_GYROS == 1 | ||
| // gyros used in the prediction and measurement | ||
| #define N_MEAS_OF_KF 3 | ||
| #else | ||
| // gyros not used at all | ||
| #define N_MEAS_OF_KF 2 | ||
| #endif | ||
|
|
||
| #endif | ||
|
|
||
| // TODO: make these parameters in the estimation scheme: | ||
| #define OF_TB_Q 0.02 | ||
| #define OF_TB_P 0.5 | ||
|
|
||
|
|
||
| #define OF_LAT_FLOW_IND 0 | ||
| #define OF_DIV_FLOW_IND 1 | ||
| #define OF_RATE_IND 2 | ||
| #if OF_USE_GYROS == 1 | ||
| #define OF_LAT_FLOW_X_IND 3 | ||
| #else | ||
| #define OF_LAT_FLOW_X_IND 2 | ||
| #endif | ||
|
|
||
|
|
||
| // use filter to different extents: | ||
| #define USE_ANGLE 1 | ||
| #define USE_VELOCITY 2 | ||
| #define USE_HEIGHT 3 | ||
|
|
||
|
|
||
| #include "modules/ahrs/ahrs.h" | ||
| #include "modules/ahrs/ahrs_int_cmpl_quat.h" | ||
| #include "modules/ahrs/ahrs_aligner.h" | ||
| #include "modules/ins/ins.h" | ||
|
|
||
| extern void ins_flow_init(void); | ||
| extern void ins_flow_update(void); | ||
|
|
||
| extern float OF_X[N_STATES_OF_KF]; | ||
| extern bool reset_filter; | ||
| extern bool run_filter; | ||
| extern int use_filter; | ||
| extern float thrust_factor; | ||
| extern float GT_phi; | ||
| extern float GT_theta; | ||
|
|
||
|
|
||
| #ifdef __cplusplus | ||
| } | ||
| #endif | ||
|
|
||
| #endif /* INS_FLOW_H */ |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,61 @@ | ||
| /* | ||
| * Copyright (C) 2023 Ewoud Smeur | ||
| * | ||
| * 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. | ||
| * | ||
| */ | ||
|
|
||
| #include "state.h" | ||
|
|
||
| float g = -9.81f; | ||
|
|
||
| struct FloatVect2 ballistic_pos; | ||
|
|
||
| void ballistic_touchdown_init(void) { | ||
| // nothing to be done here | ||
| } | ||
|
|
||
| /** | ||
| * Function that predicts the ballistic crash location | ||
| * | ||
| * Uses ENU coordinates (vertical up!) | ||
| **/ | ||
| void ballistic_touchdown_run(void) { | ||
|
|
||
| struct EnuCoor_f * v = stateGetSpeedEnu_f(); | ||
| float vz = v->z; | ||
|
|
||
| struct FloatVect2 vh; | ||
| VECT2_ASSIGN(vh, v->x, v->y); | ||
|
|
||
| float h = fabsf(stateGetPositionEnu_f()->z); // Should be height above ground, make sure to initialize local frame on ground | ||
|
|
||
| // With h always larger than 0, the sqrt can never give nan | ||
| float time_fall = (-vz - sqrtf(vz*vz -2.f*h*g))/g; | ||
|
|
||
| struct FloatVect2 crash_offset; | ||
|
|
||
| VECT2_SMUL(crash_offset, vh, time_fall); | ||
|
|
||
| struct FloatVect2 pos; | ||
| pos.x = stateGetPositionEnu_f()->x; | ||
| pos.y = stateGetPositionEnu_f()->y; | ||
|
|
||
| // The predicted crash position is the current drone position + fall distance | ||
| VECT2_SUM(ballistic_pos, pos, crash_offset); | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,26 @@ | ||
| /* | ||
| * Copyright (C) 2023 Ewoud Smeur | ||
| * | ||
| * 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. | ||
| * | ||
| */ | ||
|
|
||
| void ballistic_touchdown_init(void); | ||
| void ballistic_touchdown_run(void); | ||
|
|
||
| extern struct FloatVect2 ballistic_pos; |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,70 @@ | ||
| /* | ||
| * Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com> | ||
| * | ||
| * 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/sensors/generic_uart.c" | ||
| * @author F. van Tienen | ||
| * Generic UART sensor, forwarding towards the GCS through telemetry | ||
| */ | ||
|
|
||
| #include "modules/sensors/generic_uart.h" | ||
| #include "modules/datalink/telemetry.h" | ||
| #include "mcu_periph/uart.h" | ||
|
|
||
| /* Default end character */ | ||
| #ifndef GENERIC_UART_ENDCHAR | ||
| #define GENERIC_UART_ENDCHAR '>' | ||
| #endif | ||
|
|
||
| /* Default max sending message */ | ||
| #ifndef GENERIC_UART_MAX_SENDLEN | ||
| #define GENERIC_UART_MAX_SENDLEN 64 | ||
| #endif | ||
|
|
||
| /* Default max buffer size */ | ||
| #ifndef GENERIC_UART_MAX_BUFSIZE | ||
| #define GENERIC_UART_MAX_BUFSIZE 128 | ||
| #endif | ||
|
|
||
| /* Main variables */ | ||
| static struct link_device *gen_uart_dev = (&((GENERIC_UART_PORT).device)); ///< UART device for communication | ||
|
|
||
| /* Event function to read UART message and forward to downlink */ | ||
| void generic_uart_event(void) { | ||
| // Receive buffer | ||
| static uint8_t gen_msg_buf[GENERIC_UART_MAX_BUFSIZE]; | ||
| static uint8_t gen_msg_cnt = 0; | ||
|
|
||
| // Look for data on serial port and save it in the buffer | ||
| while (gen_msg_cnt < GENERIC_UART_MAX_BUFSIZE && gen_uart_dev->char_available(gen_uart_dev->periph)) { | ||
| gen_msg_buf[gen_msg_cnt++] = gen_uart_dev->get_byte(gen_uart_dev->periph); | ||
|
|
||
| if(gen_msg_buf[gen_msg_cnt-1] == GENERIC_UART_ENDCHAR) | ||
| break; | ||
| } | ||
|
|
||
| // Forward the message to the GCS | ||
| if(gen_msg_buf[gen_msg_cnt-1] == GENERIC_UART_ENDCHAR || gen_msg_cnt > GENERIC_UART_MAX_SENDLEN) { | ||
| DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, gen_msg_cnt, gen_msg_buf); | ||
|
|
||
| gen_msg_buf[0] = 0; | ||
| gen_msg_cnt = 0; | ||
| } | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,34 @@ | ||
| /* | ||
| * Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com> | ||
| * | ||
| * 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/sensors/generic_uart.c" | ||
| * @author F. van Tienen | ||
| * Generic UART sensor, forwarding towards the GCS through telemetry | ||
| */ | ||
|
|
||
| #ifndef GENERIC_UART_H | ||
| #define GENERIC_UART_H | ||
|
|
||
| #include "std.h" | ||
|
|
||
| extern void generic_uart_event(void); | ||
|
|
||
| #endif |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,137 @@ | ||
| #!/usr/bin/env python3 | ||
| # | ||
| # Copyright (C) 2012 TUDelft | ||
| # | ||
| # 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 3 of the License, 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. If not, see <http://www.gnu.org/licenses/>. | ||
| # | ||
|
|
||
|
|
||
| import numpy as np | ||
| import matplotlib.pyplot as plt | ||
|
|
||
| import sys | ||
| import os | ||
| from datetime import datetime | ||
| import time | ||
|
|
||
| PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), | ||
| '../../../..'))) | ||
|
|
||
| PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), | ||
| '../../../..'))) | ||
|
|
||
| sys.path.append(PPRZ_HOME + "/var/lib/python") | ||
|
|
||
| from pprzlink.ivy import IvyMessagesInterface | ||
|
|
||
| class PayloadMessage(object): | ||
| def __init__(self, msg): | ||
| self.values = ''.join(chr(int(x)) for x in msg['values']) | ||
|
|
||
| class TankPlotter(object): | ||
| def __init__(self): | ||
| self.interface = IvyMessagesInterface("fueltankplotter") | ||
| self.interface.subscribe(self.message_recv) | ||
| self.start_time = time.mktime(datetime.now().timetuple()) | ||
| self.timestamps = [] | ||
| self.pressures = [] | ||
|
|
||
| def message_recv(self, ac_id, msg): | ||
| if msg.name == "PAYLOAD": | ||
| self.payload = PayloadMessage(msg) | ||
| self.update(self.payload.values) | ||
|
|
||
| def update(self,msg): | ||
| self.msg = msg | ||
| self.msgtime = datetime.now() | ||
| elements = self.msg.strip('<').strip('>').split(',') | ||
| if (len(elements) == 4): | ||
| self.tank = float(elements[0]) | ||
| self.bar = round(5 + self.tank / 100 * 295,1) | ||
| self.battery = float(elements[1]) | ||
| self.status = elements[2] | ||
| self.error = elements[3] | ||
|
|
||
| self.errors = [] | ||
| hex = '0000' | ||
| if (len(self.error) >= 4): | ||
| hex = self.error[2:6] | ||
| #array of 16 error codes | ||
| self.error_bin = bin(int(hex, 16))[2:].zfill(16) | ||
|
|
||
| self.update_plot() | ||
|
|
||
| def update_plot(self): | ||
| if len(self.pressures) == 0: | ||
| self.start_time = time.mktime(datetime.now().timetuple()) | ||
| self.timestamps.append(time.mktime(datetime.now().timetuple()) - self.start_time) | ||
| self.pressures.append(self.bar) | ||
| #self.generate_plot( | ||
| return | ||
|
|
||
| if self.bar != self.pressures[-1]: | ||
| self.timestamps.append(time.mktime(datetime.now().timetuple()) - self.start_time) | ||
| self.pressures.append(self.bar) | ||
| #self.generate_plot() | ||
|
|
||
| if __name__ == '__main__': | ||
| plotter = TankPlotter() | ||
| plt.ion() | ||
| figure = plt.figure('Fuel tank plotter') | ||
| ax = figure.add_subplot(111) | ||
| line_pressure, = ax.plot([0], [0]) | ||
| line_prediction, = ax.plot([0], [0], '--') | ||
| ax.set_xlim((0, 30)) | ||
| ax.set_ylim((0, 300)) | ||
| ax.set_title("time [min] to 0 bar: Wait for more data",fontsize = 20) | ||
| ax.set_xlabel("t [min]", fontsize = 20) | ||
| ax.set_ylabel("pressure [bar]", fontsize = 20) | ||
| ax.grid() | ||
| figure.canvas.draw() | ||
| figure.canvas.flush_events() | ||
|
|
||
| while True: | ||
| if len(plotter.pressures) > 0: | ||
| line_pressure.set_xdata(np.array(plotter.timestamps) / 60) | ||
| line_pressure.set_ydata(plotter.pressures) | ||
| ax.set_xlim((0, plotter.timestamps[-1] / 60. + 30)) | ||
| ax.set_ylim((0, 300)) | ||
|
|
||
| if len(plotter.pressures) >= 5: | ||
| # Determine slope | ||
| dt = plotter.timestamps[-1] - plotter.timestamps[-5] | ||
| d_pressure = plotter.pressures[-1] - plotter.pressures[-5] | ||
|
|
||
| # Determine point | ||
| p_pressure = (plotter.pressures[-5] + plotter.pressures[-4] + plotter.pressures[-3] + plotter.pressures[-2] + plotter.pressures[-1]) / 5. | ||
| p_t = (plotter.timestamps[-5] + plotter.timestamps[-4] + plotter.timestamps[-3] + plotter.timestamps[-2] + plotter.timestamps[-1]) / 5. | ||
|
|
||
| # Calculate 0 point | ||
| dt0 = p_pressure / (-d_pressure/ dt) | ||
| t_pressure0 = dt0 + p_t | ||
|
|
||
| # Plot line | ||
| line_prediction.set_xdata(np.array([p_t, t_pressure0]) / 60.) | ||
| line_prediction.set_ydata([p_pressure, 0]) | ||
| ax.set_xlim([0, t_pressure0 /60. + 30]) | ||
| ax.set_title("time [min] to 0 bar: " + str(dt0 / 60.), fontsize = 20) | ||
|
|
||
| plt.draw() | ||
|
|
||
| figure.canvas.draw() | ||
| figure.canvas.flush_events() | ||
| sys.stdout.flush() | ||
| time.sleep(1) |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,183 @@ | ||
| #!/usr/bin/env python3 | ||
| # | ||
| # Copyright (C) 2017 Hector Garcia de Marina <hgdemarina@gmail.com> | ||
| # Gautier Hattenberger <gautier.hattenberger@enac.fr> | ||
| # | ||
| # 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/>. | ||
| # | ||
|
|
||
| ''' | ||
| Moving base simulator | ||
| ''' | ||
|
|
||
| # too many things here | ||
| from __future__ import print_function | ||
| import sys | ||
| import numpy as np | ||
| import json | ||
| from time import sleep | ||
| from os import path, getenv | ||
| import time | ||
| import pymap3d as pm | ||
| import math as m | ||
|
|
||
| PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) | ||
| sys.path.append(PPRZ_HOME + "/var/lib/python/") | ||
| from pprzlink.ivy import IvyMessagesInterface | ||
| from pprzlink.message import PprzMessage | ||
|
|
||
| from scipy import linalg as la | ||
|
|
||
| class UAV: | ||
| def __init__(self, ac_id): | ||
| self.initialized = False | ||
| self.id = ac_id | ||
| self.lat = 0 | ||
| self.lon = 0 | ||
| self.alt = 0 | ||
| self.timeout = 0 | ||
|
|
||
| class Base: | ||
| def __init__(self, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False): | ||
| self.step = 1. / freq | ||
| self.use_ground_ref = use_ground_ref | ||
| self.enabled = True # run sim by default | ||
| self.verbose = verbose | ||
| self.ids = [5] | ||
| self.uavs = [UAV(i) for i in self.ids] | ||
| self.time = time.mktime(time.gmtime()) | ||
| self.speed = 1 # m/s | ||
| self.course = -90 # deg | ||
| self.heading = -90 # deg | ||
| self.lat = 38.08000040764657 #deg | ||
| self.lon = -9.1 #deg | ||
| self.altitude = 2.0 # starts from 1 m high | ||
|
|
||
| # Start IVY interface | ||
| self._interface = IvyMessagesInterface("Moving Base") | ||
|
|
||
| # bind to GPS_INT message | ||
| def ins_cb(ac_id, msg): | ||
| if ac_id in self.ids and msg.name == "GPS_INT": | ||
| uav = self.uavs[self.ids.index(ac_id)] | ||
| i2p = 1. / 2**8 # integer to position | ||
| i2v = 1. / 2**19 # integer to velocity | ||
| uav.lat[0] = float(msg['lat']) / 1e7 | ||
| uav.lon[1] = float(msg['lon']) / 1e7 | ||
| uav.alt[2] = float(msg['alt']) / 100 | ||
| uav.timeout = 0 | ||
| uav.initialized = True | ||
| if not self.use_ground_ref: | ||
| self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS")) | ||
|
|
||
| # bind to GROUND_REF message | ||
| def ground_ref_cb(ground_id, msg): | ||
| ac_id = int(msg['ac_id']) | ||
| if ac_id in self.ids: | ||
| uav = self.uavs[self.ids.index(ac_id)] | ||
| # X and V in NED | ||
| uav.X[0] = float(msg['pos'][1]) | ||
| uav.X[1] = float(msg['pos'][0]) | ||
| uav.X[2] = -float(msg['pos'][2]) | ||
| uav.V[0] = float(msg['speed'][1]) | ||
| uav.V[1] = float(msg['speed'][0]) | ||
| uav.V[2] = -float(msg['speed'][2]) | ||
| uav.timeout = 0 | ||
| uav.initialized = True | ||
| if self.use_ground_ref: | ||
| self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF")) | ||
|
|
||
| def __del__(self): | ||
| self.stop() | ||
|
|
||
| def stop(self): | ||
| # Stop IVY interface | ||
| if self._interface is not None: | ||
| self._interface.shutdown() | ||
|
|
||
| def move_base(self, north, east): | ||
|
|
||
| out = pm.ned2geodetic(north, east, 0, self.lat, self.lon, self.altitude) | ||
| self.lat = out[0] | ||
| self.lon = out[1] | ||
|
|
||
| def send_pos(self): | ||
| ''' | ||
| Send position of base sation | ||
| ''' | ||
| ready = True | ||
| for uav in self.uavs: | ||
| if not uav.initialized: | ||
| if self.verbose: | ||
| print("Waiting for state of rotorcraft ", uav.id) | ||
| sys.stdout.flush() | ||
| ready = False | ||
| if uav.timeout > 0.5: | ||
| if self.verbose: | ||
| print("The state msg of rotorcraft ", uav.id, " stopped") | ||
| sys.stdout.flush() | ||
| ready = False | ||
|
|
||
| # if not ready: | ||
| # return | ||
|
|
||
|
|
||
| if self.verbose: | ||
| print("Error distances: " + str(E).replace('[','').replace(']','')) | ||
| sys.stdout.flush() | ||
|
|
||
| for ac in self.uavs: | ||
| msg = PprzMessage("datalink", "TARGET_POS") | ||
| msg['ac_id'] = ac.id | ||
| msg['target_id'] = ac.id | ||
| msg['lat'] = int(self.lat * 1e7) | ||
| msg['lon'] = int(self.lon * 1e7) | ||
| msg['alt'] = int(self.altitude *1000) | ||
| msg['speed'] = self.speed | ||
| msg['climb'] = 0 | ||
| msg['course'] = self.course | ||
| msg['heading'] = self.heading | ||
| self._interface.send(msg) | ||
|
|
||
| def run(self): | ||
| try: | ||
| # The main loop | ||
| while True: | ||
| # TODO: make better frequency managing | ||
| sleep(self.step) | ||
|
|
||
| for uav in self.uavs: | ||
| uav.timeout = uav.timeout + self.step | ||
|
|
||
| # Send base position | ||
| if self.enabled: | ||
| dn = self.speed*m.cos(self.course/180.0*m.pi) | ||
| de = self.speed*m.sin(self.course/180.0*m.pi) | ||
| self.move_base(self.step*dn,self.step*de) | ||
| self.send_pos() | ||
|
|
||
| except KeyboardInterrupt: | ||
| self.stop() | ||
|
|
||
|
|
||
| if __name__ == '__main__': | ||
| import argparse | ||
|
|
||
|
|
||
| base = Base() | ||
| base.run() | ||
|
|