mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 06:59:54 +08:00
[modules] system identification update (#3075)
* [modules] system identification updates: auto-doublet-sequence, new doublets, RC-triggered chirps, testing extra sysid rename generic test compile * defines in a section instead of gcc, cleanup docs
This commit is contained in:
Christophe De Wagter
committed by
GitHub
GitHub
parent
536a9647a3
commit
f65b4ae84d
@@ -173,14 +173,10 @@
|
||||
</module>
|
||||
|
||||
<module name="motor_mixing"/>
|
||||
<!--module name="sys_id_doublet">
|
||||
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6,7}"/>
|
||||
<define name="DOUBLET_RADIO_CHANNEL" value="9"/>
|
||||
</module-->
|
||||
<!--<module name="sys_id_chirp">
|
||||
<define name="CHIRP_AXES" value="{0,1,2,3,4,5,6,7}"/>
|
||||
<define name="CHIRP_RADIO_CHANNEL" value="9"/>
|
||||
</module>-->
|
||||
|
||||
<module name="sys_id" type="doublet"/>
|
||||
<module name="sys_id" type="auto_doublets"/>
|
||||
<module name="sys_id" type="chirp"/>
|
||||
|
||||
</firmware>
|
||||
|
||||
@@ -241,8 +237,8 @@
|
||||
|
||||
<!-- Tip props always at 80% -->
|
||||
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*85*sched_ratio_tip_props);"/>
|
||||
<!--call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/-->
|
||||
<!--<call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>-->
|
||||
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
|
||||
<call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
|
||||
|
||||
<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
|
||||
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
|
||||
@@ -284,6 +280,19 @@
|
||||
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="SYS_ID" prefix="SYS_ID_">
|
||||
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6,7}"/>
|
||||
<define name="DOUBLET_RADIO_CHANNEL" value="9"/>
|
||||
|
||||
<define name="CHIRP_ENABLED" value="FALSE"/>
|
||||
<define name="CHIRP_AXES" value="{0,1,2,3,4,5,6,7}"/>
|
||||
<define name="CHIRP_RADIO_CHANNEL" value="9"/>
|
||||
|
||||
<define name="AUTO_DOUBLETS_N_ACTUATORS" value="2"/>
|
||||
<define name="AUTO_DOUBLETS_ACTUATORS" value="{0,1}"/>
|
||||
<define name="AUTO_DOUBLETS_AMPLITUDE" value="{5000,5000}"/>
|
||||
</section>
|
||||
|
||||
<section name="FORWARD">
|
||||
<!--The Nederdrone uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
|
||||
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="sys_id_auto_doublets" dir="system_identification">
|
||||
<doc>
|
||||
<description>Module that automatically runs a doublet sequence on a list of actuators</description>
|
||||
<section name="SYS_ID" prefix="SYS_ID_">
|
||||
<define name="AUTO_DOUBLETS_N_ACTUATORS" value="1" description="Number of doublets in the sequence"/>
|
||||
<define name="AUTO_DOUBLETS_ACTUATORS" value="{0}" description="List with SYS_ID_AUTO_DOUBLETS_N_ACTUATORS actuators"/>
|
||||
<define name="AUTO_DOUBLETS_AMPLITUDE" value="{5000}" description="List with SYS_ID_AUTO_DOUBLETS_N_ACTUATORS doublet amplitudes"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings NAME="AutoDoublet">
|
||||
<dl_settings name="auto_doublet">
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="activated" var="sys_id_auto_doublets_activated" type="uint8_t" module="system_identification/sys_id_auto_doublets" handler="on_activation"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="sys_id_auto_doublets.h"/>
|
||||
</header>
|
||||
<init fun="init_sys_id_auto_doublets()"/>
|
||||
<periodic fun="periodic_sys_id_auto_doublets()" freq="20.0"/>
|
||||
<makefile>
|
||||
<file name="sys_id_auto_doublets.c"/>
|
||||
<test>
|
||||
<define name="SYS_ID_AUTO_DOUBLETS_N_ACTUATORS" value="1" />
|
||||
<define name="SYS_ID_AUTO_DOUBLETS_ACTUATORS" value="{0}" />
|
||||
<define name="SYS_ID_AUTO_DOUBLETS_AMPLITUDE" value="{5000}" />
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -29,21 +29,22 @@
|
||||
Add the message "CHIRP" to your telemetry to see chirp progress, and to your logger to automatically filter chirps in post-processing.
|
||||
|
||||
</description>
|
||||
<define name="CHIRP_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}" description="Which axes the chirp is applied to (specify as array with {})"/>
|
||||
<!-- <define name="CHIRP_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the chirp is applied to (specify as array with {})"/> -->
|
||||
<define name="CHIRP_ENABLED" value="TRUE|FALSE" description="If false, the chirp does not run and values are not added"/>
|
||||
<define name="CHIRP_USE_NOISE" value="TRUE|FALSE" description="If true, add noise to all axes (also the axes where no chirp is active)"/>
|
||||
<define name="CHIRP_EXPONENTIAL" value="TRUE|FALSE" description="If true, exponential-time chirp. Else, linear-time chirp"/>
|
||||
<define name="CHIRP_FADEIN" value="TRUE|FALSE" description="If true, start the chirp with two wavelengths of the lowest frequency"/>
|
||||
<section name="SYS_ID" prefix="SYS_ID_">
|
||||
<define name="CHIRP_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,COMMAND_THRUST}" description="Which axes the chirp is applied to (specify as array with {})"/>
|
||||
<define name="CHIRP_ENABLED" value="TRUE|FALSE" description="If false, the chirp does not run and values are not added"/>
|
||||
<define name="CHIRP_USE_NOISE" value="TRUE|FALSE" description="If true, add noise to all axes (also the axes where no chirp is active)"/>
|
||||
<define name="CHIRP_EXPONENTIAL" value="TRUE|FALSE" description="If true, exponential-time chirp. Else, linear-time chirp"/>
|
||||
<define name="CHIRP_FADEIN" value="TRUE|FALSE" description="If true, start the chirp with two wavelengths of the lowest frequency"/>
|
||||
</section>
|
||||
</doc>
|
||||
|
||||
<settings>
|
||||
<dl_settings name="System identification">
|
||||
<dl_settings name="Chirp input">
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Chirp" var="chirp_active" type="uint8_t" module="system_identification/sys_id_chirp" handler="activate_handler"/>
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Fade in" var="chirp_fade_in" type="uint8_t" module="system_identification/sys_id_chirp" handler="fade_in_activate_handler"/>
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Exponential" var="chirp_exponential" type="uint8_t" module="system_identification/sys_id_chirp" handler="exponential_activate_handler"/>
|
||||
<dl_setting min="0" max="5" step="1" shortname="Chirp axis" var="chirp_axis" type="uint8_t" module="system_identification/sys_id_chirp" handler="axis_handler"/>
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Fade in" var="chirp_fade_in" type="uint8_t" module="system_identification/sys_id_chirp" handler="fade_in_activate_handler" param="SYS_ID_CHIRP_FADEIN"/>
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Exponential" var="chirp_exponential" type="uint8_t" module="system_identification/sys_id_chirp" handler="exponential_activate_handler" param="SYS_ID_CHIRP_EXPONENTIAL"/>
|
||||
<dl_setting min="0" max="7" step="1" shortname="Chirp axis" var="chirp_axis" type="uint8_t" module="system_identification/sys_id_chirp" handler="axis_handler"/>
|
||||
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="chirp_amplitude" type="int32_t" module="system_identification/sys_id_chirp"/>
|
||||
<dl_setting min="0" max="0.5" step="0.01" shortname="on-axis noise" var="chirp_noise_stdv_onaxis_ratio" type="float" module="system_identification/sys_id_chirp"/>
|
||||
<dl_setting min="0" max="9600" step="50" shortname="off-axis noise" var="chirp_noise_stdv_offaxis" type="float" module="system_identification/sys_id_chirp"/>
|
||||
|
||||
@@ -10,47 +10,34 @@
|
||||
|
||||
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
|
||||
You can pick the axes to apply doublets to by setting the DOUBLET_AXES variable with the COMMAND_XXX variables where XXX are the actuators defined in
|
||||
You can pick the axes to apply doublets to by setting the SYS_ID_DOUBLET_AXES variable with the COMMAND_XXX variables where XXX are the actuators defined in
|
||||
the <commands> section of your airframe.
|
||||
|
||||
Remeber to deactivate the control input when the system identification input is running using (can be checked using the sys_id_wave_running() function)
|
||||
|
||||
Then, the GCS exposes the settings for the doublet.
|
||||
- 3-2-1-1 setting is to execute the 3-2-1-1 doublet maneuver when active. Otherwise, the normal 1-1 doublet maneuver will be executed.
|
||||
- The Doublet axis settings is the index (0-based) of the axis to choose within the DOUBLET_AXES variable specified. In the default, this means i.e. 0 means roll doublet.
|
||||
- The Doublet axis settings is the index (0-based) of the axis to choose within the SYS_ID_DOUBLET_AXES variable specified. In the default, this means i.e. 0 means roll doublet.
|
||||
- Amplitude is the amplitude of the doublet
|
||||
- Length_s is the length in seconds of the doublet. The lenght of each doublet step depends on the active mode and on the doublet lenght.
|
||||
- Extra_waiting_time_s is the length in second of the waiting time with zero input after the doublet maneuver is over.
|
||||
|
||||
Start the doublet by checking the Active box in the "Activate doublet". Stop the doublet by checking the Inactive box in the same input setting.
|
||||
|
||||
Add the message "DOUBLET" to your telemetry to see doublet progress, and to your logger to obtain information about the provided input.
|
||||
|
||||
Add the following message structure to the messages.xml file on a free ID
|
||||
|
||||
<!--
|
||||
<message name="DOUBLET" id="181">
|
||||
<field name="active" type="uint8" unit="bool">Doublet is active</field>
|
||||
<field name="axis" type="uint8">Current doublet axis</field>
|
||||
<field name="amplitude" type="int16">Amplitude</field>
|
||||
<field name="current_value" type="int16">Current doublet value</field>
|
||||
<field name="mode_3211_active" type="uint8">Doublet 3211 mode active</field>
|
||||
</message>
|
||||
-->
|
||||
|
||||
Start the doublet by checking the Active box in the "Activate doublet". Stop the doublet by checking the Inactive box in the same input setting.
|
||||
</description>
|
||||
<define name="DOUBLET_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the doublet is applied to (specify as array with {})"/>
|
||||
<define name="DOUBLET_ENABLED" value="TRUE|FALSE" description="If false, the doublet does not run and values are not added"/>
|
||||
<define name="DOUBLET_MOD3211" value="TRUE|FALSE" description="If true, the 3-2-1-1 doublet will be executed instead of the normal doublet"/>
|
||||
</doc>
|
||||
<section name="SYS_ID" prefix="SYS_ID_">
|
||||
<define name="DOUBLET_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the doublet is applied to (specify as array with {})"/>
|
||||
<define name="DOUBLET_ENABLED" value="TRUE|FALSE" description="If false, the doublet does not run and values are not added"/>
|
||||
<define name="DOUBLET_MOD" value="0" description="0= normal, 1=half, 2=the 3-2-1-1 doublet"/>
|
||||
</section>
|
||||
</doc>
|
||||
|
||||
<settings>
|
||||
<dl_settings name="System identification">
|
||||
<dl_settings name="Doublet input">
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Doublet" var="doublet_active" type="uint8_t" module="system_identification/sys_id_doublet" handler="activate_handler"/>
|
||||
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="3-2-1-1 mode" var="doublet_mode_3211" type="uint8_t" module="system_identification/sys_id_doublet" handler="mod3211_handler"/>
|
||||
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="doublet_amplitude" type="int32_t" module="system_identification/sys_id_doublet"/>
|
||||
<dl_setting min="0" max="5" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
|
||||
<dl_setting min="0" max="2" step="1" values="Normal|Half|3211" shortname="mode" var="doublet_mode" type="uint8_t" module="system_identification/sys_id_doublet" handler="mod_handler" param="SYS_ID_DOUBLET_MOD"/>
|
||||
<dl_setting min="-9600" max="9600" step="100" shortname="Amplitude" var="doublet_amplitude" type="int32_t" module="system_identification/sys_id_doublet"/>
|
||||
<dl_setting min="0" max="7" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
|
||||
<dl_setting min="0" max="100" step="0.5" shortname="Length_s" var="doublet_length_s" type="float" module="system_identification/sys_id_doublet"/>
|
||||
<dl_setting min="0" max="100" step="0.5" shortname="Extra_waiting_s" var="doublet_extra_waiting_time_s" type="float" module="system_identification/sys_id_doublet"/>
|
||||
</dl_settings>
|
||||
|
||||
@@ -282,7 +282,7 @@
|
||||
telemetry="telemetry/highspeed_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/nederdrone_cyberzoo.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
|
||||
settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
|
||||
@@ -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){
|
||||
@@ -74,7 +78,23 @@ 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 (doublet->mod == 0) {
|
||||
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 == 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))){
|
||||
@@ -83,16 +103,7 @@ float doublet_update(struct doublet_t *doublet, float current_time_s){
|
||||
doublet->current_value = 0.0f;
|
||||
}
|
||||
}
|
||||
else{
|
||||
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{
|
||||
} else {
|
||||
doublet->current_value = 0.0f;
|
||||
}
|
||||
return doublet->current_value;
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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);
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
|
||||
@@ -192,7 +204,7 @@ 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;
|
||||
}
|
||||
@@ -200,7 +212,29 @@ void sys_id_chirp_init(void)
|
||||
|
||||
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())) {
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -40,33 +40,37 @@
|
||||
#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)
|
||||
{
|
||||
@@ -74,7 +78,7 @@ static void set_current_doublet_values(void)
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -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,
|
||||
¤t_doublet_values[doublet_axis], &doublet_mode_3211);
|
||||
¤t_doublet_values[doublet_axis], &doublet_mode);
|
||||
}
|
||||
|
||||
static void start_doublet(void)
|
||||
@@ -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();
|
||||
@@ -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();
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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[]);
|
||||
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: 9208dc8417...5b3970d34e
Reference in New Issue
Block a user