8 changes: 4 additions & 4 deletions sw/airborne/modules/ins/imu_xsens.c
View file
Expand Up @@ -61,7 +61,7 @@ static void handle_ins_msg(void)
RATE_BFP_OF_REAL(xsens.gyro.r)
};
xsens.gyro_available = FALSE
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN);
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN, NAN); //FIXME: samplerate?
}
if (xsens.accel_available) {
struct Int32Vect3 accel = {
Expand All @@ -70,7 +70,7 @@ static void handle_ins_msg(void)
ACCEL_BFP_OF_REAL(xsens.accel.z)
};
xsens.accel_available = FALSE;
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN);
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN, NAN); //FIXME: samplerate?
}
if (xsens.mag_available) {
struct Int32Vect3 mag = {
Expand All @@ -88,7 +88,7 @@ static void handle_ins_msg(void)
RATE_BFP_OF_REAL(xsens.gyro.q),
RATE_BFP_OF_REAL(xsens.gyro.r)
};
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN);
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN, NAN); //FIXME: samplerate?
xsens.gyro_available = FALSE;
}
if (xsens.accel_available) {
Expand All @@ -97,7 +97,7 @@ static void handle_ins_msg(void)
ACCEL_BFP_OF_REAL(xsens.accel.y),
ACCEL_BFP_OF_REAL(xsens.accel.z)
};
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN);
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN, NAN); //FIXME: samplerate?
xsens.accel_available = FALSE;
}
if (xsens.mag_available) {
Expand Down
1,559 changes: 1,559 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.cpp
View file

Large diffs are not rendered by default.

145 changes: 145 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.h
View file
@@ -0,0 +1,145 @@
#ifndef EKF_AW_H
#define EKF_AW_H

#ifdef __cplusplus
extern "C" {
#endif

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"

// Settings
struct ekfAwParameters {
// Q
float Q_accel; ///< accel process noise
float Q_gyro; ///< gyro process noise
float Q_mu; ///< wind process noise
float Q_k; ///< offset process noise

// R
float R_V_gnd; ///< speed measurement noise
float R_accel_filt[3]; ///< filtered accel measurement noise
float R_V_pitot; ///< airspeed measurement noise

// Model Based Parameters
float vehicle_mass;

// X Axis
float k_fx_drag[2]; // Temporary setting for fuselage + hover prop
float k_fx_fuselage[4];
float k_fx_hover[3];
float k_fx_wing[5];
float k_fx_push[3];
float k_fx_elev[3];

// Y Axis
float k_fy_beta;
float k_fy_v;
float k_fy_wing[5];

// Z Axis
float k_fz_fuselage[4];
float k_fz_wing[4];
float k_fz_hover[5];
float k_fz_elev[2];

// Other options
bool use_model[3];
bool use_pitot;
bool propagate_offset;
bool quick_convergence;
};

struct ekfHealth{
bool healthy;
uint16_t crashes_n;
};

extern struct ekfAwParameters ekf_aw_params;

// Init functions
extern void ekf_aw_init(void);
extern void ekf_aw_reset(void);

// Filtering functions
extern void ekf_aw_propagate(struct FloatVect3 *acc,struct FloatRates *gyro, struct FloatEulers *euler, float *pusher_RPM,float *hover_RPM_array, float *skew, float *elevator_angle, struct FloatVect3 * V_gnd, struct FloatVect3 *acc_filt, float *V_pitot,float dt);

// Getter/Setter functions
extern struct NedCoor_f ekf_aw_get_speed_body(void);
extern struct NedCoor_f ekf_aw_get_wind_ned(void);
extern struct NedCoor_f ekf_aw_get_offset(void);
extern struct FloatVect3 ekf_aw_get_innov_V_gnd(void);
extern struct FloatVect3 ekf_aw_get_innov_accel_filt(void);
extern float ekf_aw_get_innov_V_pitot(void);
extern void ekf_aw_get_meas_cov(float meas_cov[7]);
extern void ekf_aw_get_state_cov(float state_cov[9]);
extern void ekf_aw_get_process_cov(float process_cov[9]);
extern void ekf_aw_get_fuselage_force(float force[3]);
extern void ekf_aw_get_wing_force(float force[3]);
extern void ekf_aw_get_elevator_force(float force[3]);
extern void ekf_aw_get_hover_force(float force[3]);
extern void ekf_aw_get_pusher_force(float force[3]);
extern struct ekfAwParameters *ekf_aw_get_param_handle(void);

extern void ekf_aw_set_speed_body(struct NedCoor_f *s);
extern void ekf_aw_set_wind(struct NedCoor_f *s);
extern void ekf_aw_set_offset(struct NedCoor_f *s);
extern struct ekfHealth ekf_aw_get_health(void);

// Settings handlers
extern void ekf_aw_update_params(void);
extern void ekf_aw_reset_health(void);


// Handlers
#define ekf_aw_update_Q_accel(_v) { \
ekf_aw_params.Q_accel = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_gyro(_v) { \
ekf_aw_params.Q_gyro = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_mu(_v) { \
ekf_aw_params.Q_mu = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_k(_v) { \
ekf_aw_params.Q_k = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_gnd(_v) { \
ekf_aw_params.R_V_gnd = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_x(_v) { \
ekf_aw_params.R_accel_filt[0] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_y(_v) { \
ekf_aw_params.R_accel_filt[1] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_z(_v) { \
ekf_aw_params.R_accel_filt[2] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_pitot(_v) { \
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}

#ifdef __cplusplus
}
#endif

#endif /* EKF_AW_H */
466 changes: 466 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.c
View file

Large diffs are not rendered by default.

117 changes: 117 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.h
View file
@@ -0,0 +1,117 @@
#ifndef EKF_AW_WRAPPER_H
#define EKF_AW_WRAPPER_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "modules/meteo/ekf_aw.h"


// EKF structure
struct ekfAw {

// States
struct NedCoor_f wind;
struct NedCoor_f V_body;
struct NedCoor_f offset;

// Inputs
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles

int32_t last_RPM_hover[4]; // Value obtained from ABI Callback
int32_t last_RPM_pusher; // Value obtained from ABI Callback
float RPM_hover[4]; /// Hover motor RPM
float RPM_pusher; /// Pusher motor RPM
float skew; /// Skew
float elevator_angle;


// Measurements
struct FloatVect3 Vg_NED; /// Ground Speed
struct FloatVect3 acc_filt; ///< Last accelerometer measurements
float V_pitot; /// Pitot tube airspeed

// Innovation
struct FloatVect3 innov_V_gnd;
struct FloatVect3 innov_acc_filt;
float innov_V_pitot;

// Covariance
float meas_cov[7];
float state_cov[9];
float process_cov[12];

// Forces
float fuselage_force[3];
float wing_force[3];
float elevator_force[3];
float hover_force[3];
float pusher_force[3];

// Other
bool reset;
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
bool override_start;
bool override_quick_convergence;

};

extern void ekf_aw_wrapper_init(void);
extern void ekf_aw_wrapper_periodic(void);
extern void ekf_aw_wrapper_fetch(void);

extern void set_in_air_status(bool);


extern float tau_filter_high;
extern float tau_filter_low;

extern struct ekfAw ekf_aw;

// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}

#define ekf_aw_wrapper_set_wind_N(_v) { \
ekf_aw.wind_guess.x = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_E(_v) { \
ekf_aw.wind_guess.y = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_D(_v) { \
ekf_aw.wind_guess.z = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_offset_x(_v) { \
ekf_aw.offset_guess.x = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_y(_v) { \
ekf_aw.offset_guess.y = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_z(_v) { \
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#endif /* EKF_AW_WRAPPER_H */
1 change: 1 addition & 0 deletions sw/airborne/modules/nav/nav_survey_hybrid.c
View file
Expand Up @@ -29,6 +29,7 @@
#include "modules/nav/nav_survey_hybrid.h"

#include "firmwares/rotorcraft/navigation.h"
#include "modules/nav/nav_rotorcraft_base.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "autopilot.h"
Expand Down
123 changes: 123 additions & 0 deletions sw/airborne/modules/sensors/airspeed_uavcan.c
View file
@@ -0,0 +1,123 @@
/*
* Copyright (C) 2023 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/sensors/airspeed_uavcan.c
* Airspeed sensor on the uavcan bus
*/

#include "airspeed_uavcan.h"
#include "uavcan/uavcan.h"
#include "core/abi.h"
#include "filters/low_pass_filter.h"

#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
#endif

#ifndef AIRSPEED_UAVCAN_LOWPASS_TAU
#define AIRSPEED_UAVCAN_LOWPASS_TAU 0.15
#endif

#ifndef AIRSPEED_UAVCAN_LOWPASS_PERIOD
#define AIRSPEED_UAVCAN_LOWPASS_PERIOD 0.1
#endif

#ifndef AIRSPEED_UAVCAN_SEND_ABI
#define AIRSPEED_UAVCAN_SEND_ABI 1
#endif

#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
static Butterworth2LowPass airspeed_filter;
#endif
static uavcan_event airspeed_uavcan_ev;

/* uavcan EQUIPMENT_ESC_STATUS message definition */
#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID 1027
#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE (0xC77DF38BA122F5DAULL)
#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE ((397 + 7)/8)

struct airspeed_uavcan_s airspeed_uavcan;

static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_AIRSPEED_UAVCAN(trans,dev,AC_ID,
&airspeed_uavcan.diff_p,
&airspeed_uavcan.temperature);
}

static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
uint16_t tmp_float = 0;
float diff_p;

/* Decode the message */
//canardDecodeScalar(transfer, (uint32_t)0, 8, false, (void*)&dest->flags);
//canardDecodeScalar(transfer, (uint32_t)8, 32, false, (void*)&static_p);
canardDecodeScalar(transfer, (uint32_t)40, 32, false, (void*)&diff_p);
//canardDecodeScalar(transfer, (uint32_t)72, 16, false, (void*)&tmp_float);
//float static_temp = canardConvertFloat16ToNativeFloat(tmp_float);
//canardDecodeScalar(transfer, (uint32_t)88, 16, false, (void*)&tmp_float);
//float diff_temp = canardConvertFloat16ToNativeFloat(tmp_float);
canardDecodeScalar(transfer, (uint32_t)104, 16, false, (void*)&tmp_float);
float static_air_temp = canardConvertFloat16ToNativeFloat(tmp_float);
//canardDecodeScalar(transfer, (uint32_t)120, 16, false, (void*)&tmp_float);
//float pitot_temp = canardConvertFloat16ToNativeFloat(tmp_float);

if(!isnan(diff_p)) {
#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
float diff_p_filt = update_butterworth_2_low_pass(&airspeed_filter, diff_p);
airspeed_uavcan.diff_p = diff_p_filt;
#else
airspeed_uavcan.diff_p = diff_p;
#endif

#if AIRSPEED_UAVCAN_SEND_ABI
AbiSendMsgBARO_DIFF(UAVCAN_SENDER_ID, airspeed_uavcan.diff_p);
#endif
}

if(!isnan(static_air_temp)) {
airspeed_uavcan.temperature = static_air_temp;
#if AIRSPEED_UAVCAN_SEND_ABI
AbiSendMsgTEMPERATURE(UAVCAN_SENDER_ID, airspeed_uavcan.temperature);
#endif
}
}

void airspeed_uavcan_init(void)
{
// Setup low pass filter with time constant and 100Hz sampling freq
#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
init_butterworth_2_low_pass(&airspeed_filter, AIRSPEED_UAVCAN_LOWPASS_TAU, AIRSPEED_UAVCAN_LOWPASS_PERIOD, 0);
#endif

airspeed_uavcan.diff_p = 0;
airspeed_uavcan.temperature = 0;

// Bind uavcan RAWAIRDATA message from EQUIPMENT.AIR_DATA
uavcan_bind(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, &airspeed_uavcan_ev, &airspeed_uavcan_cb);

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_UAVCAN, airspeed_uavcan_downlink);
#endif
}
40 changes: 40 additions & 0 deletions sw/airborne/modules/sensors/airspeed_uavcan.h
View file
@@ -0,0 +1,40 @@
/*
* Copyright (C) 2023 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/sensors/airspeed_uavcan.h
* Airspeed sensor on the uavcan bus
*/

#ifndef AIRSPEED_UAVCAN_H
#define AIRSPEED_UAVCAN_H

/* External variables */
extern struct airspeed_uavcan_s {
float diff_p;
float temperature;
} airspeed_uavcan;

/* External functions */
extern void airspeed_uavcan_init(void);



#endif /* AIRSPEED_UAVCAN_H */
48 changes: 48 additions & 0 deletions sw/airborne/peripherals/bmi088.c
View file
Expand Up @@ -115,6 +115,54 @@ void bmi088_send_config(Bmi088ConfigSet bmi_set, void *bmi, struct Bmi088Config
config->init_status++;
break;
case BMI088_CONF_DONE:
/* Set the samplerates from the ODR */
switch(config->gyro_odr) {
case BMI088_GYRO_ODR_2000_BW_532:
case BMI088_GYRO_ODR_2000_BW_230:
config->gyro_samplerate = 2000;
break;
case BMI088_GYRO_ODR_1000_BW_116:
config->gyro_samplerate = 1000;
break;
case BMI088_GYRO_ODR_400_BW_47:
config->gyro_samplerate = 400;
break;
case BMI088_GYRO_ODR_200_BW_23:
case BMI088_GYRO_ODR_200_BW_64:
config->gyro_samplerate = 200;
break;
case BMI088_GYRO_ODR_100_BW_12:
case BMI088_GYRO_ODR_100_BW_32:
config->gyro_samplerate = 100;
break;
}
switch(config->accel_odr) {
case BMI088_ACCEL_ODR_1600:
config->accel_samplerate = 1600;
break;
case BMI088_ACCEL_ODR_800:
config->accel_samplerate = 800;
break;
case BMI088_ACCEL_ODR_400:
config->accel_samplerate = 400;
break;
case BMI088_ACCEL_ODR_200:
config->accel_samplerate = 200;
break;
case BMI088_ACCEL_ODR_100:
config->accel_samplerate = 100;
break;
case BMI088_ACCEL_ODR_50:
config->accel_samplerate = 50;
break;
case BMI088_ACCEL_ODR_25:
config->accel_samplerate = 25;
break;
case BMI088_ACCEL_ODR_12:
config->accel_samplerate = 12;
break;
}

config->initialized = true;
break;
default:
Expand Down
3 changes: 3 additions & 0 deletions sw/airborne/peripherals/bmi088.h
View file
Expand Up @@ -119,6 +119,9 @@ struct Bmi088Config {
enum Bmi088AccelBW accel_bw; ///< bandwidth
enum Bmi088ConfStatus init_status; ///< init status
bool initialized; ///< config done flag

float gyro_samplerate; ///< samplerate in Hz from gyro_odr
float accel_samplerate; ///< samplerate in Hz from accel_odr
};

extern void bmi088_set_default_config(struct Bmi088Config *c);
Expand Down
6 changes: 3 additions & 3 deletions sw/airborne/peripherals/invensense2.c
View file
Expand Up @@ -74,7 +74,7 @@ static const struct Int32Vect3 invensense2_accel_scale[5][2] = {
/**
* @brief Initialize the invensense v2 sensor instance
*
* @param inv The structure containing the configuratio of the invensense v2 instance
* @param inv The structure containing the configuration of the invensense v2 instance
*/
void invensense2_init(struct invensense2_t *inv) {
/* General setup */
Expand Down Expand Up @@ -314,8 +314,8 @@ static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *

// Send the scaled values over ABI
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples, temp_f);
AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, temp_f);
AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples, gyro_samplerate, temp_f);
AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, accel_samplerate, temp_f);
}

/**
Expand Down
2 changes: 0 additions & 2 deletions sw/airborne/peripherals/invensense2.h
View file
Expand Up @@ -139,8 +139,6 @@ struct invensense2_t {
enum invensense2_gyro_range_t gyro_range; ///< Gyro range configuration
enum invensense2_accel_dlpf_t accel_dlpf; ///< Accelerometer DLPF configuration
enum invensense2_accel_range_t accel_range; ///< Accelerometer range configuration

// float temp; ///< temperature in degrees Celcius
};

/* External functions */
Expand Down
990 changes: 990 additions & 0 deletions sw/airborne/peripherals/invensense3.c
View file

Large diffs are not rendered by default.

192 changes: 192 additions & 0 deletions sw/airborne/peripherals/invensense3.h
View file
@@ -0,0 +1,192 @@
/*
* Copyright (C) 2022 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 peripherals/invensense3.h
*
* Driver for the Invensense V3 IMUs
* ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688
*/

#ifndef INVENSENSE3_H
#define INVENSENSE3_H

#include "std.h"
#include "mcu_periph/spi.h"
#include "mcu_periph/i2c.h"

/* This sensor has an option to request little-endian data */
// Hold 22 measurements + 3 for the register address and length
#define INVENSENSE3_FIFO_BUFFER_LEN 22
#define INVENSENSE3_BUFFER_SIZE INVENSENSE3_FIFO_BUFFER_LEN * 20 + 3 // 20 bytes is the maximum sample size

/* Invensense v3 SPI peripheral */
struct invensense3_spi_t {
struct spi_periph *p; ///< Peripheral device for communication
uint8_t slave_idx; ///< Slave index used for Slave Select
struct spi_transaction trans; ///< Transaction used during configuration and measurements

uint8_t tx_buf[2]; ///< Transmit buffer
uint8_t rx_buf[INVENSENSE3_BUFFER_SIZE]; ///< Receive buffer
};

/* Invensense v3 I2C peripheral */
struct invensense3_i2c_t {
struct i2c_periph *p; ///< Peripheral device for communication
uint8_t slave_addr; ///< The I2C slave address on the bus
struct i2c_transaction trans; ///< TRansaction used during configuration and measurements
};

/* Possible communication busses for the invense V3 driver */
enum invensense3_bus_t {
INVENSENSE3_SPI,
INVENSENSE3_I2C
};

/* Different states the invensense v3 driver can be in */
enum invensense3_status_t {
INVENSENSE3_IDLE,
INVENSENSE3_CONFIG,
INVENSENSE3_RUNNING
};

/* Different parsers of the invensense v3 driver */
enum invensense3_parser_t {
INVENSENSE3_PARSER_REGISTERS,
INVENSENSE3_PARSER_FIFO
};

enum invensense3_fifo_packet_t {
INVENSENSE3_SAMPLE_SIZE_PK1,
INVENSENSE3_SAMPLE_SIZE_PK2,
INVENSENSE3_SAMPLE_SIZE_PK3,
INVENSENSE3_SAMPLE_SIZE_PK4
};

/* Different device types compatible with the invensense v3 driver */
enum invensense3_device_t {
INVENSENSE3_UNKOWN,
INVENSENSE3_ICM40605,
INVENSENSE3_ICM40609,
INVENSENSE3_ICM42605,
INVENSENSE3_IIM42652,
INVENSENSE3_ICM42688
};

/* The gyro output data rate configuration */
enum invensense3_gyro_odr_t {
INVENSENSE3_GYRO_ODR_32KHZ = 1, ///< Not possible for ICM40605 and ICM42605
INVENSENSE3_GYRO_ODR_16KHZ, ///< Not possible for ICM40605 and ICM42605
INVENSENSE3_GYRO_ODR_8KHZ,
INVENSENSE3_GYRO_ODR_4KHZ,
INVENSENSE3_GYRO_ODR_2KHZ,
INVENSENSE3_GYRO_ODR_1KHZ,
INVENSENSE3_GYRO_ODR_200HZ,
INVENSENSE3_GYRO_ODR_100HZ,
INVENSENSE3_GYRO_ODR_50HZ,
INVENSENSE3_GYRO_ODR_25HZ,
INVENSENSE3_GYRO_ODR_12_5HZ,
INVENSENSE3_GYRO_ODR_6_25HZ,
INVENSENSE3_GYRO_ODR_3_125HZ,
INVENSENSE3_GYRO_ODR_1_5625HZ,
INVENSENSE3_GYRO_ODR_500HZ
};

/* The gyro range in degrees per second(dps) */
enum invensense3_gyro_range_t {
INVENSENSE3_GYRO_RANGE_2000DPS,
INVENSENSE3_GYRO_RANGE_1000DPS,
INVENSENSE3_GYRO_RANGE_500DPS,
INVENSENSE3_GYRO_RANGE_250DPS,
INVENSENSE3_GYRO_RANGE_125DPS,
INVENSENSE3_GYRO_RANGE_62_5DPS,
INVENSENSE3_GYRO_RANGE_31_25DPS,
INVENSENSE3_GYRO_RANGE_15_625DPS
};

/* The accelerometer output data rate configuration */
enum invensense3_accel_odr_t {
INVENSENSE3_ACCEL_ODR_32KHZ = 1, ///< Not possible for ICM40605 and ICM42605
INVENSENSE3_ACCEL_ODR_16KHZ, ///< Not possible for ICM40605 and ICM42605
INVENSENSE3_ACCEL_ODR_8KHZ,
INVENSENSE3_ACCEL_ODR_4KHZ,
INVENSENSE3_ACCEL_ODR_2KHZ,
INVENSENSE3_ACCEL_ODR_1KHZ,
INVENSENSE3_ACCEL_ODR_200HZ,
INVENSENSE3_ACCEL_ODR_100HZ,
INVENSENSE3_ACCEL_ODR_50HZ,
INVENSENSE3_ACCEL_ODR_25HZ,
INVENSENSE3_ACCEL_ODR_12_5HZ,
INVENSENSE3_ACCEL_ODR_6_25HZ,
INVENSENSE3_ACCEL_ODR_3_125HZ,
INVENSENSE3_ACCEL_ODR_1_5625HZ,
INVENSENSE3_ACCEL_ODR_500HZ
};

/* The accelerometer range in G */
enum invensense3_accel_range_t {
INVENSENSE3_ACCEL_RANGE_32G, ///< Only possible for ICM40609
INVENSENSE3_ACCEL_RANGE_16G,
INVENSENSE3_ACCEL_RANGE_8G,
INVENSENSE3_ACCEL_RANGE_4G,
INVENSENSE3_ACCEL_RANGE_2G
};

/* Main invensense v3 device structure */
struct invensense3_t {
uint8_t abi_id; ///< The ABI id used to broadcast the device measurements
enum invensense3_status_t status; ///< Status of the invensense v3 device
enum invensense3_device_t device; ///< The device type detected
enum invensense3_parser_t parser; ///< Parser of the device

enum invensense3_bus_t bus; ///< The communication bus used to connect the device SPI/I2C
union {
struct invensense3_spi_t spi; ///< SPI specific configuration
struct invensense3_i2c_t i2c; ///< I2C specific configuration
};
uint8_t* rx_buffer;
uint8_t* tx_buffer;
uint16_t* rx_length;

uint8_t register_bank; ///< The current register bank communicating with
uint8_t config_idx; ///< The current configuration index
uint32_t timer; ///< Used to time operations during configuration (samples left during measuring)

enum invensense3_gyro_odr_t gyro_odr; ///< Gyro Output Data Rate configuration
enum invensense3_gyro_range_t gyro_range; ///< Gyro range configuration
uint16_t gyro_aaf; ///< Gyro Anti-alias filter 3dB Bandwidth configuration [Hz]
uint16_t gyro_aaf_regs[4]; ///< Gyro Anti-alias filter register values
enum invensense3_accel_odr_t accel_odr; ///< Accelerometer Output Data Rate configuration
enum invensense3_accel_range_t accel_range; ///< Accelerometer range configuration
uint16_t accel_aaf; ///< Accelerometer Anti-alias filter 3dB Bandwidth configuration [Hz]
uint16_t accel_aaf_regs[4]; ///< Accelerometer Anti-alias filter register values
enum invensense3_fifo_packet_t sample_size; ///< FIFO packet size
int sample_numbers; ///< expected FIFO packet number, assuming reading at PERIODIC_FREQUENCY
float gyro_samplerate; ///< Sample rate in Hz from the gyro_odr
float accel_samplerate; ///< Sample rate in Hz from the accel_odr
};

/* External functions */
void invensense3_init(struct invensense3_t *inv);
void invensense3_periodic(struct invensense3_t *inv);
void invensense3_event(struct invensense3_t *inv);

#endif // INVENSENSE3_H
190 changes: 190 additions & 0 deletions sw/airborne/peripherals/invensense3_regs.h
View file
@@ -0,0 +1,190 @@
/*
* Copyright (C) 2022 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 peripherals/invensense3_regs.h
*
* Register and address definitions for the Invensense V3 from ardupilot.
*/

#ifndef INVENSENSE3_REGS_H
#define INVENSENSE3_REGS_H

#define INV3_BANK0 0x00U
#define INV3_BANK1 0x01U
#define INV3_BANK2 0x02U
#define INV3_BANK3 0x03U


#define INV3REG(b, r) ((((uint16_t)b) << 8)|(r))
#define INV3_READ_FLAG 0x80

//Register Map
#define INV3REG_DEVICE_CONFIG INV3REG(INV3_BANK0,0x11U)
# define BIT_DEVICE_CONFIG_SOFT_RESET_CONFIG 0x01
# define BIT_DEVICE_CONFIG_SPI_MODE 0x10
#define INV3REG_FIFO_CONFIG INV3REG(INV3_BANK0,0x16U)
# define FIFO_CONFIG_MODE_BYPASS 0x00
# define FIFO_CONFIG_MODE_STREAM_TO_FIFO 0x01
# define FIFO_CONFIG_MODE_STOP_ON_FULL 0x02
# define FIFO_CONFIG_MODE_SHIFT 0x06
#define INV3REG_TEMP_DATA1 INV3REG(INV3_BANK0,0x1DU)
#define INV3REG_ACCEL_DATA_X1 INV3REG(INV3_BANK0,0x1FU)
#define INV3REG_INT_STATUS INV3REG(INV3_BANK0,0x2DU)
#define INV3REG_FIFO_COUNTH INV3REG(INV3_BANK0,0x2EU)
#define INV3REG_FIFO_COUNTL INV3REG(INV3_BANK0,0x2FU)
#define INV3REG_FIFO_DATA INV3REG(INV3_BANK0,0x30U)
#define INV3REG_SIGNAL_PATH_RESET INV3REG(INV3_BANK0,0x4BU)
# define BIT_SIGNAL_PATH_RESET_FIFO_FLUSH 0x02
# define BIT_SIGNAL_PATH_RESET_TMST_STROBE 0x04
# define BIT_SIGNAL_PATH_RESET_ABORT_AND_RESET 0x08
# define BIT_SIGNAL_PATH_RESET_DMP_MEM_RESET_EN 0x20
# define BIT_SIGNAL_PATH_RESET_DMP_INIT_EN 0x40
#define INV3REG_INTF_CONFIG0 INV3REG(INV3_BANK0,0x4CU)
# define UI_SIFS_CFG_SPI_DIS 0x02
# define UI_SIFS_CFG_I2C_DIS 0x03
# define UI_SIFS_CFG_SHIFT 0x00
# define SENSOR_DATA_BIG_ENDIAN 0x10
# define FIFO_COUNT_BIG_ENDIAN 0x20
# define FIFO_COUNT_REC 0x40
# define FIFO_HOLD_LAST_DATA_EN 0x80
#define INV3REG_PWR_MGMT0 INV3REG(INV3_BANK0,0x4EU)
# define ACCEL_MODE_OFF 0x00
# define ACCEL_MODE_LN 0x03
# define ACCEL_MODE_SHIFT 0x00
# define GYRO_MODE_OFF 0x00
# define GYRO_MODE_LN 0x03
# define GYRO_MODE_SHIFT 0x02
# define BIT_PWR_MGMT_IDLE 0x08
# define BIT_PWM_MGMT_TEMP_DIS 0x10
#define INV3REG_GYRO_CONFIG0 INV3REG(INV3_BANK0,0x4FU)
# define GYRO_ODR_32KHZ 0x01
# define GYRO_ODR_16KHZ 0x02
# define GYRO_ODR_8KHZ 0x03
# define GYRO_ODR_4KHZ 0x04
# define GYRO_ODR_2KHZ 0x05
# define GYRO_ODR_1KHZ 0x06
# define GYRO_ODR_200HZ 0x07
# define GYRO_ODR_100HZ 0x08
# define GYRO_ODR_50HZ 0x09
# define GYRO_ODR_25HZ 0x0A
# define GYRO_ODR_12_5HZ 0x0B
# define GYRO_ODR_500HZ 0x0F
# define GYRO_ODR_SHIFT 0x00
# define GYRO_FS_SEL_2000DPS 0x00
# define GYRO_FS_SEL_1000DPS 0x01
# define GYRO_FS_SEL_500DPS 0x02
# define GYRO_FS_SEL_250DPS 0x03
# define GYRO_FS_SEL_125DPS 0x04
# define GYRO_FS_SEL_62_5DPS 0x05
# define GYRO_FS_SEL_31_25DPS 0x06
# define GYRO_FS_SEL_15_625DPS 0x07
# define GYRO_FS_SEL_SHIFT 0x05
#define INV3REG_ACCEL_CONFIG0 INV3REG(INV3_BANK0,0x50U)
# define ACCEL_ODR_32KHZ 0x01
# define ACCEL_ODR_16KHZ 0x02
# define ACCEL_ODR_8KHZ 0x03
# define ACCEL_ODR_4KHZ 0x04
# define ACCEL_ODR_2KHZ 0x05
# define ACCEL_ODR_1KHZ 0x06
# define ACCEL_ODR_200HZ 0x07
# define ACCEL_ODR_100HZ 0x08
# define ACCEL_ODR_50HZ 0x09
# define ACCEL_ODR_25HZ 0x0A
# define ACCEL_ODR_12_5HZ 0x0B
# define ACCEL_ODR_6_25HZ 0x0C
# define ACCEL_ODR_3_125HZ 0x0D
# define ACCEL_ODR_1_5625HZ 0x0E
# define ACCEL_ODR_500HZ 0x0F
# define ACCEL_ODR_SHIFT 0x00
# define ACCEL_FS_SEL_16G 0x00
# define ACCEL_FS_SEL_8G 0x01
# define ACCEL_FS_SEL_4G 0x02
# define ACCEL_FS_SEL_2G 0x03
# define ACCEL_FS_SEL_SHIFT 0x05
#define INV3REG_GYRO_CONFIG1 INV3REG(INV3_BANK0,0x51U)
#define INV3REG_GYRO_ACCEL_CONFIG0 INV3REG(INV3_BANK0,0x52U)
#define INV3REG_ACCEL_CONFIG1 INV3REG(INV3_BANK0,0x53U)
#define INV3REG_TMST_CONFIG INV3REG(INV3_BANK0,0x54U)
# define BIT_TMST_CONFIG_TMST_EN 0x01
#define INV3REG_FIFO_CONFIG1 INV3REG(INV3_BANK0,0x5FU)
# define BIT_FIFO_CONFIG1_ACCEL_EN 0x01
# define BIT_FIFO_CONFIG1_GYRO_EN 0x02
# define BIT_FIFO_CONFIG1_TEMP_EN 0x04
# define BIT_FIFO_CONFIG1_TMST_FSYNC_EN 0x08
# define BIT_FIFO_CONFIG1_HIRES_EN 0x10
# define BIT_FIFO_CONFIG1_WM_GT_TH 0x20
# define BIT_FIFO_CONFIG1_RESUME_PARTIAL_RD 0x40
#define INV3REG_FIFO_CONFIG2 INV3REG(INV3_BANK0,0x60U)
#define INV3REG_FIFO_CONFIG3 INV3REG(INV3_BANK0,0x61U)
#define INV3REG_INT_SOURCE0 INV3REG(INV3_BANK0,0x65U)
#define INV3REG_INT_SOURCE3 INV3REG(INV3_BANK0,0x68U)
# define BIT_FIFO_FULL_INT_EN 0x02
# define BIT_FIFO_THS_INT_EN 0x04
# define BIT_UI_DRDY_INT_EN 0x08
#define INV3REG_INT_CONFIG1 INV3REG(INV3_BANK0,0x64U)
# define BIT_INT_ASYNC_RESET 0x10
#define INV3REG_WHO_AM_I INV3REG(INV3_BANK0,0x75U)
#define INV3REG_GYRO_CONFIG_STATIC2 INV3REG(INV3_BANK1,0x0BU)
# define BIT_GYRO_NF_DIS 0x01
# define BIT_GYRO_AAF_DIS 0x02
#define INV3REG_GYRO_CONFIG_STATIC3 INV3REG(INV3_BANK1,0x0CU)
# define GYRO_AAF_DELT_SHIFT 0x00
#define INV3REG_GYRO_CONFIG_STATIC4 INV3REG(INV3_BANK1,0x0DU)
# define GYRO_AAF_DELTSQR_LOW_SHIFT 0x00 //[0:7]
#define INV3REG_GYRO_CONFIG_STATIC5 INV3REG(INV3_BANK1,0x0EU)
# define GYRO_AAF_DELTSQR_HIGH_SHIFT 0x00 //[11:8]
# define GYRO_AAF_BITSHIFT_SHIFT 0x04
#define INV3REG_GYRO_CONFIG_STATIC6 INV3REG(INV3_BANK1,0x0FU)
# define GYRO_X_NF_COSWZ_LOW_SHIFT 0x00 //[0:7]
#define INV3REG_GYRO_CONFIG_STATIC7 INV3REG(INV3_BANK1,0x10U)
# define GYRO_Y_NF_COSWZ_LOW_SHIFT 0x00 //[0:7]
#define INV3REG_GYRO_CONFIG_STATIC8 INV3REG(INV3_BANK1,0x11U)
# define GYRO_Z_NF_COSWZ_LOW_SHIFT 0x00 //[0:7]
#define INV3REG_GYRO_CONFIG_STATIC9 INV3REG(INV3_BANK1,0x12U)
# define GYRO_X_NF_COSWZ_HIGH_SHIFT 0x00 //[8]
# define GYRO_Y_NF_COSWZ_HIGH_SHIFT 0x01 //[8]
# define GYRO_Z_NF_COSWZ_HIGH_SHIFT 0x02 //[8]
# define GYRO_X_NF_COSWZ_SEL_SHIFT 0x03 //[0]
# define GYRO_Y_NF_COSWZ_SEL_SHIFT 0x04 //[0]
# define GYRO_Z_NF_COSWZ_SEL_SHIFT 0x05 //[0]
#define INV3REG_GYRO_CONFIG_STATIC10 INV3REG(INV3_BANK1,0x13U)
# define GYRO_NF_BW_SEL_SHIFT 0x04
#define INV3REG_ACCEL_CONFIG_STATIC2 INV3REG(INV3_BANK2,0x03U)
# define ACCEL_AAF_DIS 0x01
# define ACCEL_AAF_DELT_SHIFT 0x01
#define INV3REG_ACCEL_CONFIG_STATIC3 INV3REG(INV3_BANK2,0x04U)
# define ACCEL_AAF_DELTSQR_LOW_SHIFT 0x00 //[0:7]
#define INV3REG_ACCEL_CONFIG_STATIC4 INV3REG(INV3_BANK2,0x05U)
# define ACCEL_AAF_DELTSQR_HIGH_SHIFT 0x00 //[11:8]
# define ACCEL_AAF_BITSHIFT_SHIFT 0x04

#define INV3REG_BANK_SEL 0x76

// WHOAMI values
#define INV3_WHOAMI_ICM40605 0x33
#define INV3_WHOAMI_ICM40609 0x3b
#define INV3_WHOAMI_ICM42605 0x42
#define INV3_WHOAMI_ICM42688 0x47
#define INV3_WHOAMI_IIM42652 0x6f
#define INV3_WHOAMI_ICM42670 0x67

#endif /* INVENSENSE3_REGS_H */
33 changes: 13 additions & 20 deletions sw/airborne/peripherals/rm3100.c
View file
Expand Up @@ -155,27 +155,20 @@ void rm3100_event(struct Rm3100 *mag)
return;
}

switch (mag->status) {

case RM3100_STATUS_MEAS:
if (mag->i2c_trans.status == I2CTransSuccess) {
// Copy the data
mag->data.vect.x = rm3100_get_raw_from_buf(mag->i2c_trans.buf, 0);
mag->data.vect.y = rm3100_get_raw_from_buf(mag->i2c_trans.buf, 3);
mag->data.vect.z = rm3100_get_raw_from_buf(mag->i2c_trans.buf, 6);
mag->data_available = true;
// End reading, back to idle
mag->status = RM3100_STATUS_IDLE;
}
break;
// If we have a succesfull reading copy the data
if (mag->status == RM3100_STATUS_MEAS && mag->i2c_trans.status == I2CTransSuccess) {
// Copy the data
mag->data.vect.x = rm3100_get_raw_from_buf(mag->i2c_trans.buf, 0);
mag->data.vect.y = rm3100_get_raw_from_buf(mag->i2c_trans.buf, 3);
mag->data.vect.z = rm3100_get_raw_from_buf(mag->i2c_trans.buf, 6);
mag->data_available = true;
}

default:
if (mag->i2c_trans.status == I2CTransSuccess || mag->i2c_trans.status == I2CTransFailed) {
// Goto idle
mag->i2c_trans.status = I2CTransDone;
mag->status = RM3100_STATUS_IDLE;
}
break;
// Always go back to idle
if (mag->i2c_trans.status == I2CTransSuccess || mag->i2c_trans.status == I2CTransFailed) {
// Goto idle
mag->i2c_trans.status = I2CTransDone;
mag->status = RM3100_STATUS_IDLE;
}
}

8 changes: 4 additions & 4 deletions sw/airborne/test/modules/test_radio_control.c
View file
Expand Up @@ -30,7 +30,7 @@
static inline void main_init(void);
static inline void main_periodic_task(void);
static inline void main_event_task(void);
static void main_on_radio_control_frame(void);
//static void main_on_radio_control_frame(void);

int main(void)
{
Expand Down Expand Up @@ -91,9 +91,9 @@ static inline void main_event_task(void)
//RadioControlEvent(main_on_radio_control_frame); FIXME
}

static void main_on_radio_control_frame(void)
/*static void main_on_radio_control_frame(void)
{
// RunOnceEvery(10, {DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL, radio_control.values);});
RunOnceEvery(10, {DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL, radio_control.values);});
}
}*/
2 changes: 1 addition & 1 deletion sw/ext/Makefile
View file
Expand Up @@ -91,7 +91,7 @@ mavlink: mavlink.update mavlink.build

mavlink.build:
@echo GENERATE $(PAPARAZZI_SRC)/var/include/mavlink
$(Q)PYTHONPATH=$(EXT_DIR)/mavlink python $(EXT_DIR)/mavlink/pymavlink/tools/mavgen.py --output $(PAPARAZZI_SRC)/var/include/mavlink --lang C $(EXT_DIR)/mavlink/message_definitions/v1.0/paparazzi.xml --no-validate > /dev/null
$(Q)PYTHONPATH=$(EXT_DIR)/mavlink python $(EXT_DIR)/mavlink/pymavlink/tools/mavgen.py --output $(PAPARAZZI_SRC)/var/include/mavlink --lang C $(EXT_DIR)/mavlink/message_definitions/v1.0/paparazzi.xml --wire-protocol 2.0 --no-validate > /dev/null

libsbp: libsbp.update

Expand Down
2 changes: 1 addition & 1 deletion sw/ext/tudelft_gazebo_models
Submodule tudelft_gazebo_models updated 15 files
+1,131 −0 src/ply/RotatingWingV3/15 inch prop Disc.ply
+17,817 −0 src/ply/RotatingWingV3/HoriTail.ply
+1,248 −0 src/ply/RotatingWingV3/LeftAileron.ply
+1,179 −0 src/ply/RotatingWingV3/LeftFlap.ply
+158,834 −0 src/ply/RotatingWingV3/RWV4 NonMoving.ply
+1,248 −0 src/ply/RotatingWingV3/RightAileron.ply
+1,179 −0 src/ply/RotatingWingV3/RightFlap.ply
+1,149 −0 src/ply/RotatingWingV3/Rudder.ply
+4,859 −0 src/ply/RotatingWingV3/RudderSolid.ply
+78,497 −0 src/ply/RotatingWingV3/WingQuad.ply
+93 −0 world/cyberzoo2019_orange_poles_panels_in_middle.world
+474 −0 world/cyberzoo_4_panels.world
+101 −0 world/cyberzoo_9_orange_poles.world
+62 −0 world/cyberzoo_mat.world
+336 −0 world/cyberzoo_panel.world
36 changes: 36 additions & 0 deletions sw/ground_segment/python/rot_wing_mon/rot_wing_mon.py
View file
@@ -0,0 +1,36 @@
#!/usr/bin/env python3
#
# Copyright (C) 2023 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 wx
import rot_wing_viewer

class RotWingApp(wx.App):
def OnInit(self):
self.main = rot_wing_viewer.RotWingFrame()
self.main.Show()
self.SetTopWindow(self.main)
return True

def main():
application = RotWingApp(0)
application.MainLoop()

if __name__ == '__main__':
main()
284 changes: 284 additions & 0 deletions sw/ground_segment/python/rot_wing_mon/rot_wing_viewer.py
View file
@@ -0,0 +1,284 @@
#
# Copyright (C) 2023 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 wx

import sys
import os
import math
import datetime
import numpy as np


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

WIDTH = 600
BARH = 140

class EscMessage(object):
def __init__(self, msg):
self.id = int(msg['motor_id'])
self.amp = float(msg['amps'])
self.rpm = float(msg['rpm'])
self.volt_b = float(msg['bat_volts'])
self.volt_m = float(msg['motor_volts'])
self.temperature = float(msg['temperature']) - 273.15
self.temperature_dev = float(msg['temperature_dev']) - 273.15
self.energy = float(msg['energy'])

def get_current(self):
return str(round(self.amp ,1)) + "A"
def get_current_perc(self):
return self.amp / 30

def get_rpm(self):
return str(round(self.rpm ,0)) + " rpm"
def get_rpm_perc(self):
return self.rpm / 5150
def get_rpm_color(self):
if self.rpm < 4000:
return 1
return 0.5


def get_volt(self):
if (self.id in [6,7,8,9,16,17,18,19]):
return "Servo " + str(self.id) + " " +str(round(self.volt_m ,1)) + "V"
else:
return "Mot " + str(self.id) + " " +str(round(self.volt_m ,1)) + "V"
def get_volt_perc(self):
return self.volt_b / (6*4.2)

def get_temp(self):
if self.temperature < -200:
return "xxx"
return str(round(self.temperature ,1)) + "C"
def get_temp_perc(self):
return self.temperature / 120.0

def get_temp_dev(self):
if self.temperature_dev < -200:
return "xxx"
return str(round(self.temperature_dev, 1)) + "C"
def get_temp_dev_perc(self):
return self.temperature_dev / 120.0

def get_temp_color(self):
if self.temperature < 0:
return 0
elif self.temperature < 60:
return 1
else:
return 0.5

def get_temp_dev_color(self):
if self.temperature_dev < 0:
return 0
elif self.temperature_dev < 100:
return 1
else:
return 0.5

class INDIMessage(object):
def __init__(self, msg):
self.u = np.array(msg['u'], dtype=np.float)

def get_u(self, id):
return str(round(self.u[id], 0)) + " PPRZ"

def get_u_perc(self, id):
return self.u[id] / 9600.

def get_u_color(self, id):
if self.u[id] < 9600 / 0.8:
return 1
else:
return 0.5

class MotorList(object):
def __init__(self):
self.mot = []

def fill_from_esc_msg(self, esc):
added = False
for i in range(len(self.mot)):
if self.mot[i].id == esc.id:
self.mot[i] = esc
added = True
break
if not added:
self.mot.append(esc)

class RotWingFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ESC":
self.esc = EscMessage(msg)
self.motors.fill_from_esc_msg(self.esc)
wx.CallAfter(self.update)

if msg.name == "STAB_ATTITUDE_FULL_INDI":
self.indi = INDIMessage(msg)
wx.CallAfter(self.update)

def update(self):
self.Refresh()

def OnSize(self, event):
self.w = event.GetSize().x
self.h = event.GetSize().y
self.cfg.Write("width", str(self.w))
self.cfg.Write("height", str(self.h))
self.Refresh()

def OnMove(self, event):
self.x = event.GetPosition().x
self.y = event.GetPosition().y
self.cfg.Write("left", str(self.x))
self.cfg.Write("top", str(self.y))

def StatusBox(self, dc, dx, dy, row, col, txt, percent, color):
if percent < 0:
percent = 0
if percent > 1:
percent = 1
boxw = self.stat
tdx = int(boxw * 10.0 / 300.0)
tdy = int(boxw * 6.0 / 300.0)
boxh = int(boxw * 40.0 / 300.0)
boxw = self.stat - 2*tdx
spacing = boxh+10

dc.SetPen(wx.Pen(wx.Colour(0,0,0)))
dc.SetBrush(wx.Brush(wx.Colour(220,220,220)))
dc.DrawRectangle(tdx + col * 200 + dx, int(row*spacing+tdx) +dy, int(boxw), boxh)
dc.SetTextForeground(wx.Colour(0, 0, 0))
if color < 0.2:
dc.SetTextForeground(wx.Colour(255, 255, 255))
dc.SetBrush(wx.Brush(wx.Colour(250,0,0)))
elif color < 0.6:
dc.SetBrush(wx.Brush(wx.Colour(250,180,0)))
else:
dc.SetBrush(wx.Brush(wx.Colour(0,250,0)))
# dc.DrawLine(200,50,350,50)
dc.DrawRectangle(tdx + col * 200 + dx, int(row*spacing+tdx+dy), int(boxw * percent), boxh)
dc.DrawText(txt,18 + col * 200 + dx,int(row*spacing+tdy+tdx+dy))

def OnPaint(self, e):
# Automatic Scaling
w = self.w
h = self.h - 25

dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()

# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))

fontscale = int(w * 11.0 / 1500.0)
if fontscale < 6:
fontscale = 6
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)

# Draw Drone
dc.SetPen(wx.Pen(wx.Colour(0,0,0)))
dc.SetBrush(wx.Brush(wx.Colour(240,240,220)))
# Fuselage
dc.DrawRoundedRectangle(int(0.425*w), int(0.05*h),int(0.15*w), int(0.4*h), int(0.05*w))
# Front Wing
#dc.DrawRectangle(int(0.05*w), int(0.25*h),int(0.9*w), int(0.15*h))
# Back Wing
#dc.DrawRectangle(int(0.01*w), int(0.65*h),int(0.98*w), int(0.15*h))

# Motors
self.stat = int(0.10*w)
dc.SetBrush(wx.Brush(wx.Colour(200,200,100)))
w1 = 0.03
w2 = 0.2
w3 = 0.37
w4 = 0.54
dw = 0.11
mw = 0.1
mh = 0.15
mm = [(0.5-0.5*mw,w1), (0.5+dw,w2), (0.5-0.5*mw,w3), (0.5-dw-mw,w2), (0.5-0.5*mw,w4)]
for m in mm:
dc.DrawRectangle(int(m[0]*w), int(m[1]*h),int(mw*w), int(mh*h))

for m in self.motors.mot:
if m.id>4:
continue
mo_co = mm[m.id]
#print(m.id, mo_co)
dx = int(mo_co[0]*w)
dy = int(mo_co[1]*h)
self.StatusBox(dc, dx, dy, 0, 0, m.get_volt(), m.get_volt_perc(), 1)
self.StatusBox(dc, dx, dy, 1, 0, m.get_current(), m.get_current_perc(), 1)
self.StatusBox(dc, dx, dy, 2, 0, m.get_rpm(), m.get_rpm_perc(), m.get_rpm_color())
self.StatusBox(dc, dx, dy, 3, 0, m.get_temp(), m.get_temp_perc(), m.get_temp_color())
self.StatusBox(dc, dx, dy, 4, 0, m.get_temp_dev(), m.get_temp_dev_perc(), m.get_temp_dev_color())
try:
self.StatusBox(dc, dx, dy, 5, 0, self.indi.get_u(m.id), self.indi.get_u_perc(m.id), self.indi.get_u_color(m.id))
except:
pass

def __init__(self):

self.w = WIDTH
self.h = WIDTH + BARH

self.cfg = wx.Config('rot_wing_conf')
if self.cfg.Exists('width'):
self.w = int(self.cfg.Read('width'))
self.h = int(self.cfg.Read('height'))

wx.Frame.__init__(self, id=-1, parent=None, name=u'RotWingFrame',
size=wx.Size(self.w, self.h), title=u'Rot Wing Monitoring')

if self.cfg.Exists('left'):
self.x = int(self.cfg.Read('left'))
self.y = int(self.cfg.Read('top'))
self.SetPosition(wx.Point(self.x,self.y))

self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_MOVE, self.OnMove)
self.Bind(wx.EVT_CLOSE, self.OnClose)

#ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO)
#self.SetIcon(ico)

self.motors = MotorList()

self.interface = IvyMessagesInterface("rotwingframe")
self.interface.subscribe(self.message_recv)

def OnClose(self, event):
self.interface.shutdown()
self.Destroy()

495 changes: 495 additions & 0 deletions sw/ground_segment/python/rot_wing_visualizer/rot_wing_vis_o3d_app.py
View file

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion sw/tools/install.py
View file
Expand Up @@ -163,7 +163,7 @@ def __init__(self):
button8 = QPushButton('8) Bebop Opencv')
button8.clicked.connect(self.cmd_bebopcv)
if float(release['RELEASE']) > 20.04:
button8.setDisables(True)
button8.setDisabled(True)
btn_layout.addWidget(button8)

button9 = QPushButton('9) VLC + Joystick + Natnet')
Expand Down
28 changes: 16 additions & 12 deletions sw/tools/opti_dist/dist.py
View file
Expand Up @@ -15,16 +15,19 @@


# This is a callback function that gets connected to the NatNet client. It is called once per rigid body per frame
def receiveRigidBodyFrame(id, position, rotation):
# print( "Received frame for rigid body", id )
global pos_x, pos_y, pos_z
global track_id
if track_id and id != track_id:
return

pos_x = position[0]
pos_y = position[1]
pos_z = position[2]
def receiveRigidBodyFrame(rigidBodyList, stamp):
# print(rigidBodyList)
for (id, position, quat, valid) in rigidBodyList:
# print( "Received frame for rigid body", id )
global pos_x, pos_y, pos_z
global track_id
if track_id and id != track_id:
continue

# print( "Received frame for rigid body", id )
pos_x = position[0]
pos_y = position[1]
pos_z = position[2]


def main(args):
Expand All @@ -42,7 +45,8 @@ def main(args):
multicast=args.multicast,
commandPort=args.commandPort,
dataPort=args.dataPort,
rigidBodyListListener=receiveRigidBodyFrame)
rigidBodyListListener=receiveRigidBodyFrame,
version=(2,9,0,0))
# Start up the streaming client now that the callbacks are set up.
# This will run perpetually, and operate on a separate thread.
streamingClient.run()
Expand Down Expand Up @@ -84,7 +88,7 @@ def main(args):

if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--server', default="169.254.201.120")
parser.add_argument('--server', default="127.0.0.1")
parser.add_argument('--multicast', default="239.255.42.99")
parser.add_argument('--commandPort', type=int, default=1510)
parser.add_argument('--dataPort', type=int, default=1511)
Expand Down
13 changes: 13 additions & 0 deletions sw/tools/px4/cube_orangeplus.prototype
View file
@@ -0,0 +1,13 @@
{
"board_id": 1063,
"magic": "PX4FWv1",
"description": "Firmware for the CubePilot CubeOrange+ board",
"image": "",
"build_time": 0,
"summary": "CubeOrange+",
"version": "0.1",
"image_size": 0,
"image_maxsize": 1966080,
"git_identity": "",
"board_revision": 0
}