diff --git a/conf/airframes/tudelft/neddrone4_tem.xml b/conf/airframes/tudelft/neddrone4_tem.xml
new file mode 100644
index 0000000000..6e81afb1fb
--- /dev/null
+++ b/conf/airframes/tudelft/neddrone4_tem.xml
@@ -0,0 +1,360 @@
+
+
+
+
+
+ Neddrone4
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/tudelft/nederdrone_cyberzoo.xml b/conf/flight_plans/tudelft/nederdrone_cyberzoo.xml
new file mode 100644
index 0000000000..5480b3482d
--- /dev/null
+++ b/conf/flight_plans/tudelft/nederdrone_cyberzoo.xml
@@ -0,0 +1,75 @@
+
+
+
+
+ #include "subsystems/datalink/datalink.h"
+ #include "subsystems/electrical.h"
+ #include "subsystems/radio_control.h"
+ #include "subsystems/ahrs.h"
+ #include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
+ #include "firmwares/rotorcraft/navigation.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
index b48fd00668..9e1858e500 100644
--- a/conf/modules/ins_ekf2.xml
+++ b/conf/modules/ins_ekf2.xml
@@ -5,6 +5,17 @@
simple INS and AHRS using EKF2 from PX4
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index 48028d1e5d..9dc899860e 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -218,6 +218,7 @@
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 80f1eaf93e..c4363501f2 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -288,12 +288,12 @@
@@ -303,9 +303,9 @@
airframe="airframes/tudelft/neddrone5.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
- flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
+ flight_plan="flight_plans/tudelft/nederdrone_waal.xml"
settings="settings/rotorcraft_basic.xml"
- settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
+ settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index 39bc5de6dc..dce99a8790 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -200,8 +200,18 @@ void guidance_indi_init(void)
* Call upon entering indi guidance
*/
void guidance_indi_enter(void) {
- thrust_in = 0.0;
- thrust_act = 0;
+ thrust_in = stabilization_cmd[COMMAND_THRUST];
+ thrust_act = thrust_in;
+
+ float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
+ float sample_time = 1.0 / PERIODIC_FREQUENCY;
+ for (int8_t i = 0; i < 3; i++) {
+ init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
+ }
+ init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi);
+ init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta);
+ init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
+ init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
}
#include "firmwares/rotorcraft/navigation.h"
diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c
index 17ad457dea..7b4f84f525 100644
--- a/sw/airborne/subsystems/gps/gps_datalink.c
+++ b/sw/airborne/subsystems/gps/gps_datalink.c
@@ -41,10 +41,10 @@ struct GpsState gps_datalink;
void gps_datalink_init(void)
{
gps_datalink.fix = GPS_FIX_NONE;
- gps_datalink.pdop = 0;
- gps_datalink.sacc = 0;
- gps_datalink.pacc = 0;
- gps_datalink.cacc = 0;
+ gps_datalink.pdop = 10;
+ gps_datalink.sacc = 5;
+ gps_datalink.pacc = 1;
+ gps_datalink.cacc = 1;
gps_datalink.comp_id = GPS_DATALINK_ID;
@@ -108,7 +108,7 @@ static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t
gps_datalink.course = ((int32_t)heading) * 1e3;
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
- gps_datalink.num_sv = 7;
+ gps_datalink.num_sv = 30;
gps_datalink.tow = tow;
gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
diff --git a/sw/airborne/subsystems/ins/ins_ekf2.cpp b/sw/airborne/subsystems/ins/ins_ekf2.cpp
index 5034620960..d0f4e47a82 100644
--- a/sw/airborne/subsystems/ins/ins_ekf2.cpp
+++ b/sw/airborne/subsystems/ins/ins_ekf2.cpp
@@ -47,6 +47,24 @@
#error INS initialization from flight plan is not yet supported
#endif
+/** The EKF2 fusion mode setting */
+#ifndef INS_EKF2_FUSION_MODE
+#define INS_EKF2_FUSION_MODE (MASK_USE_GPS)
+#endif
+PRINT_CONFIG_VAR(INS_EKF2_FUSION_MODE)
+
+/** The EKF2 primary vertical distance sensor type */
+#ifndef INS_EKF2_VDIST_SENSOR_TYPE
+#define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO
+#endif
+PRINT_CONFIG_VAR(INS_EKF2_VDIST_SENSOR_TYPE)
+
+/** The EKF2 GPS checks before initialization */
+#ifndef INS_EKF2_GPS_CHECK_MASK
+#define INS_EKF2_GPS_CHECK_MASK (MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)
+#endif
+PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK)
+
/** default AGL sensor to use in INS */
#ifndef INS_EKF2_AGL_ID
#define INS_EKF2_AGL_ID ABI_BROADCAST
@@ -143,7 +161,7 @@ static void ins_ekf2_publish_attitude(uint32_t stamp);
/* Static local variables */
static Ekf ekf; ///< EKF class itself
-static parameters *ekf_params; ///< The EKF parameters
+static parameters *ekf_params; ///< The EKF parameters
struct ekf2_t ekf2; ///< Local EKF2 status structure
static uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2; ///< Component ID for EKF
@@ -252,6 +270,9 @@ void ins_ekf2_init(void)
ekf_params = ekf.getParamHandle();
ekf_params->mag_fusion_type = MAG_FUSE_TYPE_HEADING;
ekf_params->is_moving_scaler = 0.8f;
+ ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
+ ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
+ ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
/* Initialize struct */
ekf2.ltp_stamp = 0;
@@ -553,8 +574,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
gps_msg.lat = gps_s->lla_pos.lat;
gps_msg.lon = gps_s->lla_pos.lon;
gps_msg.alt = gps_s->hmsl;
+#if INS_EKF2_GPS_COURSE_YAW
+ gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
+ gps_msg.yaw_offset = 0;
+#else
gps_msg.yaw = NAN;
gps_msg.yaw_offset = NAN;
+#endif
gps_msg.fix_type = gps_s->fix;
gps_msg.eph = gps_s->hacc / 100.0;
gps_msg.epv = gps_s->vacc / 100.0;
diff --git a/sw/ground_segment/python/herelink.py b/sw/ground_segment/python/herelink.py
new file mode 100755
index 0000000000..a2cdfcb92c
--- /dev/null
+++ b/sw/ground_segment/python/herelink.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2021 Freek van tienen
+#
+# 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
+# .
+#
+
+'''
+Read MAVLink messages from the herelink to get RSSI information
+'''
+
+
+from __future__ import print_function
+
+import sys
+from os import path, getenv
+from time import time, sleep
+import numpy as np
+from collections import deque
+import argparse
+from pymavlink import mavutil
+
+# 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")
+from pprzlink.ivy import IvyMessagesInterface
+from pprzlink.message import PprzMessage
+
+# parse args
+parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+parser.add_argument('-ac', '--ac_id', dest='ac_id', type=int, help='Aircraft ID to save the RSSI information to')
+parser.add_argument('-b', '--ivy_bus', dest='ivy_bus', help="Ivy bus address and port")
+args = parser.parse_args()
+
+if args.ac_id is None:
+ print("You need to define an aircraft ID")
+ exit()
+
+# start ivy interface
+if args.ivy_bus is not None:
+ ivy = IvyMessagesInterface("herelink2ivy", ivy_bus=args.ivy_bus)
+else:
+ ivy = IvyMessagesInterface("herelink2ivy")
+
+# Start a connection listening to a UDP port
+conn = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
+
+print("Starting Herelink RSSI for ac_id %d" % (args.ac_id))
+try:
+ # Start up the streaming client.
+ while True:
+ msg = conn.recv_match(type='RADIO_STATUS', blocking=True)
+ if not msg or msg.get_type() == "BAD_DATA":
+ continue
+
+ pmsg = PprzMessage("telemetry", "RSSI_COMBINED")
+ pmsg['remote_rssi'] = msg.remrssi
+ pmsg['tx_power'] = 0
+ pmsg['local_rssi'] = msg.rssi
+ pmsg['local_noise'] = msg.noise
+ pmsg['remote_noise'] = msg.remnoise
+ ivy.send(pmsg, args.ac_id)
+except (KeyboardInterrupt, SystemExit):
+ print("Shutting down ivy interface...")
+ ivy.shutdown()