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
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+87 −14 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
2 changes: 1 addition & 1 deletion 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 "\nInfo: Geofence max altitude set to %.0f\n" geofence_max_alt;
with
_ -> ()
end;
Expand Down