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

update INDI tu use full control effecitvens matrix (#1916)

Browse Source 
* update INDI tu use full control effecitvens matrix

Instead of using commands (roll, pitch, yaw, thrust), give the
controller direct control over all actuators.
This allows for online individual actuator control effectiveness
estimation and inversion, as well as better saturation handling.
The INDI controller as it was previous is renamed to 'simple'.

* Made INDI more flexible wrt number of actuators

- 4x4 matrix inverse to accomodate the thrust axis as well
- support for more actuators
- cleanup of code

* Change filters guidance indi to butterworth

* Added body acceleration to state interface

* Better adaptation and guidance thrust increment

- Adaptation flexible wrt. number of actuators.
- Added adaptation of the specific force effectiveness of the actuators.
- Added connection to guidance INDI through ABI. The thrust increment is
  caclulated in guidance INDI and sent to the stabilization.

* lower case variables

* change filter of indi to butterworth

* INDI fix array index & no declarations in for loop

- Also added doxygen comments

* Added actuators_pprz

the array actuators is used to drive the servos, so a new array to store
the command to the actuators in pprz scale is needed

* Removed hardcoded scaling
This commit is contained in:
Ewoud Smeur
2016-12-01 19:02:36 +01:00
committed by Michal Podhradsky
parent 6026187fa1
commit 6dcabb3a57
68 changed files with 1621 additions and 573 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+6 -1
View File
@@ -85,7 +85,12 @@
</message>
<message name="RPM" id="15">
<field name="rpm" type="uint16_t" unit="rpm"/>
<field name="rpm" type="uint16_t *" unit="rpm"/>
<field name="num_act" type="uint8_t"/>
</message>
<message name="THRUST" id="16">
<field name="thrust_increment" type="float" unit="m/s^2"/>
</message>
</msg_class>
conf/airframes/BR/bebop_indi.xml
+3 -5
View File
@@ -19,7 +19,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="float_mlkf"/>
<module name="ahrs" type="int_cmpl_quat">
@@ -171,10 +171,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/BR/bebop_indi_frog.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="float_mlkf"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
@@ -172,10 +172,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/BR/bebop_indi_frog_flip.xml
+3 -5
View File
@@ -19,7 +19,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="float_mlkf"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
@@ -165,10 +165,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/BR/ladybird_kit_indi_bart.xml
+3 -5
View File
@@ -139,10 +139,8 @@
<define name="REF_RATE_R" value="17.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
@@ -230,7 +228,7 @@
<define name="LISA_S_UPSIDE_DOWN"/> <!-- Upside down is a relative term. :) -->
</module>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
conf/airframes/CDW/cdw_bebop.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -170,10 +170,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/KS/ks_mavtec1.xml
+3 -5
View File
@@ -29,7 +29,7 @@
<module name="telemetry" type="xbee_api"/>
<module name="imu" type="aspirin_v2.2"/>
<module name="gps" type="optitrack"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</module>
@@ -182,10 +182,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
conf/airframes/OPENUAS/openuas_ardrone2.xml
+3 -5
View File
@@ -36,7 +36,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -190,10 +190,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/OPENUAS/openuas_leapfrogeye.xml
+3 -5
View File
@@ -19,7 +19,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -177,10 +177,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/OPENUAS/openuas_psi.xml
+3 -5
View File
@@ -51,7 +51,7 @@
<!-- </module> -->
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -187,10 +187,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_ardrone2_flip.xml
+3 -5
View File
@@ -20,7 +20,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -155,10 +155,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_ardrone2_indi.xml
+3 -5
View File
@@ -20,7 +20,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -150,10 +150,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml
+2 -4
View File
@@ -117,10 +117,8 @@
<define name="REF_RATE_R" value="11.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_ardrone2_optitrack.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" type="extended"/>
</firmware>
@@ -123,10 +123,8 @@
<define name="REF_RATE_R" value="11.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_bebop2_indi.xml
+3 -5
View File
@@ -17,7 +17,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -142,10 +142,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_bebop2_indi_MAVlink.xml
+3 -5
View File
@@ -17,7 +17,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -147,10 +147,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_bebop2_no_damping.xml
+3 -5
View File
@@ -16,7 +16,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -147,10 +147,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.7"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_bebop_flip.xml
+3 -5
View File
@@ -18,7 +18,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="float_mlkf"/>
<module name="ahrs" type="int_cmpl_quat">
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -170,10 +170,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_bebop_indi.xml
+3 -5
View File
@@ -19,7 +19,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
@@ -148,10 +148,8 @@
<define name="MAX_R" value="120.0" unit="deg/s"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_bebop_indi_actuators.xml
+204
View File
@@ -0,0 +1,204 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop_indi">
<firmware name="rotorcraft">
<target name="ap" board="bebop"/>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
<module name="udp"/>
</target>
<!--define name="USE_SONAR" value="TRUE"/-->
<!-- Subsystem section -->
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi">
<define name="INDI_RPM_FEEDBACK" value="TRUE"/>
</module>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
</module>
<module name="ins" type="extended"/>
<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="TRUE"/>
</module>
</firmware>
<modules main_freq="512">
<module name="geo_mag"/>
<module name="air_data"/>
<module name="send_imu_mag_current"/>
<!--module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module-->
</modules>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="6000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="9800"/>
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="9800"/>
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="9800"/>
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="9800"/>
</servos>
<command_laws>
<set servo="TOP_LEFT" value="autopilot_motors_on ? actuators_pprz[0] : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_motors_on ? actuators_pprz[1] : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_motors_on ? actuators_pprz[2] : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_motors_on ? actuators_pprz[3] : -MAX_PPRZ"/>
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>
<include href="conf/airframes/TUDELFT/calibrations/bebop7.xml" />
<section name="IMU" prefix="IMU_">
<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="0." unit="deg"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
<!-- For vibrating airfames -->
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="120" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }"/>
<define name="G1_PITCH" value="{14 , 14, -14 , -14 }"/>
<define name="G1_YAW" value="{-1, 1, -1, 1}"/>
<define name="G1_THRUST" value="{-.4, -.4, -.4, -.4}"/>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{-61.2093, 65.3670, -65.7419, 65.4516}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="28.0"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="120.0" unit="deg/s"/>
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="0.1"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="2.5"/>
<define name="DESCEND_VSPEED" value="-1.0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/TUDELFT/tudelft_bebop_mavlink.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -177,10 +177,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
+3 -5
View File
@@ -19,7 +19,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/> <!-- Optitrack -->
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -160,10 +160,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_bebop_optitrack.xml
+3 -5
View File
@@ -17,7 +17,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
@@ -148,10 +148,8 @@
<define name="MAX_R" value="120.0" unit="deg/s"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_conf.xml
+11
View File
@@ -98,6 +98,17 @@
settings_modules="modules/imu_common.xml modules/gps.xml modules/ahrs_int_cmpl_quat.xml modules/geo_mag.xml modules/air_data.xml modules/cv_blob_locator.xml [modules/nav_survey_rectangle_rotorcraft.xml] [modules/nav_survey_poly_rotorcraft.xml] modules/video_capture.xml modules/digital_cam_video.xml"
gui_color="#ffff0689b7a1"
/>
<aircraft
name="Bebop_actuators"
ac_id="24"
airframe="airframes/TUDELFT/tudelft_bebop_indi_actuators.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/imu_common.xml modules/gps.xml modules/ahrs_int_cmpl_quat.xml [modules/geo_mag.xml] modules/air_data.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
name="Bebop_default"
ac_id="20"
conf/airframes/TUDELFT/tudelft_course2016_bebop_avoider.xml
+3 -5
View File
@@ -15,7 +15,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="optitrack"/> <!-- The GPS information also comes over this wifi, as we are sitting in the cyberzoo all the time -->
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/> <!-- Heading is from Optitrack, in reguler outside flight case one would use and Magnetometer-->
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
@@ -173,10 +173,8 @@
<define name="MAX_R" value="120.0"/> <!--deg/s-->
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_course2016_bebop_colorfilter.xml
+3 -5
View File
@@ -13,7 +13,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="optitrack"/> <!-- The GPS information also comes over this wifi, as we are sitting in the cyberzoo all the time -->
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
@@ -172,10 +172,8 @@
<define name="MAX_R" value="120.0"/> <!--deg/s-->
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/airframes/TUDELFT/tudelft_guido_ardrone2_optitrack.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" type="extended"/>
</firmware>
@@ -129,10 +129,8 @@
<define name="REF_RATE_R" value="11.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_iris_indi.xml
+3 -5
View File
@@ -25,7 +25,7 @@
<module name="imu" type="px4fmu_v2.4"/>
<module name="gps" type="ublox" />
<module name="gps" type="ubx_ucenter" />
<module name="stabilization" type="indi" />
<module name="stabilization" type="indi_simple" />
<module name="ahrs" type="int_cmpl_quat" >
<define name="AHRS_ICQ_IMU_ID" value="IMU_PX4_ID" /> <!-- Meaning the lsm303 and l3g -->
<define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID" /> <!-- Meaning the external hmc-->
@@ -218,10 +218,8 @@
<define name="REF_RATE_Q" value="14.0" />
<define name="REF_RATE_R" value="14.0" />
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0" />
<define name="FILT_ZETA" value="0.55" />
<define name="FILT_OMEGA_R" value="20.0" />
<define name="FILT_ZETA_R" value="0.55" />
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.04" />
<define name="ACT_DYN_Q" value="0.04" />
conf/airframes/TUDELFT/tudelft_mavshot.xml
+3 -5
View File
@@ -193,10 +193,8 @@
<define name="REF_RATE_R" value="12.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.15"/>
@@ -280,7 +278,7 @@
<!-- <module name="telemetry" type="transparent"/> -->
<module name="imu" type="aspirin_v2.2"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="int_cmpl_quat">
conf/airframes/TUDELFT/tudelft_mavtec1.xml
+3 -5
View File
@@ -27,7 +27,7 @@
<module name="telemetry" type="xbee_api"/>
<module name="imu" type="aspirin_v2.2"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</module>
@@ -182,10 +182,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
conf/airframes/TUDELFT/tudelft_mavtec5.xml
+3 -5
View File
@@ -29,7 +29,7 @@
<module name="gps" type="ublox">
<define name="USE_PIKSI_EXT_ANTENNA" value="TRUE"/>
</module>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" type="hff"/>
</firmware>
@@ -170,10 +170,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.15"/>
conf/airframes/TUDELFT/tudelft_rm_ardrone2.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="imu" type="ardrone2"/>
<!-- gps: "ublox" or change to "sirf" for usage with parrot flight recorder -->
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -157,10 +157,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/TUDELFT/tudelft_selfie.xml
+2 -4
View File
@@ -118,10 +118,8 @@
<define name="REF_RATE_R" value="11.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/examples/ardrone2_px4flow_hover.xml
+3 -5
View File
@@ -22,7 +22,7 @@
<module name="imu" type="ardrone2"/>
<!-- gps: "ublox" or change to "sirf" for usage with parrot flight recorder -->
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -159,10 +159,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/examples/bebop2_indi.xml
+3 -5
View File
@@ -21,7 +21,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -153,10 +153,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/examples/quadrotor_navstik.xml
+3 -5
View File
@@ -29,7 +29,7 @@
<module name="telemetry" type="xbee_api"/>
<module name="imu" type="navstik"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
</module>
@@ -128,10 +128,8 @@
<define name="REF_RATE_R" value="25.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.2"/>
conf/airframes/examples/quadshot_178_pylons.xml
+3 -5
View File
@@ -220,10 +220,8 @@
<define name="REF_RATE_Q" value="12.0"/>
<define name="REF_RATE_R" value="12.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.37"/>
<define name="FILT_OMEGA_R" value="40.0"/>
<define name="FILT_ZETA_R" value="0.5"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="6.4"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
@@ -355,7 +353,7 @@
<module name="imu" type="aspirin_v2.1"/>
<module name="gps" type="ublox"/>
<!-- <module name="stabilization" type="int_quat"/> -->
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="1"/>
conf/airframes/examples/quadshot_asp21_spektrum.xml
+3 -5
View File
@@ -230,10 +230,8 @@
<define name="REF_RATE_Q" value="16.0"/>
<define name="REF_RATE_R" value="12.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
@@ -371,7 +369,7 @@
<module name="imu" type="aspirin_v2.1"/>
<module name="gps" type="ublox"/>
<!-- <module name="stabilization" type="int_quat"/> -->
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="guidance" type="hybrid"/>
<module name="ahrs" type="int_cmpl_quat">
conf/airframes/untested/ardrone2_indi.xml
+3 -5
View File
@@ -20,7 +20,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -150,10 +150,8 @@
<define name="REF_RATE_R" value="21.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/untested/ardrone2_optitrack.xml
+3 -5
View File
@@ -21,7 +21,7 @@
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" type="extended"/>
</firmware>
@@ -121,10 +121,8 @@
<define name="REF_RATE_R" value="11.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
conf/airframes/untested/bebop_indi.xml
+3 -5
View File
@@ -18,7 +18,7 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="furuno"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="float_mlkf"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
@@ -155,10 +155,8 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_R" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
conf/firmwares/subsystems/rotorcraft/stabilization_indi_simple.makefile
+15
View File
@@ -0,0 +1,15 @@
STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT
STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_indi.h\"
STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_INDI_SIMPLE=true
STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_indi_simple.c
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_indi.c
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
ap.CFLAGS += $(STAB_ATT_CFLAGS)
ap.srcs += $(STAB_ATT_SRCS)
nps.CFLAGS += $(STAB_ATT_CFLAGS)
nps.srcs += $(STAB_ATT_SRCS)
conf/firmwares/subsystems/rotorcraft/stabilization_rate_indi.makefile
+1
View File
@@ -1,6 +1,7 @@
STAB_RATE_CFLAGS = -DUSE_STABILIZATION_RATE
STAB_RATE_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_indi.c
STAB_RATE_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_rate_indi.c
STAB_RATE_CFLAGS += -DSTABILIZATION_RATE_INDI=true
ap.CFLAGS += $(STAB_RATE_CFLAGS)
ap.srcs += $(STAB_RATE_SRCS)
conf/settings/control/stabilization_indi.xml
+13 -13
View File
@@ -4,19 +4,19 @@
<dl_settings>
<dl_settings NAME="indi">
<dl_setting var="indi.reference_acceleration.err_p" min="0" step="1" max="2500" module="stabilization/stabilization_indi" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_p" min="0" step="0.1" max="100" module="stabilization/stabilization_indi" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.p" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_p" param="STABILIZATION_INDI_G1_P" persistent="true"/>
<dl_setting var="indi.reference_acceleration.err_q" min="0" step="1" max="2500" module="stabilization/stabilization_indi" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_q" min="0" step="0.1" max="100" module="stabilization/stabilization_indi" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.q" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_q" param="STABILIZATION_INDI_G1_Q" persistent="true"/>
<dl_setting var="indi.reference_acceleration.err_r" min="0" step="1" max="2500" module="stabilization/stabilization_indi" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_r" min="0" step="0.1" max="100" module="stabilization/stabilization_indi" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
<dl_setting var="indi.g2" min="0" step="0.01" max="10" module="stabilization/stabilization_indi" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
<dl_setting var="indi.adaptive" min="0" step="1" max="1" module="stabilization/stabilization_indi" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
<dl_setting var="indi.max_rate" min="0" step="0.01" max="400.0" module="stabilization/stabilization_indi" shortname="max_rate" param="STABILIZATION_INDI_MAX_RATE" unit="rad/s" alt_unit="deg/s"/>
<dl_setting var="indi.attitude_max_yaw_rate" min="0" step="0.01" max="400.0" module="stabilization/stabilization_indi" shortname="max_yaw_rate_attitude" param="STABILIZATION_INDI_MAX_R" unit="rad/s" alt_unit="deg/s"/>
<dl_setting var="indi.reference_acceleration.err_p" min="0" step="1" max="2500" module="stabilization/stabilization_indi_simple" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_p" min="0" step="0.1" max="100" module="stabilization/stabilization_indi_simple" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.p" min="0" step="0.001" max="10" module="stabilization/stabilization_indi_simple" shortname="ctl_eff_p" param="STABILIZATION_INDI_G1_P" persistent="true"/>
<dl_setting var="indi.reference_acceleration.err_q" min="0" step="1" max="2500" module="stabilization/stabilization_indi_simple" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_q" min="0" step="0.1" max="100" module="stabilization/stabilization_indi_simple" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.q" min="0" step="0.001" max="10" module="stabilization/stabilization_indi_simple" shortname="ctl_eff_q" param="STABILIZATION_INDI_G1_Q" persistent="true"/>
<dl_setting var="indi.reference_acceleration.err_r" min="0" step="1" max="2500" module="stabilization/stabilization_indi_simple" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R" persistent="true"/>
<dl_setting var="indi.reference_acceleration.rate_r" min="0" step="0.1" max="100" module="stabilization/stabilization_indi_simple" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
<dl_setting var="indi.g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_indi_simple" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
<dl_setting var="indi.g2" min="0" step="0.01" max="10" module="stabilization/stabilization_indi_simple" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
<dl_setting var="indi.adaptive" min="0" step="1" max="1" module="stabilization/stabilization_indi_simple" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
<dl_setting var="indi.max_rate" min="0" step="0.01" max="400.0" module="stabilization/stabilization_indi_simple" shortname="max_rate" param="STABILIZATION_INDI_MAX_RATE" unit="rad/s" alt_unit="deg/s"/>
<dl_setting var="indi.attitude_max_yaw_rate" min="0" step="0.01" max="400.0" module="stabilization/stabilization_indi_simple" shortname="max_yaw_rate_attitude" param="STABILIZATION_INDI_MAX_R" unit="rad/s" alt_unit="deg/s"/>
</dl_settings>
</dl_settings>
conf/telemetry/default_rotorcraft.xml
+4 -3
View File
@@ -93,11 +93,12 @@
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
<message name="INDI_G" period="2.0"/>
</mode>
<mode name="vert_loop" key_press="v">
sw/airborne/boards/bebop/actuators.c
+3 -1
View File
@@ -26,11 +26,11 @@
*/
#include "subsystems/actuators.h"
#include "subsystems/actuators/motor_mixing.h"
#include "subsystems/electrical.h"
#include "actuators.h"
#include "led_hw.h"
#include "autopilot.h"
#include "subsystems/abi.h"
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -137,6 +137,8 @@ void actuators_bebop_commit(void)
actuators_bebop.led = led_hw_values & 0x3;
}
// Send ABI message
AbiSendMsgRPM(RPM_SENSOR_ID, actuators_bebop.rpm_obs, 4);
}
static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size)
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+48 -73
View File
@@ -42,6 +42,8 @@
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "stdio.h"
#include "filters/low_pass_filter.h"
#include "subsystems/abi.h"
// The acceleration reference is calculated with these gains. If you use GPS,
// they are probably limited by the update rate of your GPS. The default
@@ -73,56 +75,47 @@ float thrust_in_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
#endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
#ifndef GUIDANCE_INDI_FILTER_CUTOFF
#ifdef STABILIZATION_INDI_FILT_CUTOFF
#define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
#endif
#endif
struct FloatVect3 filt_accel_ned;
struct FloatVect3 filt_accel_ned_d;
struct FloatVect3 filt_accel_ned_dd;
float filt_accelzbody = 0;
float filt_accelzbodyd = 0;
float filt_accelzbodydd = 0;
float roll_filt = 0;
float roll_filtd = 0;
float roll_filtdd = 0;
float pitch_filt = 0;
float pitch_filtd = 0;
float pitch_filtdd = 0;
float thrust_act = 0;
float thrust_filt = 0;
float thrust_filtd = 0;
float thrust_filtdd = 0;
Butterworth2LowPass filt_accel_ned[3];
Butterworth2LowPass roll_filt;
Butterworth2LowPass pitch_filt;
Butterworth2LowPass thrust_filt;
struct FloatMat33 Ga;
struct FloatMat33 Ga_inv;
struct FloatVect3 euler_cmd;
float filter_omega = 20.0;
float filter_zeta = 0.65;
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
struct FloatEulers guidance_euler_cmd;
float thrust_in;
static void guidance_indi_propagate_filters(void);
static void guidance_indi_filter_thrust(void);
static void guidance_indi_calcG(struct FloatMat33 *Gmat);
/**
*
* Call upon entering indi guidance
*/
void guidance_indi_enter(void) {
filt_accelzbody = 0;
filt_accelzbodyd = 0;
filt_accelzbodydd = 0;
roll_filt = 0;
roll_filtd = 0;
roll_filtdd = 0;
pitch_filt = 0;
pitch_filtd = 0;
pitch_filtdd = 0;
FLOAT_VECT3_ZERO(filt_accel_ned);
FLOAT_VECT3_ZERO(filt_accel_ned_d);
FLOAT_VECT3_ZERO(filt_accel_ned_dd);
thrust_in = 0.0;
thrust_act = 0;
thrust_filt = 0;
thrust_filtd = 0;
thrust_filtdd = 0;
float tau = 1.0/(2.0*M_PI*filter_cutoff);
float sample_time = 1.0/PERIODIC_FREQUENCY;
for(int8_t i=0; i<3; i++) {
init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
}
init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
}
/**
@@ -133,10 +126,8 @@ void guidance_indi_enter(void) {
*/
void guidance_indi_run(bool in_flight, int32_t heading) {
//filter accel to get rid of noise
guidance_indi_filter_accel();
//filter attitude to synchronize with accel
guidance_indi_filter_attitude();
//filter accel to get rid of noise and filter attitude to synchronize with accel
guidance_indi_propagate_filters();
//Linear controller to find the acceleration setpoint from position and velocity
float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
@@ -160,7 +151,7 @@ void guidance_indi_run(bool in_flight, int32_t heading) {
sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
//for rc vertical control
sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*5.0/9600.0;
sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
#endif
//Calculate matrix of partial derivatives
@@ -168,7 +159,7 @@ void guidance_indi_run(bool in_flight, int32_t heading) {
//Invert this matrix
MAT33_INV(Ga_inv, Ga);
struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, sp_accel.z -filt_accel_ned.z};
struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned[0].o[0], sp_accel.y -filt_accel_ned[1].o[0], sp_accel.z -filt_accel_ned[2].o[0]};
//Bound the acceleration error so that the linearization still holds
Bound(a_diff.x, -6.0, 6.0);
@@ -184,8 +175,10 @@ void guidance_indi_run(bool in_flight, int32_t heading) {
//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
guidance_euler_cmd.phi = roll_filt + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt + euler_cmd.y;
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
//zero psi command, because a roll/pitch quat will be constructed later
guidance_euler_cmd.psi = 0;
@@ -193,7 +186,7 @@ void guidance_indi_run(bool in_flight, int32_t heading) {
guidance_indi_filter_thrust();
//Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
thrust_in = thrust_filt + euler_cmd.z*thrust_in_specific_force_gain;
thrust_in = thrust_filt.o[0] + euler_cmd.z*thrust_in_specific_force_gain;
Bound(thrust_in, 0, 9600);
#ifdef GUIDANCE_INDI_RC_DEBUG
@@ -216,49 +209,31 @@ void guidance_indi_run(bool in_flight, int32_t heading) {
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
/**
*
* Filter the thrust, such that it corresponds to the filtered measured acceleration
* Filter the thrust, such that it corresponds to the filtered acceleration
*/
void guidance_indi_filter_thrust(void)
{
// Actuator dynamics
thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
thrust_filt = thrust_filt + thrust_filtd / PERIODIC_FREQUENCY;
thrust_filtd = thrust_filtd + thrust_filtdd / PERIODIC_FREQUENCY;
thrust_filtdd = -thrust_filtd * 2 * filter_zeta * filter_omega + (thrust_act - thrust_filt) * filter_omega*filter_omega;
// same filter as for the acceleration
update_butterworth_2_low_pass(&thrust_filt, thrust_act);
}
#endif
/**
*
* low pass the accelerometer measurements with a second order filter
* to remove noise from vibrations
* Low pass the accelerometer measurements to remove noise from vibrations.
* The roll and pitch also need to be filtered to synchronize them with the
* acceleration
*/
void guidance_indi_filter_accel(void)
{
VECT3_ADD_SCALED(filt_accel_ned, filt_accel_ned_d, 1.0/PERIODIC_FREQUENCY);
void guidance_indi_propagate_filters(void) {
struct NedCoor_f *accel = stateGetAccelNed_f();
update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
VECT3_ADD_SCALED(filt_accel_ned_d, filt_accel_ned_dd, 1.0/PERIODIC_FREQUENCY);
filt_accel_ned_dd.x = -filt_accel_ned_d.x * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->x - filt_accel_ned.x) * filter_omega*filter_omega;
filt_accel_ned_dd.y = -filt_accel_ned_d.y * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->y - filt_accel_ned.y) * filter_omega*filter_omega;
filt_accel_ned_dd.z = -filt_accel_ned_d.z * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->z - filt_accel_ned.z) * filter_omega*filter_omega;
}
/**
*
* Filter the attitude, such that it corresponds to the filtered measured acceleration
*/
void guidance_indi_filter_attitude(void)
{
roll_filt = roll_filt + roll_filtd / PERIODIC_FREQUENCY;
pitch_filt = pitch_filt + pitch_filtd / PERIODIC_FREQUENCY;
roll_filtd = roll_filtd + roll_filtdd / PERIODIC_FREQUENCY;
pitch_filtd = pitch_filtd + pitch_filtdd / PERIODIC_FREQUENCY;
roll_filtdd = -roll_filtd * 2 * filter_zeta * filter_omega + (stateGetNedToBodyEulers_f()->phi - roll_filt) * filter_omega*filter_omega;
pitch_filtdd = -pitch_filtd * 2 * filter_zeta * filter_omega + (stateGetNedToBodyEulers_f()->theta - pitch_filt) * filter_omega*filter_omega;
update_butterworth_2_low_pass(&roll_filt, stateGetNedToBodyEulers_f()->phi);
update_butterworth_2_low_pass(&pitch_filt, stateGetNedToBodyEulers_f()->theta);
}
/**
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+1 -4
View File
@@ -36,12 +36,9 @@
extern void guidance_indi_enter(void);
extern void guidance_indi_run(bool in_flight, int32_t heading);
extern void guidance_indi_filter_attitude(void);
extern void guidance_indi_calcG(struct FloatMat33 *Gmat);
extern void guidance_indi_filter_accel(void);
extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading);
extern void guidance_indi_filter_thrust(void);
extern float guidance_indi_thrust_specific_force_gain;
extern struct FloatVect3 euler_cmd;
#endif /* GUIDANCE_INDI_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
+7 -4
View File
@@ -24,17 +24,20 @@
* MAVLab Delft University of Technology
* This control algorithm is Incremental Nonlinear Dynamic Inversion (INDI)
*
* This is a simplified implementation of the (soon to be) publication in the
* This is an implementation of the publication in the
* journal of Control Guidance and Dynamics: Adaptive Incremental Nonlinear
* Dynamic Inversion for Attitude Control of Micro Aerial Vehicles
*/
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
void stabilization_attitude_init(void)
{
// Check if the indi init is already done for rate control
#ifndef STABILIZATION_RATE_INDI
stabilization_indi_init();
#endif
}
void stabilization_attitude_enter(void)
@@ -57,9 +60,9 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
stabilization_indi_set_earth_cmd_i(cmd, heading);
}
void stabilization_attitude_run(bool enable_integrator)
void stabilization_attitude_run(bool in_flight)
{
stabilization_indi_run(enable_integrator, FALSE);
stabilization_indi_run(in_flight, FALSE);
}
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h
+4
View File
@@ -33,7 +33,11 @@
#ifndef STABILIZATION_ATTITUDE_QUAT_INDI_H
#define STABILIZATION_ATTITUDE_QUAT_INDI_H
#ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
#else
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#endif
#endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */

Some files were not shown because too many files have changed in this diff Show More