50 changes: 41 additions & 9 deletions sw/airborne/modules/system_identification/sys_id_doublet.c
View file
Expand Up @@ -48,17 +48,21 @@
#define DOUBLET_ENABLED TRUE
#endif

#ifdef DOUBLET_RADIO_CHANNEL
#include "modules/radio_control/radio_control.h"
pprz_t previous_radio_value_doublet = 0;
#endif


static struct doublet_t doublet;

uint8_t doublet_active = false;
uint8_t doublet_mode_3211 = false;
uint8_t doublet_mode = 0;

uint8_t doublet_axis = 0;

pprz_t doublet_amplitude = 4500;
float doublet_length_s = 20.0f;
pprz_t doublet_amplitude = 0;
float doublet_length_s = 0.5f;
float doublet_extra_waiting_time_s = 0.0f;


Expand All @@ -83,7 +87,7 @@ static void set_current_doublet_values(void)
static void send_doublet(struct transport_tx *trans, struct link_device *dev){
pprz_msg_send_DOUBLET(trans, dev, AC_ID, &doublet_active,
&doublet_axis, &doublet_amplitude,
&current_doublet_values[doublet_axis], &doublet_mode_3211);
&current_doublet_values[doublet_axis], &doublet_mode);
}

static void start_doublet(void)
Expand All @@ -107,8 +111,15 @@ uint8_t sys_id_doublet_running(void){
void sys_id_doublet_activate_handler(uint8_t activate)
{
doublet_active = activate;
#ifdef DOUBLET_RADIO_CHANNEL
// Don't activate doublet when radio signal is low
if (radio_control.values[DOUBLET_RADIO_CHANNEL] < 1750)
{
doublet_active = 0;
}
#endif
if (doublet_active) {
doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode_3211);
doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode);
start_doublet();
} else {
stop_doublet();
Expand All @@ -122,13 +133,13 @@ void sys_id_doublet_axis_handler(uint8_t axis)
}
}

void sys_id_doublet_mod3211_handler(uint8_t mode){
doublet_mode_3211 = mode;
void sys_id_doublet_mod_handler(uint8_t mode){
doublet_mode = mode;
}

void sys_id_doublet_init(void)
{
doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode_3211);
doublet_init(&doublet, doublet_length_s, doublet_extra_waiting_time_s, get_sys_time_float(), doublet_mode);

set_current_doublet_values();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DOUBLET, send_doublet);
Expand All @@ -139,7 +150,28 @@ void sys_id_doublet_init(void)
}

void sys_id_doublet_run(void)
{
{
#ifdef DOUBLET_RADIO_CHANNEL
// Check if doublet switched on when off before
if (previous_radio_value_doublet < 1750)
{
if (radio_control.values[DOUBLET_RADIO_CHANNEL] > 1750)
{
// Activate doublet
sys_id_doublet_activate_handler(1);
}
}
// Check if doublet switched off when on before
if (previous_radio_value_doublet > 1750)
{
if (radio_control.values[DOUBLET_RADIO_CHANNEL] < 1750)
{
// Deactivate doublet
sys_id_doublet_activate_handler(0);
}
}
previous_radio_value_doublet = radio_control.values[DOUBLET_RADIO_CHANNEL];
#endif
if (doublet_active) {
if (!doublet_is_running(&doublet, get_sys_time_float())) {
stop_doublet();
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/modules/system_identification/sys_id_doublet.h
View file
Expand Up @@ -46,7 +46,7 @@ extern float doublet_extra_waiting_time_s;
// Index of doublet axis in ACTIVE_DOUBLET_AXES
extern uint8_t doublet_axis;

extern uint8_t doublet_mode_3211;
extern uint8_t doublet_mode;

extern void sys_id_doublet_init(void);

Expand All @@ -56,7 +56,7 @@ extern void sys_id_doublet_run(void);
// Handlers for changing gcs variables
extern void sys_id_doublet_activate_handler(uint8_t activate); // Activate the doublet
extern void sys_id_doublet_axis_handler(uint8_t axis); // Check if new axis
extern void sys_id_doublet_mod3211_handler(uint8_t mode);
extern void sys_id_doublet_mod_handler(uint8_t mode);
extern uint8_t sys_id_doublet_running(void);
// Add the current doublet values to the in_cmd values if motors_on is true
extern void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[]);
Expand Down
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 */
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+9 −1 message_definitions/v1.0/messages.xml
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
}