Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 14:18:00 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[generator] add support for matrix and struct in airframe (#3228)

Browse Source 
* [generator] add support for matrix and struct in airframe

update ENAC airframe and code for indi G1 matrix and IMU calib

* update calibration tool output
This commit is contained in:
Gautier Hattenberger
2024-01-09 10:46:47 +01:00
committed by GitHub
parent 175e3e018c
commit 1b345804b4
19 changed files with 285 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/hybrid/cyfoam.xml
+12 -8
View File
@@ -162,10 +162,12 @@
<section name="CTRL_EFF_SCHEDULING" prefix="FWD_">
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ 0, 0, -13.0, 13.0}"/>
<define name="G1_PITCH" value="{-12.0, 12.0, 0, 0}"/>
<define name="G1_YAW" value="{-20.0, -20.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ 0, 0, -0.9, -0.9}"/>
<define name="G1" type="matrix">
<field name="ROLL" value="{ 0, 0, -13.0, 13.0}"/>
<field name="PITCH" value="{-12.0, 12.0, 0, 0}"/>
<field name="YAW" value="{-20.0, -20.0, 0.0, 0.0}"/>
<field name="THRUST" value="{ 0, 0, -0.9, -0.9}"/>
</define>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
@@ -180,10 +182,12 @@
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ 0, 0, -13.3, 13.3}"/>
<define name="G1_PITCH" value="{-2.1, 2.1, 0, 0}"/>
<define name="G1_YAW" value="{-2.0, -2.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ 0, 0, -1.1, -1.1}"/>
<define name="G1" type="matrix">
<field name="ROLL" value="{ 0, 0, -13.3, 13.3}"/>
<field name="PITCH" value="{-2.1, 2.1, 0, 0}"/>
<field name="YAW" value="{-2.0, -2.0, 0.0, 0.0}"/>
<field name="THRUST" value="{ 0, 0, -1.1, -1.1}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0}"/>
conf/airframes/ENAC/hybrid/falcon_v1.xml
+12 -8
View File
@@ -184,10 +184,12 @@
<section name="EFF_SCHEDULING" prefix="FWD_">
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ -9.0, -9.0, 9.0, 9.0}"/>
<define name="G1_PITCH" value="{ -9.0, 9.0, 9.0, -9.0}"/>
<define name="G1_YAW" value="{ -7.0, 7.0, -7.0, 7.0}"/>
<define name="G1_THRUST" value="{ -0.6, -0.6, -0.6, -0.6}"/>
<define name="G1" type="matrix">
<field value="{ -9.0, -9.0, 9.0, 9.0}"/>
<field value="{ -9.0, 9.0, 9.0, -9.0}"/>
<field value="{ -7.0, 7.0, -7.0, 7.0}"/>
<field value="{ -0.6, -0.6, -0.6, -0.6}"/>
</define>
<!--<define name="G1_ROLL" value="{ -10.0, -10.0, 10.0, 10.0}"/>
<define name="G1_PITCH" value="{ -10.0, 10.0, 10.0, -10.0}"/>
@@ -198,10 +200,12 @@
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- UR BR BL UL-->
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0}"/>
<define name="G1_PITCH" value="{ -19.5, 19.5, 19.5, -19.5}"/>
<define name="G1_YAW" value="{ -7.0, 7.0, -7.0, 7.0}"/>
<define name="G1_THRUST" value="{ -0.85, -0.85, -0.85, -0.85}"/>
<define name="G1" type="matrix">
<field value="{ -20.0, -20.0, 20.0, 20.0}"/>
<field value="{ -19.5, 19.5, 19.5, -19.5}"/>
<field value="{ -7.0, 7.0, -7.0, 7.0}"/>
<field value="{ -0.85, -0.85, -0.85, -0.85}"/>
</define>
<!-- Big Wings -->
<!-- <define name="G1_ROLL" value="{ -4.0, -4.0, 4.0, 4.0}"/>
conf/airframes/ENAC/hybrid/falcon_v2.xml
+12 -8
View File
@@ -208,19 +208,23 @@
<section name="EFF_SCHEDULING" prefix="FWD_">
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ -9.0, -9.0, 9.0, 9.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ -20.0, 20.0, 20.0, -20.0, 15.0, -15.0}"/> <!-- 12 , -12 -->
<define name="G1_YAW" value="{ -3.0, 3.0, -3.0, 3.0, -10.0, -10.0}"/> <!-- -20 -20 -->
<define name="G1_THRUST" value="{ -0.6, -0.6, -0.6, -0.6, 0.0, 0.0}"/>
<define name="G1" type="matrix">
<field value="{ -9.0, -9.0, 9.0, 9.0, 0.0, 0.0}"/>
<field value="{ -20.0, 20.0, 20.0, -20.0, 15.0, -15.0}"/> <!-- 12 , -12 -->
<field value="{ -3.0, 3.0, -3.0, 3.0, -10.0, -10.0}"/> <!-- -20 -20 -->
<field value="{ -0.6, -0.6, -0.6, -0.6, 0.0, 0.0}"/>
</define>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- UR BR BL UL-->
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ -9.0, -9.0, 9.0, 9.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ -19.5, 19.5, 19.5, -19.5, 0.0, 0.0}"/>
<define name="G1_YAW" value="{ -3.0, 3.0, -3.0, 3.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ -0.85, -0.85, -0.85, -0.85, 0.0, 0.0}"/>
<define name="G1" type="matrix">
<field value="{ -9.0, -9.0, 9.0, 9.0, 0.0, 0.0}"/>
<field value="{ -19.5, 19.5, 19.5, -19.5, 0.0, 0.0}"/>
<field value="{ -3.0, 3.0, -3.0, 3.0, 0.0, 0.0}"/>
<field value="{ -0.85, -0.85, -0.85, -0.85, 0.0, 0.0}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0, 0, 0}"/>
conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
+6 -4
View File
@@ -182,10 +182,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{-40 , -40, 40 , 40 }"/>
<define name="G1_PITCH" value="{40 , -40, -40 , 40 }"/>
<define name="G1_YAW" value="{5, -5, 5, -5}"/>
<define name="G1_THRUST" value="{-1.5, -1.5, -1.5, -1.5}"/>
<define name="G1" type="matrix">
<field value="{-40 , -40, 40 , 40 }"/>
<field value="{40 , -40, -40 , 40 }"/>
<field value="{5, -5, 5, -5}"/>
<field value="{-1.5, -1.5, -1.5, -1.5}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{150.0, -150.0, 150.0, -150.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/ENAC/quadrotor/cobra.xml
+17 -6
View File
@@ -102,8 +102,17 @@
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=9, .calibrated={.neutral=true, .scale=true},.neutral={-12,23,29}, .scale={{65369,62166,40813},{28737,25351,16753}}}}"/>
<!--define name="MAG_CALIB" value="{{.abi_id=3, .calibrated={.neutral=true, .scale=true},.neutral={-833,-5969,-931}, .scale={{22816,38799,15650},{34941,60353,24187}}}}"/-->
<define name="ACCEL_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="9"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
</field>
<field name="neutral" value="-12,23,29" type="int[]"/>
<field name="scale" value="{{65369,62166,40813},{28737,25351,16753}}"/>
</field>
</define>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -143,10 +152,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<define name="G1_PITCH" value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<define name="G1_YAW" value="{ 3, -3, 3, -3 }"/>
<define name="G1_THRUST" value="{ -1., -1., -1., -1.}"/>
<define name="G1" type="matrix">
<field value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<field value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<field value="{ 3., -3., 3., -3. }"/>
<field value="{ -1., -1., -1., -1.}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{80, -80, 80, -80.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/ENAC/quadrotor/cobraV2.xml
+44 -8
View File
@@ -98,10 +98,44 @@
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=24, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={-10,-47,-130}, .scale={{36821,8142,5741},{60003,13319,9358}}, TAWAKIV2_IMU_ROT}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=24, .calibrated={.neutral=false, .scale=false, .rotation=true}, TAWAKIV2_IMU_ROT}}"/>
<define name="MAG_CALIB" value="{{.abi_id=3, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={791,-190,2545}, .scale={{26437,35997,25098},{35181,44272,29767}}, TAWAKIV2_MAG_ROT}}"/>
<define name="ACCEL_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="24"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<field name="rotation" value="true"/>
</field>
<field name="neutral" value="-10,-47,-130" type="int[]"/>
<field name="scale" value="{{36821,8142,5741},{60003,13319,9358}}"/>
<field value="TAWAKIV2_IMU_ROT"/>
</field>
</define>
<define name="GYRO_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="24"/>
<field name="calibrated" type="struct">
<field name="neutral" value="false"/>
<field name="scale" value="false"/>
<field name="rotation" value="true"/>
</field>
<field value="TAWAKIV2_IMU_ROT"/>
</field>
</define>
<define name="MAG_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="3"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<field name="rotation" value="true"/>
</field>
<field name="neutral" value="791,-190,2545" type="int[]"/>
<field name="scale" value="{{26437,35997,25098},{35181,44272,29767}}"/>
<field value="TAWAKIV2_MAG_ROT"/>
</field>
</define>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="180." unit="deg"/>
@@ -140,10 +174,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<define name="G1_PITCH" value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<define name="G1_YAW" value="{ 3, -3, 3, -3 }"/>
<define name="G1_THRUST" value="{ -1., -1., -1., -1.}"/>
<define name="G1" type="matrix">
<field value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<field value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<field value="{ 3., -3., 3., -3. }"/>
<field value="{ -1., -1., -1., -1.}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{80, -80, 80, -80.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/ENAC/quadrotor/maya_indoor.xml
+20 -5
View File
@@ -94,7 +94,20 @@
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=9, .calibrated={.neutral=true, .scale=true},.neutral={18,45,-22}, .scale={{47720,3841,3732},{18911,1570,1517}}}}"/>
<define name="ACCEL_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="9"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
</field>
<field name="neutral" value="18,45,-22" type="int[]"/>
<field name="scale" type="matrix">
<field value="47720,3841,3732" type="int[]"/>
<field value="18911,1570,1517" type="int[]"/>
</field>
</field>
</define>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -134,10 +147,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<define name="G1_PITCH" value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<define name="G1_YAW" value="{ 3, -3, 3, -3 }"/>
<define name="G1_THRUST" value="{ -1., -1., -1., -1.}"/>
<define name="G1" type="matrix">
<field value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<field value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<field value="{ 3., -3., 3., -3. }"/>
<field value="{ -1., -1., -1., -1.}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{80, -80, 80, -80.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/ENAC/quadrotor/maya_outdoor.xml
+6 -4
View File
@@ -165,10 +165,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<define name="G1_PITCH" value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<define name="G1_YAW" value="{ 3, -3, 3, -3 }"/>
<define name="G1_THRUST" value="{ -1., -1., -1., -1.}"/>
<define name="G1" type="matrix">
<field value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<field value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<field value="{ 3, -3, 3, -3 }"/>
<field value="{ -1., -1., -1., -1.}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{80, -80, 80, -80.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/ENAC/quadrotor/robobee.xml
+6 -4
View File
@@ -195,10 +195,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<define name="G1_PITCH" value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<define name="G1_YAW" value="{ 3, -3, 3, -3 }"/>
<define name="G1_THRUST" value="{ -0.5, -0.5, -0.5, -0.5}"/>
<define name="G1" type="matrix">
<field value="{ -20.0, -20.0, 20.0, 20.0 }"/>
<field value="{ 20.0, -20.0, -20.0, 20.0 }"/>
<field value="{ 3, -3, 3, -3 }"/>
<field value="{ -0.5, -0.5, -0.5, -0.5}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{100, -100, 100, -100.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/ENAC/quadrotor/ulysse_indi.xml
+6 -4
View File
@@ -176,10 +176,12 @@
<!-- Full INDI -->
<!-- control effectiveness -->
<define name="G1_ROLL" value="{-40 , -40, 40 , 40 }"/>
<define name="G1_PITCH" value="{40 , -40, -40 , 40 }"/>
<define name="G1_YAW" value="{5, -5, 5, -5}"/>
<define name="G1_THRUST" value="{-1.5, -1.5, -1.5, -1.5}"/>
<define name="G1" type="matrix">
<field value="{-40 , -40, 40 , 40 }"/>
<field value="{40 , -40, -40 , 40 }"/>
<field value="{5, -5, 5, -5}"/>
<field value="{-1.5, -1.5, -1.5, -1.5}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{150.0, -150.0, 150.0, -150.0 }"/>
<!-- first order actuator dynamics -->
conf/airframes/airframe.dtd
+7 -1
View File
@@ -18,7 +18,8 @@
<!ELEMENT copy EMPTY>
<!ELEMENT curve EMPTY>
<!ELEMENT let EMPTY>
<!ELEMENT define EMPTY>
<!ELEMENT define (field)*>
<!ELEMENT field (field)*>
<!ELEMENT modules (module)*>
<!ELEMENT configure EMPTY>
<!ELEMENT firmware (target|module|autopilot|configure|define|comment)*>
@@ -105,6 +106,11 @@ integer CDATA #IMPLIED
type CDATA #IMPLIED
description CDATA #IMPLIED>
<!ATTLIST field
name CDATA #IMPLIED
value CDATA #IMPLIED
type CDATA #IMPLIED>
<!ATTLIST configure
name CDATA #REQUIRED
value CDATA #REQUIRED
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+4
View File
@@ -255,6 +255,9 @@ bool indi_thrust_increment_set = false;
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
#ifdef STABILIZATION_INDI_G1
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else // old defines TODO remove
#if INDI_OUTPUTS == 5
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
@@ -265,6 +268,7 @@ float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
};
#endif
#endif
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c
+8
View File
@@ -37,19 +37,27 @@
#error "You need to define an RC channel to switch between simple and advanced scheduling"
#endif
#ifdef FWD_G1
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = FWD_G1;
#else
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {
FWD_G1_ROLL,
FWD_G1_PITCH,
FWD_G1_YAW,
FWD_G1_THRUST
};
#endif
#ifdef STABILIZATION_INDI_G1
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH,
STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST
};
#endif
static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
sw/airborne/modules/ctrl/eff_scheduling_falcon.c
+4
View File
@@ -35,12 +35,16 @@
#define EFF_SCHEDULING_FALCON_LOW_AIRSPEED 8.0f
#endif
#ifdef STABILIZATION_INDI_G1
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH,
STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST
};
#endif
void eff_scheduling_falcon_init(void)
{
sw/airborne/modules/ctrl/eff_scheduling_generic.c
+8
View File
@@ -37,19 +37,27 @@
#error "You need to define an RC channel to switch between simple and advanced scheduling"
#endif
#ifdef FWD_G1
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = FWD_G1;
#else
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {
FWD_G1_ROLL,
FWD_G1_PITCH,
FWD_G1_YAW,
FWD_G1_THRUST
};
#endif
#ifdef STABILIZATION_INDI_G1
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH,
STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST
};
#endif
static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
+4
View File
@@ -72,7 +72,11 @@ float backwing_pitch_eff_scaling = 1.0;
static float g_forward[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL_FWD, STABILIZATION_INDI_G1_PITCH_FWD, STABILIZATION_INDI_G1_YAW_FWD, STABILIZATION_INDI_G1_THRUST_FWD};
#ifdef STABILIZATION_INDI_G1
static float g_hover[4][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
#else
static float g_hover[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST};
#endif
// Functions to schedule switching on and of of tip props on front wing
float sched_ratio_tip_props = 1.0;
sw/lib/ocaml/module.ml
+15 -2
View File
@@ -61,6 +61,12 @@ let parse_configure = fun xml ->
{ cname = find_name xml; cvalue = get "value"; case = get "case";
default = get "default"; cdescription = get "description" }
type field = {
finame: string option;
fivalue: string option;
fitype: string option
}
type define = {
dname: string;
dvalue: string option;
@@ -68,16 +74,23 @@ type define = {
dunit: string option;
dtype: string option;
ddescription: string option;
cond: string option
cond: string option;
fields: field list
}
let parse_field = fun xml ->
{ finame = ExtXml.attrib_opt xml "name";
fivalue = ExtXml.attrib_opt xml "value";
fitype = ExtXml.attrib_opt xml "type" }
let parse_define = fun xml ->
let get = fun x -> ExtXml.attrib_opt xml x in
{ dname = find_name xml; dvalue = get "value";
integer = begin match get "integer" with
| None -> None | Some i -> Some (int_of_string i) end;
dunit = get "unit"; dtype = get "type";
ddescription = get "description"; cond = get "cond" }
ddescription = get "description"; cond = get "cond";
fields = List.map parse_field (Xml.children xml) }
type incl = { element: string; condition: string option }
sw/tools/calibration/calibration_utils.py
+11 -7
View File
@@ -192,14 +192,18 @@ def print_xml(p, sensor, sensor_id, res):
y_sens = continious_frac(p[4]*2**res)
z_sens = continious_frac(p[5]*2**res)
struct = "{{.abi_id="+sensor_id+", .calibrated={.neutral=true, .scale=true},"
struct += ".neutral={"+str(int(round(p[0])))+","+str(int(round(p[1])))+","+str(int(round(p[2])))+"}, "
struct += ".scale={{"+str(x_sens[0])+","+str(y_sens[0])+","+str(z_sens[0])+"},"
struct += "{"+str(x_sens[1])+","+str(y_sens[1])+","+str(z_sens[1])+"}}"
struct += "}}"
print("")
print("<define name=\"IMU_"+sensor+"_CALIB\" value=\""+struct+"\"/>")
print('<define name="IMU_'+sensor+'_CALIB" type="array"/>')
print(' <field type="struct"/>')
print(' <field name="abi_id" value="'+sensor_id+'"/>')
print(' <field name="calibrated" type="struct">')
print(' <field name="neutral" value="true"/>')
print(' <field name="scale" value="true"/>')
print(' </field>')
print(' <field name="neutral" value="'+str(int(round(p[0])))+','+str(int(round(p[1])))+','+str(int(round(p[2])))+'" type="int[]"/>')
print(' <field name="scale" value="{{'+str(x_sens[0])+','+str(y_sens[0])+','+str(z_sens[0])+'},{'+str(x_sens[1])+','+str(y_sens[1])+','+str(z_sens[1])+'}}"/>')
print(' </field>')
print('</define>')
print("")
print("<define name=\""+sensor+"_X_NEUTRAL\" value=\""+str(int(round(p[0])))+"\"/>")
print("<define name=\""+sensor+"_Y_NEUTRAL\" value=\""+str(int(round(p[1])))+"\"/>")
sw/tools/generators/gen_airframe.ml
+83 -30
View File
@@ -94,51 +94,104 @@ let convert_value_with_code_unit_coef_of_xml = function xml ->
with _ -> prerr_endline (sprintf "Error: Unit conversion of parameter %s impossible because '%s' is not a float" (Xml.attrib xml "name") (Xml.attrib xml "value")); flush stderr; exit 1 in
v *. conv
type val_t =
| Value of string
| XmlFields of Xml.xml list
let get_value_converted = fun x ->
(* fail if units conversion is not found and just copy value instead,
this is important for integer values, you can't just multiply them with 1.0 *)
try string_of_float (convert_value_with_code_unit_coef_of_xml x)
with _ -> ExtXml.display_entities (ExtXml.attrib x "value")
let array_sep = Str.regexp "[,;]"
let rec string_from_type = fun name v t ->
let sprint_array = fun v t ->
let vs = Str.split array_sep v in
let sl = List.map (fun vl -> string_from_type name vl t) vs in
"{ "^(String.concat " , " sl)^" }"
in
let rec string_from_type = fun name t x ->
(* error function *)
let error_and_exit = fun e -> prerr_endline e; flush stderr; exit 1 in
(* utility function to remove spaces *)
let rm_leading_trailing_spaces = fun s ->
let s = Str.global_replace (Str.regexp "^ *") "" s in
Str.global_replace (Str.regexp " *$") "" s
in
match t with
| "float" ->
begin
(* test valid cases and extract value or fields *)
let v = try Some (Xml.attrib x "value") with _ -> None in
let c = if List.length (Xml.children x) = 0 then None else Some (Xml.children x) in
let _val = match v, c with
| None, None -> error_and_exit (sprintf "Define %s requires 'value' attribute or 'field' node" name)
| Some _, Some _ -> error_and_exit (sprintf "Define %s can't support both 'value' attribute or 'field' node" name)
| Some _v, None -> Value (get_value_converted x)
| None, Some _c -> XmlFields _c
in
(* extract values as string *)
let rec value_from_type = fun t _val ->
(* printers for array-like and structures *)
let sprint_array t = function
| Value v ->
let vs = Str.split array_sep v in
let sl = List.map (fun vl -> value_from_type t (Value vl)) vs in
"{ "^(String.concat " , " sl)^" }"
| XmlFields xfs ->
let sl = List.map (fun x -> value_from_type t (XmlFields (Xml.children x))) xfs in
"{ "^(String.concat " , " sl)^" }"
in
let sprint_matrix = fun l ->
let fl = List.map (fun f -> string_from_type name (ExtXml.attrib_or_default f "type" "") f) l in
"{ "^(String.concat " , " fl)^" }"
in
let sprint_struct = fun l ->
let fl = List.map (fun f ->
try
string_of_float (float_of_string (rm_leading_trailing_spaces v))
with _ -> prerr_endline (sprintf "Define value %s = %s is not compatible with type float" name v); flush stderr; exit 1
let n = ExtXml.attrib f "name" in
sprintf ".%s = %s" n (string_from_type name (ExtXml.attrib_or_default f "type" "") f)
with _ ->
sprintf "%s" (string_from_type name (ExtXml.attrib_or_default f "type" "") f)
) l in
"{ "^(String.concat " , " fl)^" }"
in
(* match correct types *)
match _val with
| XmlFields xfs -> begin
match t with
| "matrix" | "array" -> sprint_matrix (Xml.children x)
| "struct" -> sprint_struct (Xml.children x)
| _ -> error_and_exit (sprintf "Unknown define type for fields of %s (specify 'matrix', 'array' or 'struct')" name)
end
| "int" ->
begin
try
string_of_int (int_of_string (rm_leading_trailing_spaces v))
with _ -> prerr_endline (sprintf "Define value %s = %s is not compatible with type int" name v); flush stderr; exit 1
end
| "string" -> "\""^(rm_leading_trailing_spaces v)^"\""
| "array" -> sprint_array v ""
| "float[]" -> sprint_array v "float"
| "int[]" -> sprint_array v "int"
| "string[]" -> sprint_array v "string"
| _ -> v
| Value v -> begin
match t with
| "float" ->
begin
try
string_of_float (float_of_string (rm_leading_trailing_spaces v))
with _ -> error_and_exit (sprintf "Define value %s = %s is not compatible with type float" name v);
end
| "int" ->
begin
try
string_of_int (int_of_string (rm_leading_trailing_spaces v))
with _ -> error_and_exit (sprintf "Define value %s = %s is not compatible with type int" name v);
end
| "string" -> "\""^(rm_leading_trailing_spaces v)^"\""
| "array" -> sprint_array "" (Value v)
| "float[]" -> sprint_array "float" (Value v)
| "int[]" -> sprint_array "int" (Value v)
| "string[]" -> sprint_array "string" (Value v)
| "matrix" -> sprint_matrix (Xml.children x)
| "struct" -> sprint_struct (Xml.children x)
| _ -> v
end
in
value_from_type t _val
let parse_element = fun out prefix s ->
match Xml.tag s with
"define" -> begin
try
(* fail if units conversion is not found and just copy value instead,
this is important for integer values, you can't just multiply them with 1.0 *)
let value =
try string_of_float (convert_value_with_code_unit_coef_of_xml s)
with _ -> ExtXml.display_entities (ExtXml.attrib s "value")
in
let name = (prefix^ExtXml.attrib s "name") in
let t = ExtXml.attrib_or_default s "type" "" in
define_out out name (string_from_type name value t);
define_out out name (string_from_type name t s);
define_integer out name (ExtXml.float_attrib s "value") (ExtXml.int_attrib s "integer");
with _ -> ();
end