| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,60 @@ | ||
| /* | ||
| * Copyright (C) 2019 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/>. | ||
| */ | ||
| #include <stdlib.h> | ||
|
|
||
| /** | ||
| * Replace new and delete operators of C++ | ||
| */ | ||
|
|
||
| void * operator new(size_t size) | ||
| { | ||
| if (size < 1) { | ||
| size = 1; | ||
| } | ||
| return(calloc(size, 1)); | ||
| } | ||
|
|
||
| void operator delete(void *p) | ||
| { | ||
| if (p) free(p); | ||
| } | ||
|
|
||
| void * operator new[](size_t size) | ||
| { | ||
| if (size < 1) { | ||
| size = 1; | ||
| } | ||
| return(calloc(size, 1)); | ||
| } | ||
|
|
||
| void operator delete[](void * ptr) | ||
| { | ||
| if (ptr) free(ptr); | ||
| } | ||
|
|
||
| /*void operator delete(void* ptr, unsigned int size) | ||
| { | ||
| if (ptr) free(ptr); | ||
| } | ||
| void operator delete [](void* ptr, unsigned int size) | ||
| { | ||
| if (ptr) free(ptr); | ||
| }*/ |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| /* | ||
| * Copyright (C) 2018 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/>. | ||
| */ | ||
|
|
||
| /** | ||
| * @file "modules/actuators/actuators_dshot.h" | ||
| * @author Gautier Hattenberger | ||
| * Driver for DSHOT speed controller protocol | ||
| */ | ||
|
|
||
| #ifndef ACTUATORS_DSHOT_H | ||
| #define ACTUATORS_DSHOT_H | ||
|
|
||
| #include "std.h" | ||
| #include "modules/actuators/actuators_dshot_arch.h" | ||
|
|
||
| /** In normal DSHOT, first 48 values are special commands | ||
| * this offset allow to use 0 as the no-throttle command | ||
| * This should not be changed unless you know what you are doing | ||
| */ | ||
| #ifndef ACTUATORS_DSHOT_OFFSET | ||
| #define ACTUATORS_DSHOT_OFFSET 48 | ||
| #endif | ||
|
|
||
| /** Maxnum number of DSHOT commands | ||
| * This should be large enough for max applications | ||
| */ | ||
| #ifndef ACTUATORS_DSHOT_NB | ||
| #define ACTUATORS_DSHOT_NB 8 | ||
| #endif | ||
|
|
||
| extern uint16_t actuators_dshot_values[ACTUATORS_DSHOT_NB]; | ||
|
|
||
| /** Arch dependent init | ||
| */ | ||
| extern void actuators_dshot_arch_init(void); | ||
| extern void actuators_dshot_arch_commit(void); | ||
|
|
||
| /* Actuator macros */ | ||
| #define ActuatorDShotSet(_i, _v) { \ | ||
| if (_v == 0) { actuators_dshot_values[_i] = 0; } \ | ||
| else { actuators_dshot_values[_i] = _v + ACTUATORS_DSHOT_OFFSET; } \ | ||
| } | ||
| #define ActuatorsDShotInit() actuators_dshot_arch_init() | ||
| #define ActuatorsDShotCommit() actuators_dshot_arch_commit() | ||
|
|
||
| #endif | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,52 @@ | ||
| /* | ||
| * Copyright (C) 2019 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/>. | ||
| */ | ||
| /** | ||
| * @file "modules/joystick/joystick.c" | ||
| * @author Gautier Hattenberger | ||
| * Handle JOYSTICK_RAW messages | ||
| */ | ||
|
|
||
| #include "modules/joystick/joystick.h" | ||
| #include "subsystems/datalink/datalink.h" | ||
| #include "subsystems/abi.h" | ||
| #include "pprzlink/dl_protocol.h" | ||
| #include "generated/airframe.h" | ||
|
|
||
| struct Joystick joystick; | ||
|
|
||
| void joystick_init(void) | ||
| { | ||
| joystick.roll = 0; | ||
| joystick.pitch = 0; | ||
| joystick.yaw = 0; | ||
| joystick.throttle = 0; | ||
| } | ||
|
|
||
| void joystick_parse(void) | ||
| { | ||
| if (DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) { | ||
| joystick.roll = DL_JOYSTICK_RAW_roll(dl_buffer); | ||
| joystick.pitch = DL_JOYSTICK_RAW_pitch(dl_buffer); | ||
| joystick.yaw = DL_JOYSTICK_RAW_yaw(dl_buffer); | ||
| joystick.throttle = DL_JOYSTICK_RAW_throttle(dl_buffer); | ||
| AbiSendMsgJOYSTICK(JOYSTICK_ID, joystick.roll, joystick.pitch, joystick.yaw, joystick.throttle); | ||
| } | ||
| } | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,55 @@ | ||
| /* | ||
| * Copyright (C) 2019 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/>. | ||
| */ | ||
| /** | ||
| * @file "modules/joystick/joystick.h" | ||
| * @author Gautier Hattenberger | ||
| * Handle JOYSTICK_RAW messages | ||
| */ | ||
|
|
||
| #ifndef JOYSTICK_H | ||
| #define JOYSTICK_H | ||
|
|
||
| #include "std.h" | ||
|
|
||
| /** | ||
| * Joystick structure | ||
| */ | ||
| struct Joystick { | ||
| int32_t roll; ///< roll command | ||
| int32_t pitch; ///< pitch command | ||
| int32_t yaw; ///< yaw command | ||
| int32_t throttle; ///< throttle command | ||
| }; | ||
|
|
||
| extern struct Joystick joystick; | ||
|
|
||
| /** | ||
| * Init function | ||
| */ | ||
| extern void joystick_init(void); | ||
|
|
||
| /** | ||
| * JOYSTICK_RAW message parser | ||
| * if valid, send a JOYSTICK ABI message | ||
| */ | ||
| extern void joystick_parse(void); | ||
|
|
||
| #endif | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,173 @@ | ||
| /* | ||
| * Copyright (C) 2019 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, write to | ||
| * the Free Software Foundation, 59 Temple Place - Suite 330, | ||
| * Boston, MA 02111-1307, USA. | ||
| * | ||
| */ | ||
|
|
||
| /** @file modules/lidar/tfmini.c | ||
| * @brief driver for the TFMini lidar | ||
| * | ||
| */ | ||
| #include "tfmini.h" | ||
| #include "mcu_periph/uart.h" | ||
| #include "subsystems/abi.h" | ||
|
|
||
| // State interface for rotation compensation | ||
| #include "state.h" | ||
|
|
||
| // Messages | ||
| #include "pprzlink/messages.h" | ||
| #include "subsystems/datalink/downlink.h" | ||
|
|
||
| struct TFMini tfmini = { | ||
| .parse_status = TFMINI_INITIALIZE | ||
| }; | ||
|
|
||
| static void tfmini_parse(uint8_t byte); | ||
|
|
||
| #if PERIODIC_TELEMETRY | ||
| #include "subsystems/datalink/telemetry.h" | ||
|
|
||
| /** | ||
| * Downlink message lidar | ||
| */ | ||
| static void tfmini_send_lidar(struct transport_tx *trans, struct link_device *dev) | ||
| { | ||
| pprz_msg_send_LIDAR(trans, dev, AC_ID, | ||
| &tfmini.distance, | ||
| &tfmini.mode, | ||
| &tfmini.parse_status); | ||
| } | ||
|
|
||
| #endif | ||
|
|
||
| /** | ||
| * Initialization function | ||
| */ | ||
| void tfmini_init(void) | ||
| { | ||
| tfmini.device = &((TFMINI_PORT).device); | ||
|
|
||
| tfmini.update_agl = USE_TFMINI_AGL; | ||
| tfmini.compensate_rotation = TFMINI_COMPENSATE_ROTATION; | ||
|
|
||
| tfmini.strength = 0; | ||
| tfmini.distance = 0; | ||
| tfmini.parse_status = TFMINI_PARSE_HEAD; | ||
|
|
||
| #if PERIODIC_TELEMETRY | ||
| register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LIDAR, tfmini_send_lidar); | ||
| #endif | ||
| } | ||
|
|
||
| /** | ||
| * Lidar event function | ||
| * Receive bytes from the UART port and parse them | ||
| */ | ||
| void tfmini_event(void) | ||
| { | ||
| while (tfmini.parse_status != TFMINI_INITIALIZE && tfmini.device->char_available(tfmini.device->periph)) { | ||
| tfmini_parse(tfmini.device->get_byte(tfmini.device->periph)); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Parse the lidar bytes 1 by 1 | ||
| */ | ||
| static void tfmini_parse(uint8_t byte) | ||
| { | ||
| switch (tfmini.parse_status) { | ||
| case TFMINI_INITIALIZE: | ||
| break; | ||
| case TFMINI_PARSE_HEAD: | ||
| if (byte == 0x59) { | ||
| tfmini.parse_crc = byte; | ||
| tfmini.parse_status++; | ||
| } | ||
| break; | ||
| case TFMINI_PARSE_HEAD2: | ||
| if (byte == 0x59) { | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| } else { | ||
| tfmini.parse_status = TFMINI_PARSE_HEAD; | ||
| } | ||
| break; | ||
|
|
||
| case TFMINI_PARSE_DIST_L: | ||
| tfmini.raw_dist = byte; | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| break; | ||
| case TFMINI_PARSE_DIST_H: | ||
| tfmini.raw_dist |= (byte << 8); | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| break; | ||
|
|
||
| case TFMINI_PARSE_STRENGTH_L: | ||
| tfmini.raw_strength = byte; | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| break; | ||
| case TFMINI_PARSE_STRENGTH_H: | ||
| tfmini.raw_strength |= (byte << 8); | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| break; | ||
|
|
||
| case TFMINI_PARSE_MODE: | ||
| tfmini.raw_mode = byte; | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| break; | ||
| case TFMINI_PARSE_BYTE7: | ||
| tfmini.parse_crc += byte; | ||
| tfmini.parse_status++; | ||
| break; | ||
|
|
||
| case TFMINI_PARSE_CHECKSUM: | ||
| // When the CRC matches | ||
| if (tfmini.parse_crc == byte) { | ||
| tfmini.distance = tfmini.raw_dist / 100.f; | ||
| tfmini.strength = tfmini.raw_strength; | ||
| tfmini.mode = tfmini.raw_mode; | ||
|
|
||
| // When the distance is valid | ||
| if (tfmini.distance != 0xFFFF) { | ||
| // compensate AGL measurement for body rotation | ||
| if (tfmini.compensate_rotation) { | ||
| float phi = stateGetNedToBodyEulers_f()->phi; | ||
| float theta = stateGetNedToBodyEulers_f()->theta; | ||
| float gain = (float)fabs((double)(cosf(phi) * cosf(theta))); | ||
| tfmini.distance = tfmini.distance / gain; | ||
| } | ||
|
|
||
| // send message (if requested) | ||
| if (tfmini.update_agl) { | ||
| AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, tfmini.distance); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| // Start reading again | ||
| tfmini.parse_status = TFMINI_PARSE_HEAD; | ||
| break; | ||
| } | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,68 @@ | ||
| /* | ||
| * Copyright (C) 2019 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, write to | ||
| * the Free Software Foundation, 59 Temple Place - Suite 330, | ||
| * Boston, MA 02111-1307, USA. | ||
| * | ||
| */ | ||
|
|
||
| /** @file modules/lidar/tfmini.h | ||
| * @brief driver for the TFMini lidar | ||
| * | ||
| */ | ||
| #ifndef LIDAR_TFMINI_H | ||
| #define LIDAR_TFMINI_H | ||
|
|
||
| #include "std.h" | ||
| #include "mcu_periph/i2c.h" | ||
|
|
||
| enum TFMiniParseStatus { | ||
| TFMINI_INITIALIZE, | ||
| TFMINI_PARSE_HEAD, | ||
| TFMINI_PARSE_HEAD2, | ||
| TFMINI_PARSE_DIST_L, | ||
| TFMINI_PARSE_DIST_H, | ||
| TFMINI_PARSE_STRENGTH_L, | ||
| TFMINI_PARSE_STRENGTH_H, | ||
| TFMINI_PARSE_MODE, | ||
| TFMINI_PARSE_BYTE7, | ||
| TFMINI_PARSE_CHECKSUM | ||
| }; | ||
|
|
||
| struct TFMini { | ||
| struct link_device *device; | ||
| enum TFMiniParseStatus parse_status; | ||
| uint8_t parse_crc; | ||
| uint16_t raw_dist; | ||
| uint16_t raw_strength; | ||
| uint8_t raw_mode; | ||
|
|
||
| uint16_t strength; | ||
| float distance; // [m] | ||
| uint8_t mode; | ||
| bool update_agl; | ||
| bool compensate_rotation; | ||
| }; | ||
|
|
||
| extern struct TFMini tfmini; | ||
|
|
||
| extern void tfmini_init(void); | ||
| extern void tfmini_event(void); | ||
| extern void tfmini_downlink(void); | ||
|
|
||
| #endif /* LIDAR_TFMINI_H */ | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,33 @@ | ||
| /* | ||
| * Copyright (C) 2018 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/>. | ||
| */ | ||
|
|
||
| /** | ||
| * @file "subsystems/actuators/actuators_dshot.h" | ||
| * @author Gautier Hattenberger | ||
| * dummy file for airframe generator (driver section) | ||
| */ | ||
|
|
||
| #ifndef SUB_ACTUATORS_DSHOT_H | ||
| #define SUB_ACTUATORS_DSHOT_H | ||
|
|
||
| #include "modules/actuators/actuators_dshot.h" | ||
|
|
||
| #endif | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,45 @@ | ||
| /* | ||
| * Copyright (C) 2016 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, write to | ||
| * the Free Software Foundation, 59 Temple Place - Suite 330, | ||
| * Boston, MA 02111-1307, USA. | ||
| */ | ||
|
|
||
| /** | ||
| * @file subsystems/ins/ins_ekf2.h | ||
| * | ||
| * INS based in the EKF2 of PX4 | ||
| * | ||
| */ | ||
|
|
||
| #ifndef INS_EKF2_H | ||
| #define INS_EKF2_H | ||
|
|
||
| #ifdef __cplusplus | ||
| extern "C" { | ||
| #endif | ||
|
|
||
| #include "subsystems/ins.h" | ||
|
|
||
| extern void ins_ekf2_init(void); | ||
| extern void ins_ekf2_update(void); | ||
|
|
||
| #ifdef __cplusplus | ||
| } | ||
| #endif | ||
|
|
||
| #endif /* INS_EKF2_H */ |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,6 @@ | ||
| matlab2pprz.py is a bridge between Natnet (Optitrack interface), Simulink (using UDP send and receive blocks) and Paparazzi (using Ivy) | ||
|
|
||
| example of Simulink model: https://github.com/enacuavlab/vto-tools/tree/master/Matlab/Simulink | ||
|
|
||
| example of command to start the script: ./matlab2pprz.py -ac 211 211 5005 -sp 9090 -ns 192.168.1.230 -v | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,149 @@ | ||
| #!/usr/bin/env python3 | ||
| # | ||
| # Copyright (C) 2019 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/>. | ||
| # | ||
|
|
||
| ''' | ||
| Forward rigid body positions and orientation from Natnet to Matlab/Simulink | ||
| via UDP ports and receive commands that are forwarded over Ivy | ||
| ''' | ||
|
|
||
| from __future__ import print_function | ||
|
|
||
| import sys | ||
| from os import path, getenv | ||
| from time import time, sleep | ||
| import argparse | ||
| import math | ||
| import socket | ||
| import struct | ||
|
|
||
| # if PAPARAZZI_HOME not set, then assume the tree containing this | ||
| # file is a reasonable substitute | ||
| PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) | ||
| sys.path.append(PPRZ_HOME + "/var/lib/python") | ||
| sys.path.append(PPRZ_HOME + "/sw/ground_segment/python/natnet3.x") | ||
|
|
||
| from NatNetClient import NatNetClient | ||
|
|
||
| from pprzlink.ivy import IvyMessagesInterface | ||
| from pprzlink.message import PprzMessage | ||
|
|
||
| # parse args | ||
| parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) | ||
| parser.add_argument('-ac', action='append', nargs=3, | ||
| metavar=('rigid_id','ac_id','udp_out'), help='tuple of rigid body, A/C id and UDP output port to Simulink (multiple possible)') | ||
| parser.add_argument('-b', '--ivy_bus', dest='ivy_bus', help="Ivy bus address and port") | ||
| parser.add_argument('-ns', '--natnet_server', dest='natnet_server', default="127.0.0.1", help="NatNet server IP address") | ||
| parser.add_argument('-m', '--multicast_addr', dest='multicast', default="239.255.42.99", help="NatNet server multicast address") | ||
| parser.add_argument('-dp', '--data_port', dest='data_port', type=int, default=1511, help="NatNet server data socket UDP port") | ||
| parser.add_argument('-cp', '--command_port', dest='command_port', type=int, default=1510, help="NatNet server command socket UDP port") | ||
| parser.add_argument('-ss', '--simulink_server', dest='simulink_server', default="127.0.0.1", help="Simulink server IP address") | ||
| parser.add_argument('-sp', '--simulink_in_port', dest='simulink_in_port', default=9090, help="Simulink input port") | ||
| parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages") | ||
| args = parser.parse_args() | ||
|
|
||
| if args.ac is None: | ||
| print("At least one pair of rigid boby / AC id must be declared") | ||
| exit() | ||
|
|
||
| # dictionary of ID associations | ||
| id_dict = { v[0]: {'ac_id':v[1], 'udp_out':v[2]} for v in args.ac } | ||
|
|
||
| # start ivy interface | ||
| if args.ivy_bus is not None: | ||
| ivy = IvyMessagesInterface("matlab2pprz", ivy_bus=args.ivy_bus) | ||
| else: | ||
| ivy = IvyMessagesInterface("matlab2pprz") | ||
| # prepare Ivy message | ||
| ivy_msg = PprzMessage("datalink", "JOYSTICK_RAW") | ||
|
|
||
| # start socket for communication with Matlab/Simulink | ||
| try: | ||
| sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) | ||
| sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) | ||
| sock.settimeout(2.0) | ||
| sock.bind(('0.0.0.0', args.simulink_in_port)) # FIXME one port for all or one for each ? | ||
| except OSError: | ||
| print("Error: unable to open socket") | ||
|
|
||
|
|
||
| def receiveRigidBodyList( rigidBodyList, stamp ): | ||
| for (body_id, pos, quat, valid) in rigidBodyList: | ||
| if not valid: | ||
| # skip if rigid body is not valid | ||
| continue | ||
|
|
||
| i = str(body_id) | ||
| if i not in id_dict.keys(): | ||
| # unknown A/C ID | ||
| continue | ||
| ac_id = id_dict[i]['ac_id'] | ||
| port = int(id_dict[i]['udp_out']) | ||
|
|
||
| # data array = pos + quat | ||
| data = pos + quat | ||
|
|
||
| msg = ac_id+" "+format(stamp,"+0.5f")+" "+" ".join(format(x,"+0.4f") for x in data) | ||
| if args.verbose: | ||
| print('sending to simulink ({},{}): {}'.format(args.simulink_server, port, msg)) | ||
|
|
||
| bindata = struct.pack('<fffffffff',float(ac_id),stamp,pos[0],pos[1],pos[2],quat[0],quat[1],quat[2],quat[3]) | ||
| sock.sendto(bindata,(args.simulink_server,port)) | ||
|
|
||
| natnet = NatNetClient( | ||
| server=args.natnet_server, | ||
| rigidBodyListListener=receiveRigidBodyList, | ||
| dataPort=args.data_port, | ||
| commandPort=args.command_port) | ||
|
|
||
| def sendCommandMsg(cmd): | ||
| # decode 5 int16 (short) and send other Ivy | ||
| bindata = struct.unpack('<hhhhh',cmd) | ||
| ivy_msg['ac_id'] = bindata[0] | ||
| ivy_msg['roll'] = bindata[1] | ||
| ivy_msg['pitch'] = bindata[2] | ||
| ivy_msg['yaw'] = bindata[3] | ||
| ivy_msg['throttle'] = bindata[4] | ||
| ivy.send(ivy_msg) | ||
|
|
||
| print("Starting Natnet3.x/Ivy to Matlab/Simulink bridge between %s and %s" % (args.natnet_server, args.simulink_server)) | ||
| try: | ||
| # Start up the streaming client. | ||
| # This will run perpetually, and operate on a separate thread. | ||
| natnet.run() | ||
| while True: | ||
| try: | ||
| (cmd, address) = sock.recvfrom(10) # receiving 5 int16 (id, roll, pitch, yaw, thrust) | ||
| if len(cmd) == 10: # something to send | ||
| sendCommandMsg(cmd) | ||
| except socket.timeout: | ||
| pass | ||
| except (KeyboardInterrupt, SystemExit): | ||
| print("Shutting down socket, ivy and natnet interfaces...") | ||
| natnet.stop() | ||
| ivy.shutdown() | ||
| sock.close() | ||
| except OSError: | ||
| print("Natnet connection error") | ||
| natnet.stop() | ||
| ivy.stop() | ||
| sock.close() | ||
| exit(-1) | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,15 @@ | ||
| #!/bin/sh | ||
| # This script sets up environment variables for gazebo_ros | ||
|
|
||
| # Paparazzi paths | ||
| export GAZEBO_MODEL_PATH="$PAPARAZZI_HOME/conf/simulator/gazebo/models:$GAZEBO_MODEL_PATH" | ||
| export GAZEBO_MODEL_PATH="$PAPARAZZI_HOME/sw/ext/tudelft_gazebo_models/models:$GAZEBO_MODEL_PATH" | ||
|
|
||
| # ROS and Gazebo defaults | ||
| ROS_SETUP=`locate --regex 'ros/[a-z]*/setup.sh$'` | ||
| GAZEBO_SETUP=`locate --regex 'gazebo/setup.sh$'` | ||
| . $ROS_SETUP | ||
| . $GAZEBO_SETUP | ||
|
|
||
| # Launch NPS-Gazebo | ||
| exec $PAPARAZZI_HOME/sw/simulator/pprzsim-launch $@ |