From d6b27969537c3aa0a50b95dce6de0487bc70bd60 Mon Sep 17 00:00:00 2001
From: Open UAS <>
Date: Wed, 4 Apr 2018 21:29:17 +0200
Subject: [PATCH] Added more complete Parrot Disco
---
.../OPENUAS/openuas_parrot_disco.xml | 688 ++++++++++++++++++
conf/boards/disco.makefile | 5 +-
conf/modules/imu_disco.xml | 2 +-
conf/userconf/OPENUAS/openuas_conf.xml | 13 +-
sw/airborne/boards/disco/board.c | 68 +-
.../modules/computer_vision/video_thread.c | 27 +-
sw/airborne/modules/loggers/file_logger.c | 52 +-
sw/airborne/subsystems/imu/imu_disco.c | 8 +
8 files changed, 809 insertions(+), 54 deletions(-)
create mode 100644 conf/airframes/OPENUAS/openuas_parrot_disco.xml
diff --git a/conf/airframes/OPENUAS/openuas_parrot_disco.xml b/conf/airframes/OPENUAS/openuas_parrot_disco.xml
new file mode 100644
index 0000000000..e3506b962f
--- /dev/null
+++ b/conf/airframes/OPENUAS/openuas_parrot_disco.xml
@@ -0,0 +1,688 @@
+
+
+
+* in Fixedwing (https://www.openuas.org/airframes/)
++ Airframe to validate all onboard functionally...
+ Parrot Disco
+ + Autopilot: Parrot C.H.U.C.K.
+ + Actuators: Default Discon servos
+ + GPS: Default uBlox M8N GNSS
+ + RC RX: Spektrum DSMX RX620R Receiver with SBUS out
+ + AIRSPEED: Default
+ + TELEMETRY: Default Wifi
+ + CURRENT: ? V/I sensor?
+
+ NOTES:
+ + The first config will mimic the Exact Parrot Flight Envelope
+ Later on the maximum performance option will be added
+ and different setting if 5200mAh (heavier) battery is used (330g)
+ + There will be a flightplan mimicing exact default behaviour of Parrot assisted flight
+ (Or better, create one for the PPRZ team and make everyone happy)
+ + WARNING: This config is NOT tested in flight and NOT tuned yet
+ + Testing with Firmware v1.4.1 (and v1.3.0 before)
+ + Using Enahanced Total Energy control as control loop in Final AC
+ + Flashing the firmware via WiFi or USB
+ + Enlarged Battery Bay to fir a 5200 mAh 3S battery
+ This makes the airframe heavier, in the end we'll use adaptive etecs
+ + Regular Telemetry is possible via USB to FTDI serial to Modem
+
+ If testing "Classic" (as in not ETECS..) make sure to disable the settings/estimation/ac_char
+ Set the define USE_AIRSPEED should be set to FALSE for the first without airspeed in loop tests
+ Use also flightplan Versatile if no airspeed sensor is ues and use
+ flightplan versatile_airspeed only with enabled airspeed sensor
+
+ FIXME magneto 90Deg off in X/Y
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/boards/disco.makefile b/conf/boards/disco.makefile
index 718af7e673..aaec6e5a59 100644
--- a/conf/boards/disco.makefile
+++ b/conf/boards/disco.makefile
@@ -2,6 +2,7 @@
#
# disco.makefile
#
+# http://wiki.paparazziuav.org/wiki/Parrot_Disco
#
BOARD=disco
@@ -39,8 +40,8 @@ $(TARGET).srcs += $(SRC_BOARD)/board.c
# Compile the video specific parts
VIDEO_SRC = boards/bebop
-#$(TARGET).CFLAGS += -DI2C_BUF_LEN=56 -DUSE_I2C0
-#$(TARGET).srcs += $(VIDEO_SRC)/mt9v117.c $(VIDEO_SRC)/mt9f002.c modules/computer_vision/lib/isp/libisp.c modules/computer_vision/lib/isp/libisp_config.c
+$(TARGET).CFLAGS += -DI2C_BUF_LEN=56 -DUSE_I2C0
+$(TARGET).srcs += $(VIDEO_SRC)/mt9v117.c $(VIDEO_SRC)/mt9f002.c modules/computer_vision/lib/isp/libisp.c modules/computer_vision/lib/isp/libisp_config.c
# Link static (Done for GLIBC)
$(TARGET).CFLAGS += -DLINUX_LINK_STATIC
diff --git a/conf/modules/imu_disco.xml b/conf/modules/imu_disco.xml
index 15892bfb20..4a74a39bae 100644
--- a/conf/modules/imu_disco.xml
+++ b/conf/modules/imu_disco.xml
@@ -19,7 +19,7 @@
-
+
diff --git a/conf/userconf/OPENUAS/openuas_conf.xml b/conf/userconf/OPENUAS/openuas_conf.xml
index ae6354d81c..fa58924f0a 100644
--- a/conf/userconf/OPENUAS/openuas_conf.xml
+++ b/conf/userconf/OPENUAS/openuas_conf.xml
@@ -1,4 +1,15 @@
+
= v1.98
+ * Process running by default for firmware >= v1.0.5
*
* - /bin/sh - /usr/bin/DragonStarter.sh -out2null
* - //usr/bin/dragon-prog
diff --git a/sw/airborne/modules/computer_vision/video_thread.c b/sw/airborne/modules/computer_vision/video_thread.c
index 7ee15d55bf..bb72333d1a 100644
--- a/sw/airborne/modules/computer_vision/video_thread.c
+++ b/sw/airborne/modules/computer_vision/video_thread.c
@@ -1,5 +1,5 @@
/*
-* Copyright (C) 2015
+* Copyright (C) 2015 The Paparazzi Team
*
* This file is part of Paparazzi.
*
@@ -26,7 +26,6 @@
// Own header
#include "modules/computer_vision/video_thread.h"
-
#include
#include
#include
@@ -42,11 +41,11 @@
#include "mcu_periph/sys_time.h"
-// include board for bottom_camera and front_camera on ARDrone2 and Bebop
+// include board for bottom_camera and front_camera on ARDrone2, Bebop and Disco
#include BOARD_CONFIG
-// Bebop uses ISP
-#ifdef BOARD_BEBOP
+// Bebop and Disco can use the ISP (Image Signal Processors) to speed up thing
+#if defined(BOARD_BEBOP) || defined(BOARD_DISCO)
#include "lib/isp/libisp.h"
#endif
@@ -79,7 +78,6 @@ void video_thread_periodic(void)
/* currently no direct periodic functionality */
}
-
/**
* Handles all the video streaming and saving of the image shots
* This is a separate thread, so it needs to be thread safe!
@@ -95,7 +93,7 @@ static void *video_thread_function(void *data)
// create the images
if (vid->filters & VIDEO_FILTER_DEBAYER) {
- // fixme: don't hardcode size, works for bebop front camera for now
+ // fixme: don't hardcode size, works for Bebop front camera for now
#define IMG_FLT_SIZE 272
image_create(&img_color, IMG_FLT_SIZE, IMG_FLT_SIZE, IMAGE_YUV422);
}
@@ -106,14 +104,14 @@ static void *video_thread_function(void *data)
return 0;
}
-#ifdef BOARD_BEBOP
+#if defined(BOARD_BEBOP) || defined(BOARD_DISCO)
// Configure ISP if needed
if (vid->filters & VIDEO_FILTER_ISP) {
configure_isp(vid->thread.dev);
}
#endif
- // be nice to the more important stuff
+ // Be nice to the more important stuff
set_nice_level(VIDEO_THREAD_NICE_LEVEL);
fprintf(stdout, "[%s] Set nice level to %i.\n", print_tag, VIDEO_THREAD_NICE_LEVEL);
@@ -126,7 +124,7 @@ static void *video_thread_function(void *data)
vid->thread.is_running = true;
while (vid->thread.is_running) {
- // get time in us since last run
+ // Get time in us since last run
clock_gettime(CLOCK_MONOTONIC, &time_now);
uint32_t dt_us = sys_time_elapsed_us(&time_prev, &time_now);
time_prev = time_now;
@@ -145,10 +143,10 @@ static void *video_thread_function(void *data)
struct image_t img;
v4l2_image_get(vid->thread.dev, &img);
- // pointer to the final image to pass for saving and further processing
+ // Pointer to the final image to pass for saving and further processing
struct image_t *img_final = &img;
- // run selected filters
+ // Run selected filters
if (vid->filters & VIDEO_FILTER_DEBAYER) {
BayerToYUV(&img, &img_color, 0, 0);
// use color image for further processing
@@ -171,7 +169,7 @@ static bool initialize_camera(struct video_config_t *camera)
{
// Initialize the V4L2 subdevice if needed
if (camera->subdev_name != NULL) {
- // FIXME! add subdev format to config, only needed on bebop front camera so far
+ // FIXME! add subdev format to config, only needed on Bebop/Disco(?) front camera so far
if (!v4l2_init_subdev(camera->subdev_name, 0, camera->subdev_format, camera->sensor_size)) {
printf("[video_thread] Could not initialize the %s subdevice.\n", camera->subdev_name);
return false;
@@ -185,7 +183,7 @@ static bool initialize_camera(struct video_config_t *camera)
return false;
}
- // Initialize OK
+ // Initialized just fine
return true;
}
@@ -278,7 +276,6 @@ void video_thread_start()
}
}
-
/**
* Stops the streaming of all cameras
* This could take some time, because the thread is stopped asynchronous.
diff --git a/sw/airborne/modules/loggers/file_logger.c b/sw/airborne/modules/loggers/file_logger.c
index 1019d08646..2a61ccf8df 100644
--- a/sw/airborne/modules/loggers/file_logger.c
+++ b/sw/airborne/modules/loggers/file_logger.c
@@ -22,6 +22,7 @@
/** @file modules/loggers/file_logger.c
* @brief File logger for Linux based autopilots
+ * This module purpose is for debugging airframest
*/
#include "file_logger.h"
@@ -30,7 +31,17 @@
#include "std.h"
#include "subsystems/imu.h"
+
+/* Ugly fix until Fixedwing and rotorcraft are uniofied in a nice way
+ * COMMAND_THRUST only and always defined in ROTORCRAFT
+ * A gliding Rotorcraft is an ecxeption ;) */
+#ifdef COMMAND_THRUST
#include "firmwares/rotorcraft/stabilization.h"
+#else
+/* adaptive also an option */
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#endif
+
#include "state.h"
/** Set the default File logger path to the USB drive */
@@ -61,7 +72,12 @@ void file_logger_start(void)
if (file_logger != NULL) {
fprintf(
file_logger,
+ /* Add and removed values here as wished to log */
+#ifdef COMMAND_THRUST
"counter,gyro_unscaled_p,gyro_unscaled_q,gyro_unscaled_r,accel_unscaled_x,accel_unscaled_y,accel_unscaled_z,mag_unscaled_x,mag_unscaled_y,mag_unscaled_z,COMMAND_THRUST,COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,qi,qx,qy,qz\n"
+#else
+ "counter,gyro_unscaled_p,gyro_unscaled_q,gyro_unscaled_r,accel_unscaled_x,accel_unscaled_y,accel_unscaled_z,mag_unscaled_x,mag_unscaled_y,mag_unscaled_z,qi,qx,qy,qz\n"
+#endif
);
}
}
@@ -75,7 +91,7 @@ void file_logger_stop(void)
}
}
-/** Log the values to a csv file */
+/** Log the values to a CSV file */
void file_logger_periodic(void)
{
if (file_logger == NULL) {
@@ -84,6 +100,8 @@ void file_logger_periodic(void)
static uint32_t counter;
struct Int32Quat *quat = stateGetNedToBodyQuat_i();
+ /* Add and removed values here as wished to log */
+#ifdef COMMAND_THRUST
fprintf(file_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
counter,
imu.gyro_unscaled.p,
@@ -96,6 +114,7 @@ void file_logger_periodic(void)
imu.mag_unscaled.y,
imu.mag_unscaled.z,
stabilization_cmd[COMMAND_THRUST],
+ stabilization_cmd[COMMAND_THROTTLE],
stabilization_cmd[COMMAND_ROLL],
stabilization_cmd[COMMAND_PITCH],
stabilization_cmd[COMMAND_YAW],
@@ -104,5 +123,36 @@ void file_logger_periodic(void)
quat->qy,
quat->qz
);
+#else
+ fprintf(file_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+ counter,
+ imu.gyro_unscaled.p,
+ imu.gyro_unscaled.q,
+ imu.gyro_unscaled.r,
+ imu.accel_unscaled.x,
+ imu.accel_unscaled.y,
+ imu.accel_unscaled.z,
+ imu.mag_unscaled.x,
+ imu.mag_unscaled.y,
+ imu.mag_unscaled.z,
+ /* imu.gyro_scaled.p,
+ imu.gyro_scaled.q,
+ imu.gyro_scaled.r,
+ imu.accel_scaled.x,
+ imu.accel_scaled.y,
+ imu.accel_scaled.z,
+ imu.mag_scaled.x,
+ imu.mag_scaled.y,
+ imu.mag_scaled.z, */
+ //stabilization_cmd[COMMAND_THROTTLE],
+ //stabilization_cmd[COMMAND_ROLL],
+ //stabilization_cmd[COMMAND_PITCH],
+ ////stabilization_cmd[COMMAND_PITCH], //for time being...
+ quat->qi,
+ quat->qx,
+ quat->qy,
+ quat->qz
+ );
+#endif
counter++;
}
diff --git a/sw/airborne/subsystems/imu/imu_disco.c b/sw/airborne/subsystems/imu/imu_disco.c
index bd2bb6bc98..626d004201 100644
--- a/sw/airborne/subsystems/imu/imu_disco.c
+++ b/sw/airborne/subsystems/imu/imu_disco.c
@@ -44,8 +44,16 @@ PRINT_CONFIG_VAR(DISCO_MPU_I2C_DEV)
#define DISCO_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define DISCO_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
+#elif PERIODIC_FREQUENCY == 512
+/* Accelerometer: Bandwidth 260Hz, Delay 0ms
+ * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
+ */
+#define DISCO_LOWPASS_FILTER MPU60X0_DLPF_256HZ
+#define DISCO_SMPLRT_DIV 3
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#endif
#endif
+
PRINT_CONFIG_VAR(DISCO_SMPLRT_DIV)
PRINT_CONFIG_VAR(DISCO_LOWPASS_FILTER)