312 changes: 312 additions & 0 deletions sw/airborne/modules/checks/pfc_actuators.c
View file
@@ -0,0 +1,312 @@
/*
* Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/**
* @file "modules/checks/pfc_actuators.c"
* @author Freek van Tienen <freek.v.tienen@gmail.com>
* Checks the actuators with feedback before takeoff
*/

#include "preflight_checks.h"
#include "core/abi.h"
#include "modules/datalink/telemetry.h"
#include <stdio.h>

/**
* Maximum error for the angle of the actuators (rad)
*/
#ifndef PFC_ACTUATORS_MAX_ANGLE_ERROR
#define PFC_ACTUATORS_MAX_ANGLE_ERROR 0.1f
#endif

/**
* Maximum error for the RPM of the actuators
*/
#ifndef PFC_ACTUATORS_MAX_RPM_ERROR
#define PFC_ACTUATORS_MAX_RPM_ERROR 250.0f
#endif

/**
* Enable debugging to set the expected feedback values
*/
#ifndef PFC_ACTUATORS_DEBUG
#define PFC_ACTUATORS_DEBUG false
#endif

/**
* @brief The status of the preflight checks
*/
enum pfc_actuators_state_t {
PFC_ACTUATORS_STATE_INIT,
PFC_ACTUATORS_STATE_RUNNING,
PFC_ACTUATORS_STATE_SUCCESS,
PFC_ACTUATORS_STATE_ERROR
};

/**
* @brief The state of the actuator during the test
*/
enum pfc_actuator_state_t {
PFC_ACTUATOR_STATE_WAIT,
PFC_ACTUATOR_STATE_LOW,
PFC_ACTUATOR_STATE_HIGH,
};

/**
* @brief The configuration struct of the actuator
*/
struct pfc_actuator_t {
uint8_t feedback_id; ///< The feedback id of the actuator (255 for none)
uint8_t feedback_id2; ///< The secondary feedback id of the actuator (255 for none)

int16_t low; ///< The low value to set the actuator to
int16_t high; ///< The high value to set the actuator to
float low_feedback; ///< The expected feedback value when the actuator is low
float high_feedback; ///< The expected feedback value when the actuator is high

float timeout; ///< The timeout for the actuator to move
};

struct pfc_actuators_t {
enum pfc_actuators_state_t state; ///< The state of the preflight checks

uint8_t act_idx; ///< The current actuator index
uint8_t act_nb; ///< The number of actuators
enum pfc_actuator_state_t act_state; ///< The state of the actuator (during the test)
float act_start_time; ///< The start time of the actuator (during the test)

float last_feedback; ///< The last measured feedback value of the actuator
float last_feedback_err; ///< The last expected feedback error of the actuator (based on RPM/angle)
float last_feedback2; ///< The last measured secondary feedback value of the actuator
float last_feedback_err2; ///< The last expected secondary feedback error of the actuator (based on RPM/angle)
};

// Local variables and functions
static struct pfc_actuator_t pfc_acts[] = PFC_ACTUATORS;
static struct pfc_actuators_t pfc_actuators;
static struct preflight_check_t actuators_pfc;
static abi_event act_feedback_ev;
static void pfc_actuators_cb(struct preflight_result_t *result);
static void pfc_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);

/**
* @brief Send an error message to the ground station
* @param fmt The format of the message
* @param ... The arguments for the format
*/
static void pfc_actuators_error(const char *fmt, ...) {
char msg[200];

// Add the error
va_list args;
va_start(args, fmt);
int rc = vsnprintf(msg, 200, fmt, args);
va_end(args);

if(rc > 0) {
DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, msg);
}
}

/**
* @brief Send a debug message to the ground station
*/
#if PFC_ACTUATORS_DEBUG
#define pfc_actuators_debug pfc_actuators_error
#else
#define pfc_actuators_debug(...)
#endif

/**
* @brief Register the preflight checks for the actuators
*/
void pfc_actuators_init(void) {
pfc_actuators.state = PFC_ACTUATORS_STATE_INIT;
pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
pfc_actuators.act_idx = 0;
pfc_actuators.act_nb = sizeof(pfc_acts) / sizeof(struct pfc_actuator_t);
pfc_actuators.last_feedback = NAN;
pfc_actuators.last_feedback2 = NAN;

preflight_check_register(&actuators_pfc, pfc_actuators_cb);
AbiBindMsgACT_FEEDBACK(ABI_BROADCAST, &act_feedback_ev, pfc_act_feedback_cb);
}

/**
* @brief Move the actuators, should be put in the command laws
*/
void pfc_actuators_run(void) {
// Only actuate when running
if(pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING || pfc_actuators.act_idx >= pfc_actuators.act_nb) {
return;
}

// Get the actuators
struct pfc_actuator_t *act = &pfc_acts[pfc_actuators.act_idx];

// Verify the result and continue
if((get_sys_time_float() - pfc_actuators.act_start_time) > act->timeout) {
switch(pfc_actuators.act_state) {
case PFC_ACTUATOR_STATE_WAIT:
pfc_actuators_debug("Actuator %d starting LOW", pfc_actuators.act_idx);
pfc_actuators.act_state = PFC_ACTUATOR_STATE_LOW;
pfc_actuators.act_start_time = get_sys_time_float();
pfc_actuators.last_feedback = NAN;
pfc_actuators.last_feedback2 = NAN;
break;
case PFC_ACTUATOR_STATE_LOW:
// Check if the feedback is correct
if(act->feedback_id != 255 &&
(pfc_actuators.last_feedback < (act->low_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->low_feedback + pfc_actuators.last_feedback_err))) {
pfc_actuators_error("Actuator %d not responding correctly LOW %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->low_feedback - pfc_actuators.last_feedback_err, pfc_actuators.last_feedback, act->low_feedback + pfc_actuators.last_feedback_err);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
} else if(act->feedback_id2 != 255 &&
(pfc_actuators.last_feedback2 < (act->low_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->low_feedback + pfc_actuators.last_feedback_err2))) {
pfc_actuators_error("Actuator %d not responding correctly LOW2 %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->low_feedback - pfc_actuators.last_feedback_err2, pfc_actuators.last_feedback2, act->low_feedback + pfc_actuators.last_feedback_err2);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
}
if(pfc_actuators.state != PFC_ACTUATORS_STATE_ERROR || PFC_ACTUATORS_DEBUG) {
pfc_actuators_debug("Actuator %d LOW ok %.2f, %.2f starting HIGH", pfc_actuators.act_idx, pfc_actuators.last_feedback, pfc_actuators.last_feedback2);
pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING;
pfc_actuators.act_state = PFC_ACTUATOR_STATE_HIGH;
pfc_actuators.act_start_time = get_sys_time_float();
pfc_actuators.last_feedback = NAN;
pfc_actuators.last_feedback2 = NAN;
}
break;
case PFC_ACTUATOR_STATE_HIGH:
// Check if the feedback is correct
if(act->feedback_id != 255 &&
(pfc_actuators.last_feedback < (act->high_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->high_feedback + pfc_actuators.last_feedback_err))) {
pfc_actuators_error("Actuator %d not responding correctly HIGH %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->high_feedback - pfc_actuators.last_feedback_err, pfc_actuators.last_feedback, act->high_feedback + pfc_actuators.last_feedback_err);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
} else if(act->feedback_id2 != 255 &&
(pfc_actuators.last_feedback2 < (act->high_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->high_feedback + pfc_actuators.last_feedback_err2))) {
pfc_actuators_error("Actuator %d not responding correctly HIGH2 %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->high_feedback - pfc_actuators.last_feedback_err2, pfc_actuators.last_feedback2, act->high_feedback + pfc_actuators.last_feedback_err2);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
}
if(pfc_actuators.state != PFC_ACTUATORS_STATE_ERROR || PFC_ACTUATORS_DEBUG) {
pfc_actuators_debug("Actuator %d HIGH ok %.2f, %.2f", pfc_actuators.act_idx, pfc_actuators.last_feedback, pfc_actuators.last_feedback2);
pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING;
pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
pfc_actuators.act_start_time = 0;
pfc_actuators.act_idx++;
}
break;
}
}

// Finished testing
if(pfc_actuators.act_idx >= pfc_actuators.act_nb) {
pfc_actuators.state = PFC_ACTUATORS_STATE_SUCCESS;
pfc_actuators_error("Actuators checks done %d", pfc_actuators.act_idx);
}
}

/**
* @brief Start the actuator testing
*/
void pfc_actuators_start(bool start) {
if(start && pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING) {
pfc_actuators.act_idx = 0;
pfc_actuators.act_start_time = 0;
pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING;
pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
pfc_actuators.last_feedback = NAN;
pfc_actuators.last_feedback2 = NAN;
}
else if(!start && pfc_actuators.state == PFC_ACTUATORS_STATE_RUNNING) {
pfc_actuators.act_idx = 0;
pfc_actuators.act_start_time = 0;
pfc_actuators.state = PFC_ACTUATORS_STATE_INIT;
pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT;
}
}

/**
* @brief Get the actuator value in the command laws to move the actuator during the preflight checks
* @param idx The index of the actuator in the preflight checks struct
* @param value The value if no checks are performed on this actuator
*/
int16_t pfc_actuators_value(uint8_t idx, int16_t value) {
if(idx != pfc_actuators.act_idx || pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING) {
return value;
}

if(pfc_actuators.act_state == PFC_ACTUATOR_STATE_LOW) {
return pfc_acts[pfc_actuators.act_idx].low;
}
else if(pfc_actuators.act_state == PFC_ACTUATOR_STATE_HIGH) {
return pfc_acts[pfc_actuators.act_idx].high;
}

return value;
}

/**
* @brief Check the actuators with feedback
*
* @param result The result of the preflight checks
*/
static void pfc_actuators_cb(struct preflight_result_t *result)
{
switch(pfc_actuators.state) {
case PFC_ACTUATORS_STATE_INIT:
preflight_error(result, "Actuators not checked perform checks first[%d]", pfc_actuators.act_nb);
break;
case PFC_ACTUATORS_STATE_RUNNING:
preflight_error(result, "Actuators are currently being checked[%d/%d]",pfc_actuators.act_idx, pfc_actuators.act_nb);
break;
case PFC_ACTUATORS_STATE_SUCCESS:
preflight_success(result, "Actuators checked and moved succesfully[%d]", pfc_actuators.act_nb);
break;
case PFC_ACTUATORS_STATE_ERROR:
preflight_error(result, "Actuators not responding correctly[%d/%d]",pfc_actuators.act_idx, pfc_actuators.act_nb);
break;
}
}

/**
* @brief Callback for the actuator feedback
*/
static void pfc_act_feedback_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *feedback, uint8_t num_act) {
if(pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING || pfc_actuators.act_idx >= pfc_actuators.act_nb) {
return;
}

// Save the last feedback values
for(uint8_t i = 0; i < num_act; i++) {
if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id && feedback[i].set.rpm) {
pfc_actuators.last_feedback = feedback[i].rpm;
pfc_actuators.last_feedback_err = PFC_ACTUATORS_MAX_RPM_ERROR;
} else if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id && feedback[i].set.position) {
pfc_actuators.last_feedback = feedback[i].position;
pfc_actuators.last_feedback_err = PFC_ACTUATORS_MAX_ANGLE_ERROR;
}

if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id2 && feedback[i].set.rpm) {
pfc_actuators.last_feedback2 = feedback[i].rpm;
pfc_actuators.last_feedback_err2 = PFC_ACTUATORS_MAX_RPM_ERROR;
} else if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id2 && feedback[i].set.position) {
pfc_actuators.last_feedback2 = feedback[i].position;
pfc_actuators.last_feedback_err2 = PFC_ACTUATORS_MAX_ANGLE_ERROR;
}
}
}
37 changes: 37 additions & 0 deletions sw/airborne/modules/checks/pfc_actuators.h
View file
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/**
* @file "modules/checks/pfc_actuators.h"
* @author Freek van Tienen <freek.v.tienen@gmail.com>
* Checks the actuators with feedback before takeoff
*/

#ifndef PFC_ACTUATORS_H
#define PFC_ACTUATORS_H

#include "std.h"

extern void pfc_actuators_init(void);
extern void pfc_actuators_run(void);
extern void pfc_actuators_start(bool start);
extern int16_t pfc_actuators_value(uint8_t idx, int16_t value);

#endif /* PFC_ACTUATORS_H */
5 changes: 3 additions & 2 deletions sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
View file
Expand Up @@ -180,11 +180,12 @@ void schdule_control_effectiveness(void) {

// Determine thrust of the wing to adjust the effectiveness of servos
float wing_thrust_scaling = 1.0;
#if INDI_NUM_ACT != 8
#if (INDI_NUM_ACT != 8) || (COMMAND_REAR_RIGHT_FLAP != 7)
#error "ctfl_eff_scheduling_nederdrone is very specific and only works for one Nederdrone configuration!"
#endif
if (i>3) {
float wing_thrust = actuators_pprz[i-4];
// Increase servo effectiveness depending on the thrust of the propeller of that wing: 0,1,2,3 = motors, 4,5,6,7 = flaps
float wing_thrust = stabilization_cmd[i-4];
Bound(wing_thrust,3000.0,9600.0);
wing_thrust_scaling = wing_thrust/9600.0/0.8;
}
Expand Down
7 changes: 4 additions & 3 deletions sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
View file
Expand Up @@ -257,9 +257,10 @@ void eff_scheduling_rot_wing_update_MMOI(void)

void eff_scheduling_rot_wing_update_cmd(void)
{
eff_sched_var.cmd_elevator = actuators_pprz[SERVO_SERVO_ELEVATOR];
eff_sched_var.cmd_pusher_scaled = actuators_pprz[SERVO_MOTOR_PUSH] * 0.000853229; // Scaled with 8181 / 9600 / 1000
eff_sched_var.cmd_T_mean_scaled = (actuators_pprz[SERVO_MOTOR_FRONT] + actuators_pprz[SERVO_MOTOR_RIGHT] + actuators_pprz[SERVO_MOTOR_BACK] + actuators_pprz[SERVO_MOTOR_LEFT]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
// These indexes depend on the INDI sequence, not the actuator IDX
eff_sched_var.cmd_elevator = stabilization_cmd[COMMAND_ELEVATOR];
eff_sched_var.cmd_pusher_scaled = stabilization_cmd[COMMAND_THRUST_X] * 0.000853229; // Scaled with 8181 / 9600 / 1000
eff_sched_var.cmd_T_mean_scaled = (stabilization_cmd[COMMAND_FRONT_MOTOR] + stabilization_cmd[COMMAND_RIGHT_MOTOR] + stabilization_cmd[COMMAND_BACK_MOTOR] + stabilization_cmd[COMMAND_LEFT_MOTOR]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
}

void eff_scheduling_rot_wing_update_airspeed(void)
Expand Down
16 changes: 16 additions & 0 deletions sw/airborne/modules/energy/electrical.c
View file
Expand Up @@ -69,6 +69,9 @@ PRINT_CONFIG_VAR(MIN_BAT_LEVEL)
#ifndef VoltageOfAdc
#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
#endif
#ifndef VBoardOfAdc
#define VBoardOfAdc(adc) DefaultVBoardOfAdc(adc)
#endif
#ifndef MilliAmpereOfAdc
#define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
#endif
Expand All @@ -93,6 +96,9 @@ static struct {
#ifdef ADC_CHANNEL_VSUPPLY
struct adc_buf vsupply_adc_buf;
#endif
#if defined ADC_CHANNEL_VBOARD
struct adc_buf vboard_adc_buf;
#endif
#if defined ADC_CHANNEL_CURRENT && !defined SITL
struct adc_buf current_adc_buf;
#endif
Expand Down Expand Up @@ -122,6 +128,7 @@ static void electrical_preflight(struct preflight_result_t *result) {
void electrical_init(void)
{
electrical.vsupply = 0.f;
electrical.vboard = 0.f;
electrical.current = 0.f;
electrical.charge = 0.f;
electrical.energy = 0.f;
Expand All @@ -135,6 +142,10 @@ void electrical_init(void)
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif

#if defined ADC_CHANNEL_VBOARD
adc_buf_channel(ADC_CHANNEL_VBOARD, &electrical_priv.vboard_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif

/* measure current if available, otherwise estimate it */
#if defined ADC_CHANNEL_CURRENT && !defined SITL
adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE);
Expand Down Expand Up @@ -163,6 +174,11 @@ void electrical_periodic(void)
electrical_priv.vsupply_adc_buf.av_nb_sample));
#endif

#if defined(ADC_CHANNEL_VBOARD) && !defined(SITL)
electrical.vboard = VBoardOfAdc((electrical_priv.vboard_adc_buf.sum /
electrical_priv.vboard_adc_buf.av_nb_sample));
#endif

#ifdef ADC_CHANNEL_CURRENT
#ifndef SITL
int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample;
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/energy/electrical.h
View file
Expand Up @@ -43,6 +43,7 @@

struct Electrical {
float vsupply; ///< supply voltage in V
float vboard; ///< board voltage in V
float current; ///< current in A
float charge; ///< consumed electric charge in Ah
float energy; ///< consumed energy in Wh
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/imu/imu.h
View file
Expand Up @@ -35,7 +35,7 @@
#include "generated/airframe.h"

#ifndef IMU_MAX_SENSORS
#define IMU_MAX_SENSORS 3
#define IMU_MAX_SENSORS 4
#endif

struct imu_calib_t {
Expand Down
10 changes: 5 additions & 5 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.c
View file
Expand Up @@ -579,7 +579,7 @@ void rotwing_state_skew_actuator_periodic(void)
#endif

#if USE_NPS
actuators_pprz[INDI_NUM_ACT] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command
stabilization_cmd[COMMAND_SKEW] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command
rotwing_state_skewing.wing_angle_deg = (float) rotwing_state_skewing.servo_pprz_cmd / MAX_PPRZ * 45. + 45.;

// SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback
Expand Down Expand Up @@ -613,13 +613,13 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
// Sanity check that index is valid
int idx = feedback_msg[i].idx;
if (feedback_msg[i].set.rpm) {
if (idx == SERVO_MOTOR_FRONT_IDX) {
if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) {
rotwing_state_hover_rpm[0] = feedback_msg->rpm;
} else if (idx == SERVO_MOTOR_RIGHT_IDX) {
} else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) {
rotwing_state_hover_rpm[1] = feedback_msg->rpm;
} else if (idx == SERVO_MOTOR_BACK_IDX) {
} else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) {
rotwing_state_hover_rpm[2] = feedback_msg->rpm;
} else if (idx == SERVO_MOTOR_LEFT_IDX) {
} else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) {
rotwing_state_hover_rpm[3] = feedback_msg->rpm;
}
}
Expand Down
28 changes: 23 additions & 5 deletions sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c
View file
Expand Up @@ -39,6 +39,11 @@
#include "modules/datalink/telemetry.h"
#endif

/* Enable ABI sending */
#ifndef AIRSPEED_MS45XX_SEND_ABI
#define AIRSPEED_MS45XX_SEND_ABI true
#endif

/** Default I2C device
*/
#ifndef MS45XX_I2C_DEV
Expand Down Expand Up @@ -261,6 +266,19 @@ void ms45xx_i2c_event(void)
*/

float p_out = (p_raw - ms45xx.pressure_offset) * ms45xx.pressure_scale;

/* 0 = -50degC, 20147 = 150degC
* ms45xx_temperature in 0.1 deg Celcius
*/
ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;

// if(electrical.vboard != 0) {
// float volt_diff = electrical.vboard - 5.0f;
// Bound(volt_diff, -0.7f, 0.5f);

// p_out -= 65.0f * volt_diff;
// ms45xx.temperature -= 8.87f * volt_diff;
// }
#ifdef USE_AIRSPEED_LOWPASS_FILTER
ms45xx.pressure = update_butterworth_2_low_pass(&ms45xx_filter, p_out);
#else
Expand All @@ -280,16 +298,16 @@ void ms45xx_i2c_event(void)
}
}

/* 0 = -50degC, 20147 = 150degC
* ms45xx_temperature in 0.1 deg Celcius
*/
ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;

// Send (differential) pressure via ABI
#if AIRSPEED_MS45XX_SEND_ABI
AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.pressure);

// Send temperature as float in deg Celcius via ABI
float temp = ms45xx.temperature / 10.0f;

AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp);
#endif

// Compute airspeed
float sign = 1.0f;
if (ms45xx.pressure < 0.0f) {
Expand Down
28 changes: 21 additions & 7 deletions sw/airborne/modules/sensors/airspeed_uavcan.c
View file
Expand Up @@ -32,6 +32,11 @@
#define AIRSPEED_UAVCAN_SEND_ABI true
#endif

/* Default pressure scaling */
#ifndef AIRSPEED_UAVCAN_DIFF_P_SCALE
#define AIRSPEED_UAVCAN_DIFF_P_SCALE 1.0f
#endif

/* Airspeed lowpass filter*/
#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
#include "filters/low_pass_filter.h"
Expand All @@ -52,14 +57,8 @@ static Butterworth2LowPass airspeed_filter;
#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE (0xC77DF38BA122F5DAULL)
#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE ((397 + 7)/8)

/* Local structure */
struct airspeed_uavcan_t {
float diff_p; ///< Differential pressure
float temperature; ///< Temperature in Celsius
};

/* Local variables */
static struct airspeed_uavcan_t airspeed_uavcan = {0};
struct airspeed_uavcan_t airspeed_uavcan = {0};
static uavcan_event airspeed_uavcan_ev;

#if PERIODIC_TELEMETRY
Expand Down Expand Up @@ -103,13 +102,19 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse
//float pitot_temp = canardConvertFloat16ToNativeFloat(tmp_float);

if(!isnan(diff_p)) {
// Remove the offset and apply a scaling factor
diff_p -= airspeed_uavcan.diff_p_offset;
diff_p *= airspeed_uavcan.diff_p_scale;

// Filtering
#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
float diff_p_filt = update_butterworth_2_low_pass(&airspeed_filter, diff_p);
airspeed_uavcan.diff_p = diff_p_filt;
#else
airspeed_uavcan.diff_p = diff_p;
#endif

// Send the ABI message
#if AIRSPEED_UAVCAN_SEND_ABI
AbiSendMsgBARO_DIFF(UAVCAN_SENDER_ID, airspeed_uavcan.diff_p);
#endif
Expand All @@ -125,6 +130,9 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse

void airspeed_uavcan_init(void)
{
// Set the default values
airspeed_uavcan.diff_p_scale = AIRSPEED_UAVCAN_DIFF_P_SCALE;

// Setup the low pass filter
#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
init_butterworth_2_low_pass(&airspeed_filter, AIRSPEED_UAVCAN_LOWPASS_TAU, AIRSPEED_UAVCAN_LOWPASS_PERIOD, 0);
Expand All @@ -137,3 +145,9 @@ void airspeed_uavcan_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink);
#endif
}

void airspeed_uavcan_autoset_offset(bool set) {
if(set) {
airspeed_uavcan.diff_p_offset = airspeed_uavcan.diff_p;
}
}
13 changes: 13 additions & 0 deletions sw/airborne/modules/sensors/airspeed_uavcan.h
View file
Expand Up @@ -26,7 +26,20 @@
#ifndef AIRSPEED_UAVCAN_H
#define AIRSPEED_UAVCAN_H

#include "std.h"

/* Airspeed UAVCAN structure */
struct airspeed_uavcan_t {
float diff_p; ///< Differential pressure
float temperature; ///< Temperature in Celsius

float diff_p_offset; ///< Differential pressure offset
float diff_p_scale; ///< Differential pressure scale
};
extern struct airspeed_uavcan_t airspeed_uavcan;

/* External functions */
extern void airspeed_uavcan_init(void);
extern void airspeed_uavcan_autoset_offset(bool set);

#endif /* AIRSPEED_UAVCAN_H */
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+2 −1 message_definitions/v1.0/messages.xml
2 changes: 1 addition & 1 deletion sw/simulator/nps/nps_autopilot.h
View file
Expand Up @@ -39,7 +39,7 @@ extern "C" {
#if defined MOTOR_MIXING_NB_MOTOR
#define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR
#else
#define NPS_COMMANDS_NB COMMANDS_NB
#define NPS_COMMANDS_NB ACTUATORS_NB // uses actuators_pprz[ACTUATORS_NB]
#endif /* #if defined MOTOR_MIXING_NB_MOTOR */
#endif /* #ifndef NPS_COMMANDS_NB */

Expand Down
4 changes: 2 additions & 2 deletions sw/simulator/nps/nps_autopilot_rotorcraft.c
View file
Expand Up @@ -169,8 +169,8 @@ void nps_autopilot_run_step(double time)
/* scale final motor commands to 0-1 for feeding the fdm */
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
#if NPS_NO_MOTOR_MIXING
actuators_pprz[i] = autopilot_get_motors_on() ? actuators_pprz[i] : 0;
nps_autopilot.commands[i] = (double)actuators_pprz[i] / MAX_PPRZ;
double sim_act = autopilot_get_motors_on() ? stabilization_cmd[i] : 0;
nps_autopilot.commands[i] = sim_act / MAX_PPRZ;
#else
nps_autopilot.commands[i] = (double)motor_mixing.commands[i] / MAX_PPRZ;
#endif
Expand Down
23 changes: 15 additions & 8 deletions sw/tools/generators/gen_airframe.ml
View file
Expand Up @@ -151,7 +151,7 @@ let print_reverse_servo_table = fun out driver servos ->
fprintf out " switch (_idx) {\n";
List.iter (fun c ->
let name = ExtXml.attrib c "name" in
fprintf out " case SERVO_%s: return SERVO_%s_MIN;\n" name name;
fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_MIN;\n" name name;
) servos;
fprintf out " default: return 0;\n";
fprintf out " };\n";
Expand All @@ -160,7 +160,16 @@ let print_reverse_servo_table = fun out driver servos ->
fprintf out " switch (_idx) {\n";
List.iter (fun c ->
let name = ExtXml.attrib c "name" in
fprintf out " case SERVO_%s: return SERVO_%s_MAX;\n" name name;
fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_MAX;\n" name name;
) servos;
fprintf out " default: return 0;\n";
fprintf out " };\n";
fprintf out "}\n\n";
fprintf out "static inline int get_servo_idx%s(int _idx) {\n" d;
fprintf out " switch (_idx) {\n";
List.iter (fun c ->
let name = ExtXml.attrib c "name" in
fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_IDX;\n" name name;
) servos;
fprintf out " default: return 0;\n";
fprintf out " };\n";
Expand All @@ -170,8 +179,10 @@ let parse_servo = fun out driver c ->
let shortname = ExtXml.attrib c "name" in
let name = "SERVO_"^shortname
and no_servo = int_of_string (ExtXml.attrib c "no") in
let global_idx = Hashtbl.length servos_drivers in

define_out out name (string_of_int no_servo);
define_out out (name^"_DRIVER_NO") (string_of_int no_servo);
define_out out (name^"_IDX") (string_of_int global_idx);

let s_min = fos (ExtXml.attrib c "min" )
and neutral = fos (ExtXml.attrib c "neutral")
Expand All @@ -194,7 +205,6 @@ let parse_servo = fun out driver c ->
fprintf out "\n";

(* Memorize the associated driver (if any) and global index (insertion order) *)
let global_idx = Hashtbl.length servos_drivers in
Hashtbl.add servos_drivers shortname (driver, global_idx)

(* Characters checked in Gen_radio.checl_function_name *)
Expand All @@ -207,11 +217,10 @@ let preprocess_value = fun s v prefix ->

let print_actuators_idx = fun out ->
Hashtbl.iter (fun s (d, i) ->
fprintf out "#define SERVO_%s_IDX %d\n" s i;
(* Set servo macro *)
fprintf out "#define Set_%s_Servo(_v) { \\\n" s;
fprintf out " actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
fprintf out " Actuator%sSet(SERVO_%s, actuators[SERVO_%s_IDX]); \\\n" d s s;
fprintf out " Actuator%sSet(SERVO_%s_DRIVER_NO, actuators[SERVO_%s_IDX]); \\\n" d s s;
fprintf out "}\n\n"
) servos_drivers;
define_out out "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers));
Expand Down Expand Up @@ -315,10 +324,8 @@ 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