diff --git a/conf/airframes/tudelft/bebop_autonomous_race.xml b/conf/airframes/tudelft/bebop_autonomous_race_2017.xml
similarity index 100%
rename from conf/airframes/tudelft/bebop_autonomous_race.xml
rename to conf/airframes/tudelft/bebop_autonomous_race_2017.xml
diff --git a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
new file mode 100644
index 0000000000..ba1c5bbcf9
--- /dev/null
+++ b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
@@ -0,0 +1,221 @@
+
+
+
+ Autonomous Race 2018 Bebop1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml
index ebaac5e882..4cf3fd66f1 100644
--- a/conf/airframes/tudelft/bebop_indi_actuators.xml
+++ b/conf/airframes/tudelft/bebop_indi_actuators.xml
@@ -34,16 +34,14 @@
-
-
-
+
diff --git a/conf/modules/jevois_mavlink.xml b/conf/modules/jevois_mavlink.xml
new file mode 100644
index 0000000000..f5d5be9ed0
--- /dev/null
+++ b/conf/modules/jevois_mavlink.xml
@@ -0,0 +1,27 @@
+
+
+
+
+ Send sensor data to jevois and read commands from jevois using MAVLINK messages.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index e513ba8892..5585df2889 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -129,7 +129,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
- settings_modules="modules/video_rtp_stream.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+ settings_modules="modules/bebop_ae_awb.xml modules/video_rtp_stream.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="#ffffbc3bce5b"
/>
+
.
+ */
+/**
+ * @file "modules/sensors/cameras/jevois_mavlink.c"
+ * @author MAVLab
+ * Send sensor data to jevois and read commands from jevois
+ */
+
+#include "modules/sensors/cameras/jevois_mavlink.h"
+
+
+#define DEBUG_PRINT printf
+#include
+
+
+/*
+ * MavLink protocol
+ */
+
+#include
+#include "mavlink/paparazzi/mavlink.h"
+
+#include "subsystems/imu.h"
+#include "autopilot.h"
+
+mavlink_system_t mavlink_system;
+
+#ifndef MAVLINK_SYSID
+#define MAVLINK_SYSID 1
+#endif
+
+
+
+static void mavlink_send_heartbeat(void);
+static void mavlink_send_attitude(void);
+static void mavlink_send_highres_imu(void);
+static void mavlink_send_set_mode(void);
+
+
+/*
+ * Exported data
+ */
+
+
+mavlink_manual_setpoint_t jevois_mavlink_manual_setpoint;
+mavlink_debug_t jevois_mavlink_debug;
+mavlink_altitude_t jevois_mavlink_altitude;
+mavlink_attitude_t jevois_mavlink_attitude;
+mavlink_local_position_ned_t jevois_mavlink_local_position;
+
+
+
+/*
+ * Paparazzi Module functions : filter functions
+ */
+
+#include "filters/low_pass_filter.h"
+
+Butterworth2LowPass_int ax_filtered;
+Butterworth2LowPass_int ay_filtered;
+Butterworth2LowPass_int az_filtered;
+
+void jevois_mavlink_filter_periodic(void)
+{
+ update_butterworth_2_low_pass_int(&ax_filtered, imu.accel.x);
+ update_butterworth_2_low_pass_int(&ay_filtered, imu.accel.y);
+ update_butterworth_2_low_pass_int(&az_filtered, imu.accel.z);
+}
+
+void jevois_mavlink_filter_init(void);
+void jevois_mavlink_filter_init(void)
+{
+ init_butterworth_2_low_pass_int(&ax_filtered, 20.0, 1.0 / ((float)MODULES_FREQUENCY), imu.accel_unscaled.x);
+ init_butterworth_2_low_pass_int(&ay_filtered, 20.0, 1.0 / ((float)MODULES_FREQUENCY), imu.accel_unscaled.y);
+ init_butterworth_2_low_pass_int(&az_filtered, 20.0, 1.0 / ((float)MODULES_FREQUENCY), imu.accel_unscaled.z);
+}
+
+
+/*
+ * Paparazzi Module functions : communications
+ */
+
+void jevois_mavlink_init(void)
+{
+ mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
+ mavlink_system.compid = 0; // Component/Subsystem ID, 1-255
+
+ jevois_mavlink_filter_init();
+}
+
+void jevois_mavlink_periodic(void)
+{
+ RunOnceEvery(100, mavlink_send_heartbeat());
+ RunOnceEvery(2, mavlink_send_attitude());
+ RunOnceEvery(1, mavlink_send_highres_imu());
+ RunOnceEvery(1, mavlink_send_set_mode());
+}
+
+
+void jevois_mavlink_event(void)
+{
+ mavlink_message_t msg;
+ mavlink_status_t status;
+
+
+ while (MAVLinkChAvailable()) {
+ uint8_t c = MAVLinkGetch();
+ if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status)) {
+ //printf("msg.msgid is %d\n",msg.msgid);
+ switch (msg.msgid) {
+ case MAVLINK_MSG_ID_HEARTBEAT: {
+ mavlink_heartbeat_t heartbeat;
+ mavlink_msg_heartbeat_decode(&msg, &heartbeat);
+ // do something with heartbeat variable
+ }
+ break;
+
+ case MAVLINK_MSG_ID_ATTITUDE: {
+
+ // read attitude command from Jevois
+ mavlink_msg_attitude_decode(&msg, &jevois_mavlink_attitude);
+
+ DEBUG_PRINT("[jevois mavlink] phi_cmd = %f\n", DegOfRad(jevois_mavlink_attitude.roll));
+ DEBUG_PRINT("[jevois mavlink] theta_cmd = %f\n", DegOfRad(jevois_mavlink_attitude.pitch));
+ DEBUG_PRINT("[jevois mavlink] psi_cmd = %f\n", DegOfRad(jevois_mavlink_attitude.yaw));
+ }
+ break;
+
+ case MAVLINK_MSG_ID_ALTITUDE: {
+ mavlink_msg_altitude_decode(&msg, &jevois_mavlink_altitude);
+
+ DEBUG_PRINT("[jevois mavlink] desired altitude is %f", jevois_mavlink_altitude.altitude_relative);
+ }
+ break;
+
+ case MAVLINK_MSG_ID_DEBUG: {
+ mavlink_msg_debug_decode(&msg, &jevois_mavlink_debug);
+
+ DEBUG_PRINT("[jevois mavlink] debug value is %f", jevois_mavlink_debug.value);
+ DEBUG_PRINT("[jevois mavlink] debug ind is %d", jevois_mavlink_debug.ind);
+ }
+ break;
+
+ case MAVLINK_MSG_ID_MANUAL_SETPOINT: {
+ mavlink_msg_manual_setpoint_decode(&msg, &jevois_mavlink_manual_setpoint);
+
+ DEBUG_PRINT("[jevois mavlink] phi_cmd = %f\n", DegOfRad(jevois_mavlink_manual_setpoint.roll));
+ DEBUG_PRINT("[jevois mavlink] theta_cmd = %f\n", DegOfRad(jevois_mavlink_manual_setpoint.pitch));
+ DEBUG_PRINT("[jevois mavlink] psi_cmd = %f\n", DegOfRad(jevois_mavlink_manual_setpoint.yaw));
+ DEBUG_PRINT("[jevois mavlink] alt_cmd = %f\n", jevois_mavlink_manual_setpoint.thrust);
+ }
+ break;
+
+
+ case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
+ mavlink_msg_local_position_ned_decode(&msg, &jevois_mavlink_local_position);
+ }
+ break;
+ }
+ }
+ }
+}
+
+
+/////////////////////////////
+
+
+#include "state.h"
+#include "mcu_periph/sys_time.h"
+
+
+
+static void mavlink_send_attitude(void)
+{
+ mavlink_msg_attitude_send(MAVLINK_COMM_0,
+ get_sys_time_msec(),
+ stateGetNedToBodyEulers_f()->phi, // Phi
+ stateGetNedToBodyEulers_f()->theta, // Theta
+ stateGetNedToBodyEulers_f()->psi, // Psi
+ stateGetBodyRates_f()->p, // p
+ stateGetBodyRates_f()->q, // q
+ stateGetBodyRates_f()->r); // r
+ MAVLinkSendMessage();
+}
+
+
+static void mavlink_send_set_mode(void)
+{
+ mavlink_msg_set_mode_send(MAVLINK_COMM_0,
+ get_sys_time_msec(),
+ 0, //autopilotMode.currentMode,
+ 0
+ );
+ MAVLinkSendMessage();
+}
+
+static void mavlink_send_heartbeat(void)
+{
+ mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
+ MAV_TYPE_FIXED_WING,
+ MAV_AUTOPILOT_PPZ,
+ MAV_MODE_FLAG_AUTO_ENABLED,
+ 0, // custom_mode
+ MAV_STATE_CALIBRATING);
+ MAVLinkSendMessage();
+}
+
+
+static void mavlink_send_highres_imu(void)
+{
+ mavlink_msg_highres_imu_send(MAVLINK_COMM_0,
+ get_sys_time_msec(),
+ get_butterworth_2_low_pass_int(&ax_filtered) / 1024.0,
+ get_butterworth_2_low_pass_int(&ay_filtered) / 1024.0,
+ get_butterworth_2_low_pass_int(&az_filtered) / 1024.0,
+ stateGetBodyRates_f()->p,
+ stateGetBodyRates_f()->q,
+ stateGetBodyRates_f()->r,
+ stateGetNedToBodyEulers_f()->phi,
+ stateGetNedToBodyEulers_f()->theta,
+ stateGetNedToBodyEulers_f()->psi,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0);
+ MAVLinkSendMessage();
+}
+
diff --git a/sw/airborne/modules/sensors/cameras/jevois_mavlink.h b/sw/airborne/modules/sensors/cameras/jevois_mavlink.h
new file mode 100644
index 0000000000..016fa7a8a4
--- /dev/null
+++ b/sw/airborne/modules/sensors/cameras/jevois_mavlink.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) MAVLab
+ *
+ * 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 "modules/sensors/cameras/jevois_mavlink.h"
+ * @author MAVLab
+ * Send sensor data to jevois and read commands from jevois
+ */
+
+#ifndef JEVOIS_MAVLINK_H
+#define JEVOIS_MAVLINK_H
+
+
+
+
+/*
+ * Paparazzi UART over USB
+ */
+
+#include "mcu_periph/uart.h"
+
+#ifndef JEVOIS_DEV
+#define JEVOIS_DEV uart2
+#endif
+
+#define MAVLinkDev (&(JEVOIS_DEV).device)
+#define MAVLinkTransmit(c) MAVLinkDev->put_byte(MAVLinkDev->periph, 0, c)
+#define MAVLinkChAvailable() MAVLinkDev->char_available(MAVLinkDev->periph)
+#define MAVLinkGetch() MAVLinkDev->get_byte(MAVLinkDev->periph)
+#define MAVLinkSendMessage() MAVLinkDev->send_message(MAVLinkDev->periph, 0)
+
+
+
+
+/*
+ * MavLink protocol
+ */
+
+#include
+
+
+#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+#define MAVLINK_ALIGNED_FIELDS 0
+
+extern mavlink_system_t mavlink_system;
+
+static inline void comm_send_ch(mavlink_channel_t chan __attribute__((unused)), uint8_t ch)
+{
+ // Send bytes
+ MAVLinkTransmit(ch);
+}
+
+
+
+
+
+/*
+ * Paparazzi Module functions
+ */
+
+
+extern void jevois_mavlink_init(void);
+extern void jevois_mavlink_periodic(void);
+extern void jevois_mavlink_filter_periodic(void);
+extern void jevois_mavlink_event(void);
+
+
+
+
+
+
+
+
+
+
+#endif
+