10 changes: 5 additions & 5 deletions conf/userconf/tudelft/guido_conf.xml
View file
Expand Up @@ -7,7 +7,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml"
gui_color="#fffff996b847"
/>
<aircraft
Expand All @@ -18,18 +18,18 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
settings_modules="modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="red"
/>
<aircraft
name="Bebop2_opticflow_sim"
ac_id="4"
airframe="airframes/tudelft/tudelft_bebop2_opticflow_sim.xml"
airframe="airframes/tudelft/bebop2_opticflow_sim.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml [settings/control/stabilization_indi.xml] settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/cv_opticflow.xml [modules/cv_textons.xml] modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
settings_modules="modules/air_data.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue"
/>
<aircraft
Expand All @@ -51,7 +51,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml modules/video_capture.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml modules/video_capture.xml"
gui_color="blue"
/>
</conf>
11 changes: 8 additions & 3 deletions sw/airborne/arch/chibios/modules/actuators/actuators_dshot_arch.c
View file
Expand Up @@ -93,7 +93,8 @@ static void esc_msg_send(struct transport_tx *trans, struct link_device *dev) {
float bat_voltage = electrical.vsupply;
float power = actuators_dshot_values[i].current * bat_voltage;
float energy = (float)dtelem->consumption;
float temp = dtelem->temp + 273.15;
float temp = dtelem->temp;
float temp_dev = 0;
pprz_msg_send_ESC(trans, dev, AC_ID,
&actuators_dshot_values[i].current,
&bat_voltage,
Expand All @@ -102,6 +103,7 @@ static void esc_msg_send(struct transport_tx *trans, struct link_device *dev) {
&actuators_dshot_values[i].voltage,
&energy,
&temp,
&temp_dev,
&i,
&i);
}
Expand Down Expand Up @@ -325,11 +327,14 @@ void actuators_dshot_arch_commit(void)
dshotSendFrame(&DSHOTD9);
#endif

uint16_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 };

struct rpm_act_t rpm_list[ACTUATORS_DSHOT_NB] = { 0 };
for (uint8_t i = 0; i < ACTUATORS_DSHOT_NB; i++) {
rpm_list[i].actuator_idx = ACTUATORS_DSHOT_OFFSET + i;
rpm_list[i].rpm = 0;
if (actuators_dshot_values[i].activated) {
const DshotTelemetry *dtelem = dshotGetTelemetry(actuators_dshot_private[i].driver, actuators_dshot_private[i].channel);
rpm_list[i] = dtelem->rpm;
rpm_list[i].rpm = dtelem->rpm;
}
}
AbiSendMsgRPM(RPM_DSHOT_ID, rpm_list, ACTUATORS_DSHOT_NB);
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/arch/sim/led_hw.c
View file
Expand Up @@ -10,7 +10,7 @@ bool led_disabled = false;

value register_leds_cb(value cb_name)
{
leds_closure = caml_named_value(String_val(cb_name));
leds_closure = (value *)caml_named_value(String_val(cb_name));
return Val_unit;
}

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.c
View file
Expand Up @@ -27,7 +27,7 @@

#include "modules/actuators/actuators_dshot.h"

uint16_t actuators_dshot_values[ACTUATORS_DSHOT_NB];
struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB];

void actuators_dshot_arch_init(void) {}

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/arch/sim/sim_ap.c
View file
Expand Up @@ -107,7 +107,7 @@ value get_commands(value val_commands)
value set_datalink_message(value s)
{
int n = string_length(s);
char *ss = String_val(s);
char *ss = (char *)String_val(s);
assert(n <= MSG_SIZE);

int i;
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/boards/ardrone/actuators.c
View file
Expand Up @@ -66,6 +66,7 @@ int actuator_ardrone2_fd; /**< File descriptor for the port */
#define ARDRONE_GPIO_PIN_IRQ_INPUT 176

uint32_t led_hw_values;
uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB];

static inline void actuators_ardrone_reset_flipflop(void)
{
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/boards/ardrone/actuators.h
View file
Expand Up @@ -48,7 +48,7 @@
#define MOT_LEDGREEN 2
#define MOT_LEDORANGE 3

uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB];
extern uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB];

extern void actuators_ardrone_commit(void);
extern void actuators_ardrone_init(void);
Expand Down
11 changes: 10 additions & 1 deletion sw/airborne/boards/bebop/actuators.c
View file
Expand Up @@ -138,7 +138,16 @@ void actuators_bebop_commit(void)
actuators_bebop.led = led_hw_values & 0x3;
}
// Send ABI message
AbiSendMsgRPM(RPM_SENSOR_ID, actuators_bebop.rpm_obs, 4);
struct rpm_act_t rpm_message[4];
for (int i=0;i<4;i++) {
#ifdef SERVOS_BEBOP_OFFSET
rpm_message[i].actuator_idx = SERVOS_BEBOP_OFFSET + i;
#else
rpm_message[i].actuator_idx = SERVOS_DEFAULT_OFFSET + i;
#endif
rpm_message[i].rpm = actuators_bebop.rpm_obs[i];
}
AbiSendMsgRPM(RPM_SENSOR_ID, rpm_message, 4);
}

static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size)
Expand Down
9 changes: 8 additions & 1 deletion sw/airborne/boards/disco/actuators.c
View file
Expand Up @@ -169,7 +169,14 @@ void actuators_disco_commit(void)
}

// Send ABI message
AbiSendMsgRPM(RPM_SENSOR_ID, &actuators_disco.rpm_obs, 1);//FIXME & or not
struct rpm_act_t rpm_message = {0};
#ifdef SERVOS_DISCO_OFFSET
rpm_message.actuator_idx = SERVOS_DISCO_OFFSET;
#else
rpm_message.actuator_idx = SERVOS_DEFAULT_OFFSET;
#endif
rpm_message.rpm = actuators_disco.rpm_obs;
AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1);//FIXME & or not
}

static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size)
Expand Down
48 changes: 33 additions & 15 deletions sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
View file
Expand Up @@ -77,6 +77,35 @@ static uint32_t autopilot_in_flight_counter;
#define THRESHOLD_GROUND_DETECT 25.0
#endif

/** Default ground-detection estimation based on accelerometer shock */
__attribute__((weak)) bool autopilot_ground_detection(void) {
struct NedCoor_f *accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
return true;
}
return false;
}


/** Default in-flight detection estimation based on thrust and speed */
__attribute__((weak)) bool autopilot_in_flight_detection(void) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
return true;
}
} else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
return false;
}


#if USE_MOTOR_MIXING
#include "modules/actuators/motor_mixing.h"
Expand Down Expand Up @@ -234,9 +263,7 @@ void autopilot_event(void)
|| autopilot.mode == AP_MODE_FAILSAFE
#endif
) {
struct NedCoor_f *accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
if (autopilot_ground_detection()) {
autopilot.ground_detected = true;
autopilot.detect_ground_once = false;
}
Expand All @@ -255,18 +282,9 @@ void autopilot_reset_in_flight_counter(void)
void autopilot_check_in_flight(bool motors_on)
{
if (autopilot.in_flight) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot.in_flight = false;
}
} else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
if (autopilot_in_flight_detection()) {
autopilot.in_flight = false;
autopilot_in_flight_counter == 0;
}
} else { /* currently not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/firmwares/rotorcraft/autopilot_firmware.h
View file
Expand Up @@ -33,6 +33,12 @@

extern uint8_t autopilot_mode_auto2;

// Detect the ground and set NavGroundDetect() to true
__attribute__((weak)) extern bool autopilot_ground_detection(void);

// Detect the end of in_flight and stop integrators in control loops
__attribute__((weak)) extern bool autopilot_in_flight_detection(void);

extern void autopilot_firmware_init(void);

#endif /* AUTOPILOT_FIRMWARE_H */
1 change: 0 additions & 1 deletion sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Expand Up @@ -220,7 +220,6 @@ void guidance_indi_init(void)
*/
void guidance_indi_enter(void) {
/*Obtain eulers with zxy rotation order*/
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
nav.heading = eulers_zxy.psi;

Expand Down
6 changes: 3 additions & 3 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Expand Up @@ -179,7 +179,7 @@ struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;

abi_event rpm_ev;
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act);

abi_event thrust_ev;
static void thrust_cb(uint8_t sender_id, float thrust_increment);
Expand Down Expand Up @@ -846,12 +846,12 @@ void calc_g1g2_pseudo_inv(void)
}
}

static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
static void rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t UNUSED *rpm_msg, uint8_t UNUSED num_act)
{
#if INDI_RPM_FEEDBACK
int8_t i;
for (i = 0; i < num_act; i++) {
act_obs[i] = (rpm[i] - get_servo_min(i));
act_obs[i] = (rpm_msg[i].rpm - get_servo_min(i));
act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
Bound(act_obs[i], 0, MAX_PPRZ);
}
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
View file
Expand Up @@ -43,6 +43,8 @@ extern float act_pref[INDI_NUM_ACT];

extern float indi_Wu[INDI_NUM_ACT];

extern bool act_is_servo[INDI_NUM_ACT];

struct Indi_gains {
struct FloatRates att;
struct FloatRates rate;
Expand Down
5 changes: 4 additions & 1 deletion sw/airborne/main_ap.c
View file
Expand Up @@ -49,8 +49,11 @@ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
#endif

/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
* defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
* defaults to PERIODIC_FREQUENCY or set by TELEMETRY_FREQUENCY configure option in airframe file
*/
#if (TELEMETRY_FREQUENCY > SYS_TIME_FREQUENCY) || !(SYS_TIME_FREQUENCY/TELEMETRY_FREQUENCY*TELEMETRY_FREQUENCY == SYS_TIME_FREQUENCY)
#warning "The TELEMETRY_FREQUENCY can not be faster than the SYS_TIME_FREQUENCY and needs to be dividable by the SYS_TIME_FREQUENCY."
#endif
PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)

#if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/main_chibios.c
View file
Expand Up @@ -33,7 +33,7 @@

#ifndef SYS_TIME_FREQUENCY
#error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
#elif CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) && SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
#elif CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) || SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
#error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
#endif

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/math/pprz_matrix_decomp_float.c
View file
Expand Up @@ -167,7 +167,7 @@ static int imin(int num1, int num2) {
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
{
/* Householder reduction to bidiagonal form. */
int flag, i, its, j, jj, k, l, NM = 0;
int flag, i, its, j, jj, k, l = 0, NM = 0;
float C, F, H, S, X, Y, Z, tmp;
float G = 0.0;
float Scale = 0.0;
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/modules/actuators/actuators.h
View file
Expand Up @@ -40,6 +40,12 @@
extern void actuators_init(void);
extern void actuators_periodic(void);

// Actuator RPM structure for ABI Message
struct rpm_act_t {
uint8_t actuator_idx;
int32_t rpm;
};

#if ACTUATORS_NB

extern uint32_t actuators_delay_time;
Expand Down
71 changes: 69 additions & 2 deletions sw/airborne/modules/actuators/actuators_uavcan.c
View file
Expand Up @@ -43,6 +43,7 @@ struct actuators_uavcan_telem_t {
float voltage;
float current;
float temperature;
float temperature_dev;
int32_t rpm;
uint32_t energy;
};
Expand Down Expand Up @@ -85,9 +86,15 @@ int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB];
#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE (0xD8A7486238EC3AF3ULL)
#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE ((484 + 7)/8)

/* uavcan EQUIMPENT_DEVICE_TEMPERATURE message definition */
#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID 1110
#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE (0x70261C28A94144C6ULL)
#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE ((40 + 7)/8)

/* private variables */
static bool actuators_uavcan_initialized = false;
static uavcan_event esc_status_ev;
static uavcan_event device_temperature_ev;

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
Expand Down Expand Up @@ -150,7 +157,7 @@ static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_de
float rpm = telem->rpm;
float energy = telem->energy;
pprz_msg_send_ESC(trans, dev, AC_ID, &telem->current, &electrical.vsupply, &power,
&rpm, &telem->voltage, &energy, &telem->temperature, &telem->node_id, &old_idx);
&rpm, &telem->voltage, &energy, &telem->temperature, &telem->temperature_dev, &telem->node_id, &old_idx);
}
#endif

Expand Down Expand Up @@ -191,7 +198,7 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
canardDecodeScalar(transfer, 48, 16, true, (void *)&tmp_float);
telem[esc_idx].current = canardConvertFloat16ToNativeFloat(tmp_float);
canardDecodeScalar(transfer, 64, 16, true, (void *)&tmp_float);
telem[esc_idx].temperature = canardConvertFloat16ToNativeFloat(tmp_float);
telem[esc_idx].temperature = canardConvertFloat16ToNativeFloat(tmp_float) - 273.15;
canardDecodeScalar(transfer, 80, 18, true, (void *)&telem[esc_idx].rpm);

#if UAVCAN_ACTUATORS_USE_CURRENT
Expand All @@ -208,8 +215,65 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
}
#endif
#endif

// Feedback ABI RPM messages
#ifdef SERVOS_UAVCAN1_NB
if (iface == &uavcan1) {
struct rpm_act_t rpm_message;
rpm_message.actuator_idx = SERVOS_UAVCAN1_OFFSET + esc_idx;
rpm_message.rpm = telem[esc_idx].rpm;

// Send ABI message
AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1);
}
#endif
#ifdef SERVOS_UAVCAN2_NB
if (iface == &uavcan2) {
struct rpm_act_t rpm_message;
rpm_message.actuator_idx = SERVOS_UAVCAN2_OFFSET + esc_idx;
rpm_message.rpm = telem[esc_idx].rpm;

// Send ABI message
AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, 1);
}
#endif
}

/**
* Whevener an DEVICE_TEMPERATURE message from the EQUIPMENT group is received
*/
static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
{
uint16_t device_id;
uint16_t tmp_float;

struct actuators_uavcan_telem_t *telem = NULL;
uint8_t max_id = 0;
#ifdef SERVOS_UAVCAN1_NB
if (iface == &uavcan1) {
telem = uavcan1_telem;
max_id = SERVOS_UAVCAN1_NB;
}
#endif
#ifdef SERVOS_UAVCAN2_NB
if (iface == &uavcan2) {
telem = uavcan2_telem;
max_id = SERVOS_UAVCAN2_NB;
}
#endif


canardDecodeScalar(transfer, 0, 16, false, (void*)&device_id);
//Could not find the right interface
if (device_id >= max_id || telem == NULL || max_id == 0) {
return;
}

canardDecodeScalar(transfer, 16, 16, false, (void*)&tmp_float);
telem[device_id].temperature_dev = canardConvertFloat16ToNativeFloat(tmp_float) - 273.15;
}


/**
* Initialize an uavcan interface
*/
Expand All @@ -221,6 +285,9 @@ void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
// Bind uavcan ESC_STATUS message from EQUIPMENT
uavcan_bind(UAVCAN_EQUIPMENT_ESC_STATUS_ID, UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, &esc_status_ev,
&actuators_uavcan_esc_status_cb);
// Bind uavcan DEVICE_TEMPERATURE message from EQUIPMENT
uavcan_bind(UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE, &device_temperature_ev,
&actuators_uavcan_device_temperature_cb);

// Configure telemetry
#if PERIODIC_TELEMETRY
Expand Down
8 changes: 6 additions & 2 deletions sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
View file
Expand Up @@ -43,12 +43,16 @@
PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
#endif

#ifdef AHRS_FLOATING_HEADING
PRINT_CONFIG_MSG("No heading feedback in AHRS: FLOATING HEADING")
#endif

#if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
#warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
#endif

#if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING
#warning "Please use either USE_MAGNETOMETER or AHRS_USE_GPS_HEADING."
#if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING &&!AHRS_FLOATING_HEADING
#warning "Please use either USE_MAGNETOMETER, AHRS_USE_GPS_HEADING or accept AHRS_FLOATING_HEADING."
#endif

#if AHRS_USE_GPS_HEADING && !USE_GPS
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/modules/computer_vision/textons.c
View file
Expand Up @@ -139,8 +139,8 @@ static FILE *dictionary_logger = NULL;
* @param[out] *img The output image
* @param[in] *img The input image (YUV422)
*/
struct image_t *texton_func(struct image_t *img, uint8_t p);
struct image_t *texton_func(struct image_t *img, uint8_t p)
struct image_t *texton_func(struct image_t *img, UNUSED uint8_t p);
struct image_t *texton_func(struct image_t *img, UNUSED uint8_t p)
{
// whether to execute the function:
if (!running) { return img; }
Expand Down Expand Up @@ -286,7 +286,7 @@ void DictionaryTrainingYUV(uint8_t *frame, uint16_t width, uint16_t height)
}

// Extract and learn from n_samples_image per image
for (s = 0; s < n_samples_image; s++) {
for (s = 0; s < (int) n_samples_image; s++) {
// select a random sample from the image
x = rand() % (width - patch_size);
y = rand() % (height - patch_size);
Expand Down Expand Up @@ -446,7 +446,7 @@ void DistributionExtraction(uint8_t *frame, uint16_t width, uint16_t height)
texton_distribution[assignment]++;
n_extracted_textons++;

if (!FULL_SAMPLING && n_extracted_textons == n_samples_image) {
if (!FULL_SAMPLING && n_extracted_textons == (int) n_samples_image) {
finished = 1;
} else {
// FULL_SAMPLING is actually a sampling that covers the image:
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/core/abi_common.h
View file
Expand Up @@ -34,6 +34,7 @@
#include "math/pprz_algebra_float.h"
#include "modules/gps/gps.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
/* Include here headers with structure definition you may want to use with ABI
* Ex: '#include "modules/gps/gps.h"' in order to use the GpsState structure
*/
Expand Down
119 changes: 74 additions & 45 deletions sw/airborne/modules/ctrl/approach_moving_target.c
View file
Expand Up @@ -23,28 +23,51 @@
* Approach a moving target (e.g. ship)
*/

#include "generated/airframe.h"

#include "approach_moving_target.h"

#include "generated/modules.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"


#ifndef AMT_ERR_SLOWDOWN_GAIN
#define AMT_ERR_SLOWDOWN_GAIN 0.25
// Filter value in Hz
#ifndef APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ
#define APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ 0.5
#endif

// Filter value in Hz
#ifndef CUTOFF_FREQ_FILTERS_HZ
#define CUTOFF_FREQ_FILTERS_HZ 0.5
#endif
// Approach slope angle in degrees
#ifndef APPROACH_MOVING_TARGET_SLOPE
#define APPROACH_MOVING_TARGET_SLOPE 35.0
#endif

// Approach initial distance in meters
#ifndef APPROACH_MOVING_TARGET_DISTANCE
#define APPROACH_MOVING_TARGET_DISTANCE 60.0
#endif

// Approach speed in meters per second along the slope
#ifndef APPROACH_MOVING_TARGET_SPEED
#define APPROACH_MOVING_TARGET_SPEED -1.0
#endif

// Gains
#ifndef APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN
#define APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN 0.25
#endif

float amt_err_slowdown_gain = AMT_ERR_SLOWDOWN_GAIN;
#ifndef APPROACH_MOVING_TARGET_POS_GAIN
#define APPROACH_MOVING_TARGET_POS_GAIN 0.3
#endif

float approach_moving_target_angle_deg;
#ifndef APPROACH_MOVING_TARGET_SPEED_GAIN
#define APPROACH_MOVING_TARGET_SPEED_GAIN 1.0
#endif

float cutoff_freq_filters_hz = CUTOFF_FREQ_FILTERS_HZ;
#ifndef APPROACH_MOVING_TARGET_RELVEL_GAIN
#define APPROACH_MOVING_TARGET_RELVEL_GAIN 1.0
#endif

Butterworth2LowPass target_pos_filt[3];
Butterworth2LowPass target_vel_filt[3];
Expand All @@ -54,15 +77,17 @@ Butterworth2LowPass target_heading_filt;
#include <stdio.h>

struct Amt amt = {
.distance = 60,
.speed = -1.0,
.pos_gain = 0.3,
.psi_ref = 180.0,
.slope_ref = 35.0,
.speed_gain = 1.0,
.relvel_gain = 1.0,
.enabled_time = 0,
.wp_id = 0,
.distance = APPROACH_MOVING_TARGET_DISTANCE,
.speed = APPROACH_MOVING_TARGET_SPEED,
.psi_ref = 180.0,
.slope_ref = APPROACH_MOVING_TARGET_SLOPE,
.err_slowdown_gain = APPROACH_MOVING_TARGET_ERR_SLOWDOWN_GAIN,
.pos_gain = APPROACH_MOVING_TARGET_POS_GAIN,
.speed_gain = APPROACH_MOVING_TARGET_SPEED_GAIN,
.relvel_gain = APPROACH_MOVING_TARGET_RELVEL_GAIN,
.cutoff_freq_filters_hz = APPROACH_MOVING_TARGET_CUTOFF_FREQ_FILTERS_HZ,
.enabled_time = 0,
.wp_id = 0,
};

struct AmtTelem {
Expand All @@ -74,35 +99,36 @@ struct AmtTelem amt_telem;
bool approach_moving_target_enabled = false;

struct FloatVect3 nav_get_speed_sp_from_diagonal(struct EnuCoor_i target, float pos_gain, float rope_heading);
void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned);
void update_waypoint(uint8_t wp_id, struct FloatVect3 *target_ned);

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_approach_moving_target(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_APPROACH_MOVING_TARGET(trans, dev, AC_ID,
&amt_telem.des_pos.x,
&amt_telem.des_pos.y,
&amt_telem.des_pos.z,
&amt_telem.des_vel.x,
&amt_telem.des_vel.y,
&amt_telem.des_vel.z,
&amt.distance);
&amt_telem.des_pos.x,
&amt_telem.des_pos.y,
&amt_telem.des_pos.z,
&amt_telem.des_vel.x,
&amt_telem.des_vel.y,
&amt_telem.des_vel.z,
&amt.distance);
}
#endif

void approach_moving_target_init(void)
{
approach_moving_target_set_low_pass_freq(cutoff_freq_filters_hz);
approach_moving_target_set_low_pass_freq(amt.cutoff_freq_filters_hz);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_APPROACH_MOVING_TARGET, send_approach_moving_target);
#endif
}

void approach_moving_target_set_low_pass_freq(float filter_freq) {
cutoff_freq_filters_hz = filter_freq;
void approach_moving_target_set_low_pass_freq(float filter_freq)
{
amt.cutoff_freq_filters_hz = filter_freq;
// tau = 1/(2*pi*Fc)
float tau = 1.0 / (2.0 * M_PI * cutoff_freq_filters_hz);
float tau = 1.0 / (2.0 * M_PI * amt.cutoff_freq_filters_hz);
float sample_time = 1.0 / FOLLOW_DIAGONAL_APPROACH_FREQ;
for (uint8_t i = 0; i < 3; i++) {
init_butterworth_2_low_pass(&target_pos_filt[i], tau, sample_time, 0.0);
Expand All @@ -113,25 +139,27 @@ void approach_moving_target_set_low_pass_freq(float filter_freq) {
}

// Update a waypoint such that you can see on the GCS where the drone wants to go
void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned) {
void update_waypoint(uint8_t wp_id, struct FloatVect3 *target_ned)
{

// Update the waypoint
struct EnuCoor_f target_enu;
ENU_OF_TO_NED(target_enu, *target_ned);
waypoint_set_enu(wp_id, &target_enu);

// Send waypoint update every half second
RunOnceEvery(100/2, {
RunOnceEvery(100 / 2, {
// Send to the GCS that the waypoint has been moved
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
} );
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
});
}

// Function to enable from flight plan (call repeatedly!)
void approach_moving_target_enable(uint8_t wp_id) {
void approach_moving_target_enable(uint8_t wp_id)
{
amt.enabled_time = get_sys_time_msec();
amt.wp_id = wp_id;
}
Expand All @@ -140,21 +168,22 @@ void approach_moving_target_enable(uint8_t wp_id) {
* @brief Generates a velocity reference from a diagonal approach path
*
*/
void follow_diagonal_approach(void) {
void follow_diagonal_approach(void)
{

// Check if the flight plan recently called the enable function
if ( (get_sys_time_msec() - amt.enabled_time) > (2000 / NAVIGATION_FREQUENCY)) {
if ((get_sys_time_msec() - amt.enabled_time) > (2000 / NAVIGATION_FREQUENCY)) {
return;
}

// Get the target position, velocity and heading
struct NedCoor_f target_pos, target_vel = {0};
float target_heading;
if(!target_get_pos(&target_pos, &target_heading)) {
if (!target_get_pos(&target_pos, &target_heading)) {
// TODO: What to do? Same can be checked for the velocity
return;
}
if(target_heading > 360.f) {
if (target_heading > 360.f) {
// TODO: We got an invalid heading and need to do something with it
return;
}
Expand Down Expand Up @@ -227,7 +256,7 @@ void follow_diagonal_approach(void) {
float_vect3_bound_in_3d(&des_vel, 10.0);

// Bound vertical speed setpoint
if(stateGetAirspeed_f() > 13.0) {
if (stateGetAirspeed_f() > 13.0) {
Bound(des_vel.z, -4.0, 5.0);
} else {
Bound(des_vel.z, -nav.climb_vspeed, -nav.descend_vspeed);
Expand All @@ -241,7 +270,7 @@ void follow_diagonal_approach(void) {
float min_speed;
float sin_gamma_ref = sinf(gamma_ref);
if (sin_gamma_ref > 0.05) {
min_speed = (nav.descend_vspeed+0.1) / sin_gamma_ref;
min_speed = (nav.descend_vspeed + 0.1) / sin_gamma_ref;
} else {
min_speed = -5.0; // prevent dividing by zero
}
Expand All @@ -251,11 +280,11 @@ void follow_diagonal_approach(void) {

// Reduce approach speed if the error is large
float norm_pos_err_sq = VECT3_NORM2(pos_err);
float int_speed = amt.speed / (norm_pos_err_sq * amt_err_slowdown_gain + 1.0);
float int_speed = amt.speed / (norm_pos_err_sq * amt.err_slowdown_gain + 1.0);

// integrate speed to get the distance
float dt = FOLLOW_DIAGONAL_APPROACH_PERIOD;
amt.distance += int_speed*dt;
amt.distance += int_speed * dt;

// For display purposes
struct FloatVect3 disp_pos_target;
Expand Down
8 changes: 3 additions & 5 deletions sw/airborne/modules/ctrl/approach_moving_target.h
View file
Expand Up @@ -29,25 +29,23 @@
#include "std.h"
#include "math/pprz_algebra_float.h"

// Angle of the approach in degrees
extern float approach_moving_target_angle_deg;

struct Amt {
struct FloatVect3 rel_unit_vec;
float distance;
float speed;
float pos_gain;
float psi_ref;
float slope_ref;
float err_slowdown_gain;
float pos_gain;
float speed_gain;
float relvel_gain;
float cutoff_freq_filters_hz;
int32_t enabled_time;
uint8_t wp_id;
};

extern struct Amt amt;
extern float amt_err_slowdown_gain;
extern float cutoff_freq_filters_hz;

extern void approach_moving_target_init(void);
extern void follow_diagonal_approach(void);
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/modules/ctrl/optical_flow_landing.c
View file
Expand Up @@ -330,7 +330,7 @@ void vertical_ctrl_module_init(void)
of_landing_ctrl.vel = 0.0f;
of_landing_ctrl.divergence_setpoint = 0.0f; // For exponential gain landing, pick a negative value
of_landing_ctrl.cov_set_point = OFL_COV_SETPOINT;
of_landing_ctrl.cov_limit = fabsf(OFL_COV_LANDING_LIMIT);
of_landing_ctrl.cov_limit = fabsf((float)OFL_COV_LANDING_LIMIT);
of_landing_ctrl.lp_const = OFL_LP_CONST;
Bound(of_landing_ctrl.lp_const, 0.001f, 1.f);
of_landing_ctrl.pgain = OFL_PGAIN;
Expand Down Expand Up @@ -1125,7 +1125,7 @@ void guidance_h_module_enter(void)

}

void guidance_h_module_run(bool in_flight)
void guidance_h_module_run(bool UNUSED in_flight)
{

}
Expand Down Expand Up @@ -1248,7 +1248,7 @@ void learn_from_file(void)
printf("Learned! Fit error = %f\n", fit_error);

// free learning distributions:
for (i = 0; i < n_read_samples; i++) {
for (i = 0; i < (int) n_read_samples; i++) {
free(text_dists[i]);
}
}
Expand Down Expand Up @@ -1312,7 +1312,7 @@ void recursive_least_squares_batch(float *targets, float **samples, uint8_t D, u
(*fit_error) = sum_abs_err / count;
}

void recursive_least_squares(float target, float *sample, uint8_t length_sample, float *params)
void recursive_least_squares(float target, float *sample, uint8_t length_sample, UNUSED float *params)
{
// MATLAB procedure:
/*
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/modules/esc32/esc32.c
View file
Expand Up @@ -223,6 +223,7 @@ static void parse_esc32(struct esc32_private *esc, uint8_t c) {

static void esc32_msg_send(struct transport_tx *trans, struct link_device *dev) {
float temp = 0;
float temp_dev = 0;
uint8_t id0 = 0;

pprz_msg_send_ESC(trans, dev, AC_ID,
Expand All @@ -233,6 +234,7 @@ static void esc32_msg_send(struct transport_tx *trans, struct link_device *dev)
&esc32.params.volts_motor,
&esc32.energy,
&temp,
&temp_dev,
&id0,
&id0); // only one motor handled for now
}
Expand Down
6 changes: 3 additions & 3 deletions sw/airborne/modules/helicopter/throttle_curve.c
View file
Expand Up @@ -54,7 +54,7 @@
#define THROTTLE_CURVE_RPM_ACT 0
#endif
static abi_event rpm_ev;
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act);

/* Initialize the throttle curves from the airframe file */
struct throttle_curve_t throttle_curve = {
Expand Down Expand Up @@ -99,12 +99,12 @@ void throttle_curve_init(void)
/**
* RPM callback for RPM based control throttle curves
*/
static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t *rpm, uint8_t num_act)
static void rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act)
{
if(num_act <= THROTTLE_CURVE_RPM_ACT)
return;

throttle_curve.rpm_meas = rpm[THROTTLE_CURVE_RPM_ACT];
throttle_curve.rpm_meas = rpm_msg[THROTTLE_CURVE_RPM_ACT].rpm;
throttle_curve.rpm_measured = true;
}

Expand Down
62 changes: 62 additions & 0 deletions sw/airborne/modules/imu/imu.c
View file
Expand Up @@ -155,115 +155,173 @@ PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PHI)
PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_THETA)
PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)

/** Which gyro measurements to send over telemetry/logging */
#ifndef IMU_GYRO_ABI_SEND_ID
#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(IMU_GYRO_ABI_SEND_ID)

/** Which accel measurements to send over telemetry/logging */
#ifndef IMU_ACCEL_ABI_SEND_ID
#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(IMU_ACCEL_ABI_SEND_ID)

/** Which mag measurements to send over telemetry/logging */
#ifndef IMU_MAG_ABI_SEND_ID
#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(IMU_MAG_ABI_SEND_ID)


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

static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
{
if(imu.accel_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
&imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
{
if(imu.accel_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
&imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_accel(struct transport_tx *trans, struct link_device *dev)
{
if(imu.accel_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
&accel_float.x, &accel_float.y, &accel_float.z);
if(imu.accel_abi_send_id != ABI_BROADCAST && imu.accels[id].abi_id == imu.accel_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
{
if(imu.gyro_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
&imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
{
if(imu.gyro_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
&imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_gyro(struct transport_tx *trans, struct link_device *dev)
{
if(imu.gyro_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
&gyro_float.p, &gyro_float.q, &gyro_float.r);
if(imu.gyro_abi_send_id != ABI_BROADCAST && imu.gyros[id].abi_id == imu.gyro_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
{
if(imu.mag_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
&imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
{
if(imu.mag_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
&imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_mag(struct transport_tx *trans, struct link_device *dev)
{
if(imu.mag_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
struct FloatVect3 mag_float;
MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
&mag_float.x, &mag_float.y, &mag_float.z);
if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
id = 0;
}

static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
{
if(imu.mag_abi_send_id == ABI_DISABLE)
return;
static uint8_t id = 0;
pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
&imu.mags[id].abi_id,
&imu.mags[id].unscaled.x,
&imu.mags[id].unscaled.y,
&imu.mags[id].unscaled.z,
&electrical.current);
if(imu.mag_abi_send_id != ABI_BROADCAST && imu.mags[id].abi_id == imu.mag_abi_send_id)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
id = 0;
Expand Down Expand Up @@ -396,6 +454,10 @@ void imu_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
#endif // DOWNLINK

// Set defaults
imu.gyro_abi_send_id = IMU_GYRO_ABI_SEND_ID;
imu.accel_abi_send_id = IMU_ACCEL_ABI_SEND_ID;
imu.mag_abi_send_id = IMU_MAG_ABI_SEND_ID;
imu.initialized = true;
}

Expand Down
3 changes: 3 additions & 0 deletions sw/airborne/modules/imu/imu.h
View file
Expand Up @@ -87,6 +87,9 @@ struct Imu {
struct imu_accel_t accels[IMU_MAX_SENSORS]; ///< The accelerometer sensors
struct imu_mag_t mags[IMU_MAX_SENSORS]; ///< The magnetometer sensors
struct OrientationReps body_to_imu; ///< Rotation from body to imu (all sensors) frame
uint8_t gyro_abi_send_id; ///< Filter out and send only a specific ABI id in telemetry for the gyro
uint8_t accel_abi_send_id; ///< Filter out and send only a specific ABI id in telemetry for the accelerometer
uint8_t mag_abi_send_id; ///< Filter out and send only a specific ABI id in telemetry for the magnetometer

/** flag for adjusting body_to_imu via settings.
* if FALSE, reset to airframe values, if TRUE set current roll/pitch
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/imu/imu_cube.c
View file
Expand Up @@ -74,7 +74,7 @@ void imu_cube_init(void)
imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;

imu2.gyro_odr = INVENSENSE3_GYRO_ODR_4KHZ;
imu2.gyro_range = INVENSENSE3_GYRO_RANGE_500DPS;
imu2.gyro_range = INVENSENSE3_GYRO_RANGE_2000DPS;
imu2.accel_odr = INVENSENSE3_ACCEL_ODR_4KHZ;
imu2.accel_range = INVENSENSE3_ACCEL_RANGE_32G;
imu2.gyro_aaf = 977; // ~ODR/4
Expand Down
10 changes: 5 additions & 5 deletions sw/airborne/modules/ins/ins_flow.c
View file
Expand Up @@ -120,7 +120,7 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
static void ins_rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
static void ins_rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm, uint8_t num_act);
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
Expand Down Expand Up @@ -673,7 +673,7 @@ void ins_reset_local_origin(void)

void ins_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, int32_t flow_x UNUSED,
int32_t flow_y UNUSED,
int32_t flow_der_x, int32_t flow_der_y, float quality UNUSED, float size_divergence)
int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence)
{

// TODO: make parameters:
Expand Down Expand Up @@ -1467,18 +1467,18 @@ static void set_body_state_from_quat(void)

}

static void ins_rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act)
static void ins_rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg, uint8_t num_act)
{
ins_flow.RPM_num_act = num_act;
for (int i = 0; i < num_act; i++) {
ins_flow.RPM[i] = rpm[i];
ins_flow.RPM[i] = rpm_msg[i].rpm;
}
}


/* Update INS based on GPS information */
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp,
uint32_t stamp UNUSED,
struct GpsState *gps_s)
{

Expand Down
109 changes: 109 additions & 0 deletions sw/airborne/modules/nav/ground_detect_sensor.c
View file
@@ -0,0 +1,109 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* 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
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/nav/ground_detect_sensor.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Ground detection module relying on lidar to detect ground
*/

#include "modules/nav/ground_detect_sensor.h"
#include "state.h"

#if USE_GROUND_DETECT_INDI_THRUST
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#endif

#if USE_GROUND_DETECT_AGL_DIST
#include "modules/sonar/agl_dist.h"
#define GROUND_DETECT_SENSOR_AGL_MIN_VALUE 0.1
#endif

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

bool ground_detected = false;


#ifndef GROUND_DETECT_SENSOR_COUNTER_TRIGGER
#define GROUND_DETECT_SENSOR_COUNTER_TRIGGER 10
#endif

#ifndef GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD
#define GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD -5.0
#endif


void ground_detect_sensor_init(void)
{
ground_detected = false;
}

bool ground_detect(void) {
return ground_detected;
}

void ground_detect_sensor_periodic(void)
{
static int32_t counter = 0;

#if USE_GROUND_DETECT_INDI_THRUST
// Evaluate thrust given (less than hover thrust)
// Use the control effectiveness in thrust in order to estimate the thrust delivered (only works for multicopters)
float specific_thrust = 0.0; // m/s2
uint8_t i;
for (i = 0; i < INDI_NUM_ACT; i++) {
specific_thrust += actuator_state_filt_vect[i] * g1g2[3][i] * -((int32_t) act_is_servo[i] - 1);
}

ground_detected = false;
if (specific_thrust > GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD ) {
if (counter > GROUND_DETECT_SENSOR_COUNTER_TRIGGER) {
ground_detected = true;
}
} else {
counter = 0;
}

#ifdef DEBUG_GROUND_DETECT
uint8_t test_gd = ground_detected;
float payload[2];
payload[0] = specific_thrust;
payload[1] = stateGetAccelNed_f()->z;

RunOnceEvery(10, {DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, &test_gd); DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 4, payload);} );
#endif

#elif USE_GROUND_DETECT_AGL_DIST
if (agl_value && (agl_dist_value_filtered < GROUND_DETECT_SENSOR_AGL_MIN_VALUE)) {
counter += 1;
} else {
counter = 0;
}
if (counter > GROUND_DETECT_SENSOR_COUNTER_TRIGGER) {
ground_detected = true;
} else {
ground_detected = false;
}
#else
ground_detected = false;
#endif


}
36 changes: 36 additions & 0 deletions sw/airborne/modules/nav/ground_detect_sensor.h
View file
@@ -0,0 +1,36 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* 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
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/nav/ground_detect_sensor.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Ground detection module
*/

#ifndef GROUND_DETECT_SENSOR_H
#define GROUND_DETECT_SENSOR_H

#include "std.h"

extern void ground_detect_sensor_init(void);
extern void ground_detect_sensor_periodic(void);

extern bool ground_detect(void);

#endif // GROUND_DETECT_SENSOR_H
4 changes: 4 additions & 0 deletions sw/airborne/modules/px4_flash/px4_flash.c
View file
Expand Up @@ -37,6 +37,10 @@
#include "led.h"
#include "mcu_arch.h"

#ifdef STM32_MCU_ARCH_H // scb_reset_system from OpenCM3
#include <libopencm3/cm3/scb.h>
#endif

#include "mcu_periph/sys_time.h"
#ifdef INTER_MCU_AP
tid_t px4iobl_tid; ///< id for time out of the px4 bootloader reset
Expand Down
32 changes: 27 additions & 5 deletions sw/airborne/modules/sensors/airspeed_ets.c
View file
Expand Up @@ -49,15 +49,19 @@
#include "modules/datalink/downlink.h"
#include <math.h>

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

#ifndef USE_AIRSPEED_ETS
#if USE_AIRSPEED
#define USE_AIRSPEED_ETS TRUE
PRINT_CONFIG_MSG("USE_AIRSPEED_ETS automatically set to TRUE")
#endif
#endif

#if !USE_AIRSPEED_ETS && !AIRSPEED_ETS_SYNC_SEND
#warning either set USE_AIRSPEED_ETS or AIRSPEED_ETS_SYNC_SEND to use airspeed_ets
#if !USE_AIRSPEED_ETS
PRINT_CONFIG_MSG("AIRSPEED_ETS not used")
#endif

#define AIRSPEED_ETS_ADDR 0xEA
Expand Down Expand Up @@ -93,6 +97,7 @@ bool log_airspeed_ets_started;
#endif



// Global variables
uint16_t airspeed_ets_raw;
uint16_t airspeed_ets_offset;
Expand All @@ -111,6 +116,21 @@ uint16_t airspeed_ets_cnt;
uint32_t airspeed_ets_delay_time;
bool airspeed_ets_delay_done;

static void airspeed_ets_downlink(struct transport_tx *trans, struct link_device *dev)
{
uint8_t dev_id = AIRSPEED_ETS_ID;
float press = 0;
float temp = 0;
float offset = airspeed_ets_offset;
pprz_msg_send_AIRSPEED_RAW(trans,dev,AC_ID,
&dev_id,
&airspeed_ets_raw,
&offset,
&press,
&temp,
&airspeed_ets);
}

void airspeed_ets_init(void)
{
int n;
Expand All @@ -133,6 +153,11 @@ void airspeed_ets_init(void)
airspeed_ets_delay_done = false;
SysTimeTimerStart(airspeed_ets_delay_time);


#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_ets_downlink);
#endif

#ifndef SITL
#if AIRSPEED_ETS_SDLOG
log_airspeed_ets_started = false;
Expand Down Expand Up @@ -232,9 +257,6 @@ void airspeed_ets_read_event(void)

#if USE_AIRSPEED_ETS
stateSetAirspeed_f(airspeed_ets);
#endif
#if AIRSPEED_ETS_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
airspeed_ets = 0.0;
Expand Down
13 changes: 10 additions & 3 deletions sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c
View file
Expand Up @@ -175,9 +175,14 @@ static Butterworth2LowPass ms45xx_filter;

static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_AIRSPEED_MS45XX(trans,dev,AC_ID,
uint8_t dev_id = MS45XX_SENDER_ID;
float temp = ((float)ms45xx.temperature) * 0.1f;
pprz_msg_send_AIRSPEED_RAW(trans,dev,AC_ID,
&dev_id,
&ms45xx.raw_p,
&ms45xx.pressure_offset,
&ms45xx.pressure,
&ms45xx.temperature,
&temp,
&ms45xx.airspeed);
}

Expand All @@ -186,6 +191,7 @@ void ms45xx_i2c_init(void)
ms45xx.pressure = 0.;
ms45xx.temperature = 0;
ms45xx.airspeed = 0.;
ms45xx.raw_p = 0;
ms45xx.pressure_type = MS45XX_PRESSURE_TYPE;
ms45xx.pressure_scale = MS45XX_PRESSURE_SCALE;
ms45xx.pressure_offset = MS45XX_PRESSURE_OFFSET;
Expand All @@ -200,7 +206,7 @@ void ms45xx_i2c_init(void)
#endif

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, ms45xx_downlink);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, ms45xx_downlink);
#endif
}

Expand Down Expand Up @@ -228,6 +234,7 @@ void ms45xx_i2c_event(void)
if (status == 0) {
/* 14bit raw pressure */
uint16_t p_raw = 0x3FFF & (((uint16_t)(ms45xx_trans.buf[0]) << 8) | (uint16_t)(ms45xx_trans.buf[1]));
ms45xx.raw_p = p_raw;

/* 11bit raw temperature, 5 LSB bits not used */
uint16_t temp_raw = 0xFFE0 & (((uint16_t)(ms45xx_trans.buf[2]) << 8) |
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/sensors/airspeed_ms45xx_i2c.h
View file
Expand Up @@ -31,6 +31,7 @@
struct AirspeedMs45xx {
float pressure; ///< (differential) pressure in Pascal
int16_t temperature; ///< Temperature in 0.1 deg Celcius
uint16_t raw_p; ///< Raw pressure
float airspeed; ///< Airspeed in m/s estimated from (differential) pressure.
bool pressure_type; ///< Pressure type Differential of Gauge
float airspeed_scale; ///< Quadratic scale factor to convert (differential) pressure to airspeed
Expand Down
12 changes: 8 additions & 4 deletions sw/airborne/modules/sensors/airspeed_sdp3x.c
View file
Expand Up @@ -143,10 +143,13 @@ static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)

static void sdp3x_downlink(struct transport_tx *trans, struct link_device *dev)
{
int16_t temp = (int16_t)(sdp3x.temperature * 10.f);
pprz_msg_send_AIRSPEED_MS45XX(trans,dev,AC_ID,
uint8_t dev_id = SDP3X_SENDER_ID;
pprz_msg_send_AIRSPEED_RAW(trans,dev,AC_ID,
&dev_id,
&sdp3x.raw_p,
&sdp3x.pressure_offset,
&sdp3x.pressure,
&temp,
&sdp3x.temperature,
&sdp3x.airspeed);
}

Expand All @@ -170,7 +173,7 @@ void sdp3x_init(void)
#endif

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, sdp3x_downlink); // FIXME
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, sdp3x_downlink); // FIXME
#endif
}

Expand Down Expand Up @@ -216,6 +219,7 @@ void sdp3x_event(void)
}

int16_t p_raw = ((int16_t)(buf[0]) << 8) | (int16_t)(buf[1]);
sdp3x.raw_p = (uint16_t) p_raw;

float p_out = ((float)p_raw / sdp3x.pressure_scale) - sdp3x.pressure_offset;

Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/sensors/airspeed_sdp3x.h
View file
Expand Up @@ -37,6 +37,7 @@ struct AirspeedSdp3x {
bool autoset_offset; ///< Set offset value from current filtered value
bool sync_send; ///< Flag to enable sending every new measurement via telemetry for debugging purpose
bool initialized; ///< init flag
uint16_t raw_p; ///< raw value from chip
};

extern struct AirspeedSdp3x sdp3x;
Expand Down
14 changes: 11 additions & 3 deletions sw/airborne/modules/sensors/airspeed_uavcan.c
View file
Expand Up @@ -61,9 +61,17 @@ 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,
uint8_t dev_id = UAVCAN_SENDER_ID;
uint16_t raw = 0;
float offset = 0;
float airspeed = 0;
pprz_msg_send_AIRSPEED_RAW(trans,dev,AC_ID,
&dev_id,
&raw,
&offset,
&airspeed_uavcan.diff_p,
&airspeed_uavcan.temperature);
&airspeed_uavcan.temperature,
&airspeed);
}

static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
Expand Down Expand Up @@ -118,6 +126,6 @@ void airspeed_uavcan_init(void)
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);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink);
#endif
}
5 changes: 3 additions & 2 deletions sw/airborne/modules/sensors/rpm_sensor.c
View file
Expand Up @@ -60,8 +60,9 @@ void rpm_sensor_init(void)
/* RPM periodic */
void rpm_sensor_periodic(void)
{
rpm = update_first_order_low_pass(&rpm_lp, rpm_sensor_get_rpm());
AbiSendMsgRPM(RPM_SENSOR_ID, &rpm, 1);
struct rpm_act_t rpm_msg = {0, 0};
rpm_msg.rpm = update_first_order_low_pass(&rpm_lp, rpm_sensor_get_rpm());
AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_msg, 1);
}

/* Get the RPM sensor */
Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c
View file
Expand Up @@ -34,6 +34,10 @@
#include "mcu_periph/uart.h"
#include "mcu_periph/gpio.h"

#ifdef RADIO_CONTROL_BIND_IMPL_FUNC
#include "modules/radio_control/spektrum.h"
#endif

void spektrum_soft_bind_init(void)
{

Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c
View file
Expand Up @@ -31,6 +31,10 @@

#include "mcu_periph/gpio.h"

#ifdef RADIO_CONTROL_BIND_IMPL_FUNC
#include "modules/radio_control/spektrum.h"
#endif

void spektrum_soft_bind_init(void)
{

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/stereocam/stereocam.c
View file
Expand Up @@ -64,7 +64,7 @@

// general stereocam definitions
#if !defined(STEREO_BODY_TO_STEREO_PHI) || !defined(STEREO_BODY_TO_STEREO_THETA) || !defined(STEREO_BODY_TO_STEREO_PSI)
#warning "STEREO_BODY_TO_STEREO_XXX not defined. Using default Euler rotation angles (0,0,0)"
#pragma message "STEREO_BODY_TO_STEREO_XXX not defined. Using default Euler rotation angles (0,0,0)"
#endif

#ifndef STEREO_BODY_TO_STEREO_PHI
Expand Down
45 changes: 28 additions & 17 deletions sw/airborne/modules/system_identification/pprz_doublet.c
View file
Expand Up @@ -28,30 +28,34 @@



void doublet_init(struct doublet_t *doublet, float length_s, float extra_waiting_time_s, float current_time_s, bool mod3211)
void doublet_init(struct doublet_t *doublet, float length_s, float extra_waiting_time_s, float current_time_s, uint8_t mod)
{
doublet->t0 = current_time_s;
doublet->tf = length_s;
doublet->total_length_s = doublet->tf + extra_waiting_time_s;
doublet->mod3211 = mod3211;
doublet->mod = mod;
doublet->current_value = 0;
doublet->current_time_s = current_time_s;

if (mod3211) {
doublet->t1 = length_s / 9;
doublet->t2 = doublet->t1 * 4;
doublet->t3 = doublet->t1 * 6;
doublet->t4 = doublet->t1 * 7;
doublet->t5 = doublet->t1 * 8;
} else{
if (mod==0) { // Normal doublet
doublet->t1 = length_s / 4;
doublet->t2 = doublet->t1 * 2;
doublet->t3 = doublet->t1 * 3;
doublet->t4 = doublet->t1 * 4;
doublet->t5 = doublet->tf;

} else if (mod==1) { // Half doublet
doublet->t1 = length_s / 4;
doublet->t2 = doublet->t1 * 2;
doublet->t3 = doublet->t1 * 3;
doublet->t4 = doublet->t1 * 4;
doublet->t5 = doublet->tf;
} else if (mod==2) { // 3211 doublet
doublet->t1 = length_s / 9;
doublet->t2 = doublet->t1 * 4;
doublet->t3 = doublet->t1 * 6;
doublet->t4 = doublet->t1 * 7;
doublet->t5 = doublet->t1 * 8;
}

}

void doublet_reset(struct doublet_t *doublet, float current_time_s){
Expand All @@ -74,25 +78,32 @@ float doublet_update(struct doublet_t *doublet, float current_time_s){

float t = current_time_s - doublet->t0; // since the start of the doublet
if ((t>=0) & (t<=doublet->tf)){
if (doublet->mod3211){
if (((t >= doublet->t1) & (t <= doublet->t2)) | ((t >= doublet->t3) & (t <= doublet->t4))){
if (doublet->mod == 0) {
if((t >= doublet->t1) & (t <= doublet->t2)){
doublet->current_value = 1.0f;
}else if(((t >= doublet->t2) && (t <= doublet->t3)) | ((t >= doublet->t4) && (t <= doublet->t5))){
}else if((t >= doublet->t2) & (t <= doublet->t3)){
doublet->current_value = -1.0f;
}else{
doublet->current_value = 0.0f;
}
}
else{
} else if (doublet->mod == 1) {
if((t >= doublet->t1) & (t <= doublet->t2)){
doublet->current_value = 1.0f;
}else if((t >= doublet->t2) & (t <= doublet->t3)){
doublet->current_value = 1.0f;
}else{
doublet->current_value = 0.0f;
}
} else if (doublet->mod == 2) {
if (((t >= doublet->t1) & (t <= doublet->t2)) | ((t >= doublet->t3) & (t <= doublet->t4))){
doublet->current_value = 1.0f;
}else if(((t >= doublet->t2) && (t <= doublet->t3)) | ((t >= doublet->t4) && (t <= doublet->t5))){
doublet->current_value = -1.0f;
}else{
doublet->current_value = 0.0f;
}
}
}else{
} else {
doublet->current_value = 0.0f;
}
return doublet->current_value;
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/modules/system_identification/pprz_doublet.h
View file
Expand Up @@ -65,11 +65,11 @@ struct doublet_t{
float current_value;
float current_time_s;

bool mod3211;
uint8_t mod;
};

extern void doublet_init(struct doublet_t *doublet, float length_s,
float extra_waiting_time_s, float current_time_s, bool mod3211);
float extra_waiting_time_s, float current_time_s, uint8_t mod3211);

extern void doublet_reset(struct doublet_t *doublet, float current_time_s);

Expand Down
155 changes: 155 additions & 0 deletions sw/airborne/modules/system_identification/sys_id_auto_doublets.c
View file
@@ -0,0 +1,155 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* 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
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/system_identification/sys_id_auto_doublets.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Module that automatically runs a doublet program for the rotating wing drone
*/
#include "std.h"

#include "modules/system_identification/sys_id_auto_doublets.h"
#include "modules/system_identification/sys_id_doublet.h"
#include "modules/system_identification/pprz_doublet.h"

#include "mcu_periph/sys_time.h"
#include "generated/airframe.h"

// Perform checks if mandatory macros are defined

#ifndef SYS_ID_AUTO_DOUBLETS_N_ACTUATORS
#error No doublet actuators SYS_ID_AUTO_DOUBLETS_N_ACTUATORS defined
#endif

#ifndef SYS_ID_AUTO_DOUBLETS_ACTUATORS
#error No doublet actuators SYS_ID_AUTO_DOUBLETS_ACTUATORS defined
#endif

#ifndef SYS_ID_AUTO_DOUBLETS_AMPLITUDE
#error No doublet actuators SYS_ID_AUTO_DOUBLETS_AMPLITUDE defined
#endif

uint8_t sys_id_auto_doublets_actuators[SYS_ID_AUTO_DOUBLETS_N_ACTUATORS] = SYS_ID_AUTO_DOUBLETS_ACTUATORS;
int16_t sys_id_auto_doublets_amplitude[SYS_ID_AUTO_DOUBLETS_N_ACTUATORS] = SYS_ID_AUTO_DOUBLETS_AMPLITUDE;

float sys_id_auto_doublets_time = 0.5; // time of one doublet
float sys_id_auto_doublets_interval_time = 5.; // time interval for doublets
int8_t sys_id_auto_doublets_n_repeat = 5; // The number of times a doublet has to be repeated on a single actuator

bool sys_id_auto_doublets_activated = false;

// Other variables needed for the periodic loop
uint8_t sys_id_auto_doublets_n_doublets;

float sys_id_auto_doublets_start_time_s;
float sys_id_auto_doublets_timer;
uint8_t sys_id_auto_doublets_counter;

inline void perform_sys_id_auto_doublets(uint8_t actuator_index);
inline void sys_id_auto_doublets_on_deactivation(void);

void init_sys_id_auto_doublets(void)
{
sys_id_auto_doublets_n_doublets = SYS_ID_AUTO_DOUBLETS_N_ACTUATORS * sys_id_auto_doublets_n_repeat;
sys_id_auto_doublets_start_time_s = 0.;
sys_id_auto_doublets_timer = 0.;
sys_id_auto_doublets_counter = 0;
}

void periodic_sys_id_auto_doublets(void)
{
// your periodic code here.
// freq = 20.0 Hz

// Run if doublet activated
if (sys_id_auto_doublets_activated)
{
float current_time_s = get_sys_time_float();
sys_id_auto_doublets_timer = current_time_s - sys_id_auto_doublets_start_time_s;

// Check when to increase the counters and perform doublet
uint8_t current_index = sys_id_auto_doublets_timer / sys_id_auto_doublets_interval_time;

// stop doublets when all doublets done
if (current_index >= sys_id_auto_doublets_n_doublets)
{
sys_id_auto_doublets_on_deactivation();
}
else
{ // continue doublets when not finished yet
// Check if doublet should be performed and index inxreased
if (current_index > sys_id_auto_doublets_counter)
{
uint8_t actuator_index = current_index / sys_id_auto_doublets_n_repeat;
perform_sys_id_auto_doublets(actuator_index);
sys_id_auto_doublets_counter++;
}
}
}
}

void perform_sys_id_auto_doublets(uint8_t actuator_index)
{
// Do nothing id doublet index is invalid
if (actuator_index >= SYS_ID_AUTO_DOUBLETS_N_ACTUATORS)
{
return;
}
// Set correct settings
sys_id_doublet_axis_handler(sys_id_auto_doublets_actuators[actuator_index]);
doublet_length_s = sys_id_auto_doublets_time;
doublet_amplitude = sys_id_auto_doublets_amplitude[actuator_index];

// call doublet activation handler
sys_id_doublet_activate_handler(1);
}

void sys_id_auto_doublets_on_activation(uint8_t active)
{
sys_id_auto_doublets_activated = active;
// If the activation boolean is set to true
if (sys_id_auto_doublets_activated)
{
if (sys_id_auto_doublets_timer > 0)
{
// Do nothing if already activated
}
else
{
// Activate auto doublet if not activated yet
sys_id_auto_doublets_start_time_s = get_sys_time_float();
sys_id_auto_doublets_timer = 0;
perform_sys_id_auto_doublets(0);
}
}
else
{ // If the activation boolean is set to false
sys_id_auto_doublets_on_deactivation();
}
}

void sys_id_auto_doublets_on_deactivation(void)
{
// call doublet deactivation handler
sys_id_doublet_activate_handler(0);
sys_id_auto_doublets_activated = false;
sys_id_auto_doublets_timer = 0;
sys_id_auto_doublets_counter = 0;
doublet_amplitude = 0;
}
37 changes: 37 additions & 0 deletions sw/airborne/modules/system_identification/sys_id_auto_doublets.h
View file
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* 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
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/system_identification/sys_id_auto_doublets.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Module that automatically runs a doublet program for the rotating wing drone
*/

#ifndef SYS_ID_AUTO_DOUBLETS_H
#define SYS_ID_AUTO_DOUBLETS_H

#include "std.h"

extern bool sys_id_auto_doublets_activated;

extern void init_sys_id_auto_doublets(void);
extern void periodic_sys_id_auto_doublets(void);
extern void sys_id_auto_doublets_on_activation(uint8_t active);

#endif // SYS_ID_AUTO_DOUBLETS_H
89 changes: 61 additions & 28 deletions sw/airborne/modules/system_identification/sys_id_chirp.c
View file
Expand Up @@ -35,24 +35,29 @@
#include "math/pprz_random.h"


#ifndef CHIRP_AXES
#define CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#ifndef SYS_ID_CHIRP_AXES
#define SYS_ID_CHIRP_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#endif

#ifndef CHIRP_ENABLED
#define CHIRP_ENABLED TRUE
#ifndef SYS_ID_CHIRP_ENABLED
#define SYS_ID_CHIRP_ENABLED TRUE
#endif

#ifndef CHIRP_USE_NOISE
#define CHIRP_USE_NOISE TRUE
#ifndef SYS_ID_CHIRP_USE_NOISE
#define SYS_ID_CHIRP_USE_NOISE TRUE
#endif

// #ifndef CHIRP_EXPONENTIAL
// #define CHIRP_EXPONENTIAL TRUE
#ifdef SYS_ID_CHIRP_RADIO_CHANNEL
#include "modules/radio_control/radio_control.h"
pprz_t previous_radio_value_chirp = 0;
#endif

// #ifndef SYS_ID_CHIRP_EXPONENTIAL
// #define SYS_ID_CHIRP_EXPONENTIAL TRUE
// #endif

// #ifndef CHIRP_FADEIN
// #define CHIRP_FADEIN TRUE
// #ifndef SYS_ID_CHIRP_FADEIN
// #define SYS_ID_CHIRP_FADEIN TRUE
// #endif


Expand All @@ -70,28 +75,28 @@ uint8_t chirp_fade_in = false;
uint8_t chirp_exponential = false;

// The axes on which noise and chirp values can be applied
static const int8_t ACTIVE_CHIRP_AXES[] = CHIRP_AXES;
#define CHIRP_NB_AXES sizeof ACTIVE_CHIRP_AXES / sizeof ACTIVE_CHIRP_AXES[0] // Number of items in ACTIVE_CHIRP_AXES
static const int8_t SYS_ID_ACTIVE_CHIRP_AXES[] = SYS_ID_CHIRP_AXES;
#define SYS_ID_CHIRP_NB_AXES sizeof SYS_ID_ACTIVE_CHIRP_AXES / sizeof SYS_ID_ACTIVE_CHIRP_AXES[0] // Number of items in ACTIVE_CHIRP_AXES

// Filters used to cut-off the gaussian noise fed into the actuator channels
static struct FirstOrderLowPass filters[CHIRP_NB_AXES];
static struct FirstOrderLowPass filters[SYS_ID_CHIRP_NB_AXES];

// Chirp and noise values for all axes (indices correspond to the axes given in CHIRP_AXES)
static pprz_t current_chirp_values[CHIRP_NB_AXES];
static pprz_t current_chirp_values[SYS_ID_CHIRP_NB_AXES];

static void set_current_chirp_values(void)
{
// initializing at zero the chirp input for every axis
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) {
current_chirp_values[i] = 0;
}
// adding values if the chirp is active
if (chirp_active) {
// adding extra on the chirp signal (both on-axis and off axis)
#if CHIRP_USE_NOISE
#if SYS_ID_CHIRP_USE_NOISE

float amplitude, noise;
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) {
noise = update_first_order_low_pass(&filters[i], rand_gaussian());
amplitude = chirp_axis == i ? chirp_noise_stdv_onaxis_ratio * chirp_amplitude : chirp_noise_stdv_offaxis;
current_chirp_values[i] += (int32_t)(noise * amplitude);
Expand All @@ -101,7 +106,7 @@ static void set_current_chirp_values(void)
// adding nominal chirp value
current_chirp_values[chirp_axis] += (int32_t)(chirp_amplitude * chirp.current_value);
} else {
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) {
current_chirp_values[i] = 0;
}
}
Expand Down Expand Up @@ -132,6 +137,13 @@ static void stop_chirp(void)
void sys_id_chirp_activate_handler(uint8_t activate)
{
chirp_active = activate;
#ifdef SYS_ID_CHIRP_RADIO_CHANNEL
// Don't activate chirp when radio signal is low
if (radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL] < 1750)
{
chirp_active = 0;
}
#endif
if (chirp_active) {
chirp_init(&chirp, chirp_fstart_hz, chirp_fstop_hz, chirp_length_s, get_sys_time_float(), chirp_exponential,
chirp_fade_in);
Expand All @@ -148,7 +160,7 @@ uint8_t sys_id_chirp_running(void)

extern void sys_id_chirp_axis_handler(uint8_t axis)
{
if (axis < CHIRP_NB_AXES) {
if (axis < SYS_ID_CHIRP_NB_AXES) {
chirp_axis = axis;
}
}
Expand Down Expand Up @@ -179,7 +191,7 @@ extern void sys_id_chirp_exponential_activate_handler(uint8_t exponential)

void sys_id_chirp_init(void)
{
#if CHIRP_USE_NOISE
#if SYS_ID_CHIRP_USE_NOISE

init_random();

Expand All @@ -192,15 +204,37 @@ void sys_id_chirp_init(void)

// Filter cutoff frequency is the chirp maximum frequency
float tau = 1 / (chirp_fstop_hz * 2 * M_PI);
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) {
init_first_order_low_pass(&filters[i], tau, SYS_ID_CHIRP_RUN_PERIOD, 0);
current_chirp_values[i] = 0;
}
}

void sys_id_chirp_run(void)
{
#if CHIRP_ENABLED
#if SYS_ID_CHIRP_ENABLED

#ifdef SYS_ID_CHIRP_RADIO_CHANNEL
// Check if chirp switched on when off before
if (previous_radio_value_chirp < 1750)
{
if (radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL] > 1750)
{
// Activate chirp
sys_id_chirp_activate_handler(1);
}
}
// Check if chirp switched off when on before
if (previous_radio_value_chirp > 1750)
{
if (radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL] < 1750)
{
// Deactivate chirp
sys_id_chirp_activate_handler(0);
}
}
previous_radio_value_chirp = radio_control.values[SYS_ID_CHIRP_RADIO_CHANNEL];
#endif

if (chirp_active) {
if (!chirp_is_running(&chirp, get_sys_time_float())) {
Expand All @@ -214,16 +248,15 @@ void sys_id_chirp_run(void)
#endif
}

void sys_id_chirp_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
void sys_id_chirp_add_values(bool UNUSED motors_on, bool UNUSED override_on, pprz_t UNUSED in_cmd[])
{
(void)(override_on); // Suppress unused parameter warnings

#if CHIRP_ENABLED
#if SYS_ID_CHIRP_ENABLED

if (motors_on) {
for (uint8_t i = 0; i < CHIRP_NB_AXES; i++) {
in_cmd[ACTIVE_CHIRP_AXES[i]] += current_chirp_values[i];
BoundAbs(in_cmd[ACTIVE_CHIRP_AXES[i]], MAX_PPRZ);
for (uint8_t i = 0; i < SYS_ID_CHIRP_NB_AXES; i++) {
in_cmd[SYS_ID_ACTIVE_CHIRP_AXES[i]] += current_chirp_values[i];
BoundAbs(in_cmd[SYS_ID_ACTIVE_CHIRP_AXES[i]], MAX_PPRZ);
}
}

Expand Down
81 changes: 56 additions & 25 deletions sw/airborne/modules/system_identification/sys_id_doublet.c
View file
Expand Up @@ -40,41 +40,45 @@
#include "generated/airframe.h"
#include "mcu_periph/sys_time.h"

#ifndef DOUBLET_AXES
#define DOUBLET_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#ifndef SYS_ID_DOUBLET_AXES
#define SYS_ID_DOUBLET_AXES {COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}
#endif

#ifndef DOUBLET_ENABLED
#define DOUBLET_ENABLED TRUE
#ifndef SYS_ID_DOUBLET_ENABLED
#define SYS_ID_DOUBLET_ENABLED TRUE
#endif

#ifdef SYS_ID_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;



static const int8_t ACTIVE_DOUBLET_AXES[] = DOUBLET_AXES;
#define DOUBLET_NB_AXES sizeof ACTIVE_DOUBLET_AXES / sizeof ACTIVE_DOUBLET_AXES[0] // Number of items in ACTIVE_DOUBLET_AXES
static const int8_t SYS_ID_ACTIVE_DOUBLET_AXES[] = SYS_ID_DOUBLET_AXES;
#define SYS_ID_DOUBLET_NB_AXES sizeof SYS_ID_ACTIVE_DOUBLET_AXES / sizeof SYS_ID_ACTIVE_DOUBLET_AXES[0] // Number of items in ACTIVE_DOUBLET_AXES

static pprz_t current_doublet_values[DOUBLET_NB_AXES];
static pprz_t current_doublet_values[SYS_ID_DOUBLET_NB_AXES];

static void set_current_doublet_values(void)
{
if (doublet_active) {
current_doublet_values[doublet_axis] = (int32_t)(doublet_amplitude * doublet.current_value);

} else {
for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
for (uint8_t i = 0; i < SYS_ID_DOUBLET_NB_AXES; i++) {
current_doublet_values[i] = 0;
}
}
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[SYS_ID_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 @@ -117,29 +128,50 @@ void sys_id_doublet_activate_handler(uint8_t activate)

void sys_id_doublet_axis_handler(uint8_t axis)
{
if (axis < DOUBLET_NB_AXES) {
if (axis < SYS_ID_DOUBLET_NB_AXES) {
doublet_axis = 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);

for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
for (uint8_t i = 0; i < SYS_ID_DOUBLET_NB_AXES; i++) {
current_doublet_values[i] = 0;
}
}

void sys_id_doublet_run(void)
{
{
#ifdef SYS_ID_DOUBLET_RADIO_CHANNEL
// Check if doublet switched on when off before
if (previous_radio_value_doublet < 1750)
{
if (radio_control.values[SYS_ID_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[SYS_ID_DOUBLET_RADIO_CHANNEL] < 1750)
{
// Deactivate doublet
sys_id_doublet_activate_handler(0);
}
}
previous_radio_value_doublet = radio_control.values[SYS_ID_DOUBLET_RADIO_CHANNEL];
#endif
if (doublet_active) {
if (!doublet_is_running(&doublet, get_sys_time_float())) {
stop_doublet();
Expand All @@ -152,16 +184,15 @@ void sys_id_doublet_run(void)

}

void sys_id_doublet_add_values(bool motors_on, bool override_on, pprz_t in_cmd[])
void sys_id_doublet_add_values(bool UNUSED motors_on, bool UNUSED override_on, pprz_t UNUSED in_cmd[])
{
(void)(override_on); // Suppress unused parameter warnings

#if DOUBLET_ENABLED
#if SYS_ID_DOUBLET_ENABLED

if (motors_on) {
for (uint8_t i = 0; i < DOUBLET_NB_AXES; i++) {
in_cmd[ACTIVE_DOUBLET_AXES[i]] += current_doublet_values[i];
BoundAbs(in_cmd[ACTIVE_DOUBLET_AXES[i]], MAX_PPRZ);
for (uint8_t i = 0; i < SYS_ID_DOUBLET_NB_AXES; i++) {
in_cmd[SYS_ID_ACTIVE_DOUBLET_AXES[i]] += current_doublet_values[i];
BoundAbs(in_cmd[SYS_ID_ACTIVE_DOUBLET_AXES[i]], MAX_PPRZ);
}
}

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
5 changes: 3 additions & 2 deletions sw/airborne/test/test_can.c
View file
Expand Up @@ -34,7 +34,8 @@
static inline void main_init(void);
static inline void main_periodic_task(void);
static inline void main_event_task(void);
void main_on_can_msg(uint32_t id, uint8_t *data, int len);
void main_on_can_msg(uint32_t id, uint8_t *data, uint8_t len);


uint8_t tx_data[8];
uint8_t rx_data[8];
Expand Down Expand Up @@ -124,7 +125,7 @@ static inline void main_event_task(void)

}

void main_on_can_msg(uint32_t id __attribute__((unused)), uint8_t *data, int len __attribute__((unused)))
void main_on_can_msg(uint32_t id UNUSED, uint8_t *data, uint8_t len UNUSED)
{
for (int i = 0; i < 8; i++) {
rx_data[i] = data[i];
Expand Down
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+19 −11 message_definitions/v1.0/messages.xml
2 changes: 2 additions & 0 deletions sw/tools/generators/gen_airframe.ml
View file
Expand Up @@ -315,8 +315,10 @@ let rec parse_section = fun out ac_id s ->
let driver = ExtXml.attrib_or_default s "driver" "Default" in
let servos = Xml.children s in
let nb_servos = List.fold_right (fun s m -> max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in
let servos_offset = Hashtbl.length servos_drivers in

define_out out (sprintf "SERVOS_%s_NB" (String.uppercase_ascii driver)) (string_of_int nb_servos);
define_out out (sprintf "SERVOS_%s_OFFSET" (String.uppercase_ascii driver)) (string_of_int servos_offset);
fprintf out "#include \"modules/actuators/actuators_%s.h\"\n" (String.lowercase_ascii driver);
fprintf out "\n";
List.iter (parse_servo out driver) servos;
Expand Down
4 changes: 2 additions & 2 deletions sw/tools/generators/gen_flight_plan.ml
View file
Expand Up @@ -1065,7 +1065,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
else if geofence_max_alt < (float_of_string alt) then
fprintf stderr "\nWarning: Geofence max altitude below default waypoint alt (%.0f < %.0f)\n" geofence_max_alt (float_of_string alt);
Xml2h.define_out out "GEOFENCE_MAX_ALTITUDE" (sof geofence_max_alt);
fprintf stderr "\nWarning: Geofence max altitude set to %.0f\n" geofence_max_alt;
fprintf stderr "\nNOTICE: Geofence max altitude set to %.0f\n" geofence_max_alt;
with
_ -> ()
end;
Expand All @@ -1086,7 +1086,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
else if (geofence_max_height +. !ground_alt) < (float_of_string alt) then
fprintf stderr "\nWarning: Geofence max AGL below default waypoint AGL (%.0f < %.0f)\n" (geofence_max_height +. !ground_alt) (float_of_string alt);
Xml2h.define_out out "GEOFENCE_MAX_HEIGHT" (sof geofence_max_height);
fprintf stderr "\nWarning: Geofence max AGL set to %.0f\n" geofence_max_height;
fprintf stderr "\nNOTICE: Geofence max AGL set to %.0f\n" geofence_max_height;
with
_ -> ()
end;
Expand Down
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