Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Cyclone guidance code (#2533)

Browse Source
This commit is contained in:
Ewoud Smeur
2020-07-27 15:35:26 +02:00
committed by GitHub
parent de22fb1c29
commit 3b4ce8232e
24 changed files with 1414 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/cyfoam.xml
+7
View File
@@ -25,6 +25,9 @@
<define name="CE_YAW_A" value="5.631"/>
<define name="CE_YAW_B" value="0.0515"/>
</module>
<!--Switch advanced INDI scheduling functions on or off-->
<define name="INDI_FUNCTIONS_RC_CHANNEL" value="6"/>
</target>
<target name="nps" board="pc">
@@ -34,6 +37,10 @@
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>
<!--Switch advanced INDI scheduling functions on or off-->
<!--Take the mode channel for simulation-->
<define name="INDI_FUNCTIONS_RC_CHANNEL" value="4"/>
</target>
<module name="actuators" type="pwm">
conf/airframes/tudelft/cyfoam.xml
+326
View File
@@ -0,0 +1,326 @@
<!--Mini-Cyclone EPP Airframe
Chimera AP
Xbee API
Ublox M8T
SBUS Futaba -->
<airframe name="foam cylone">
<servos driver="Pwm">
<servo name="ELEVON_LEFT" no="0" min="1000" neutral="1500" max="2000"/> <!--neutral 1526-->
<servo name="ELEVON_RIGHT" no="1" min="1000" neutral="1500" max="2000"/> <!--neutral 1515-->
<servo name="RM" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="LM" no="3" min="1000" neutral="1100" max="2000"/>
<!--<servo name="SERVO_TEST" no="4" min="1000" neutral="1500" max="2000"/>-->
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<set servo="ELEVON_LEFT" value="autopilot.motors_on ? actuators_pprz[0] : 0"/>
<set servo="ELEVON_RIGHT" value="autopilot.motors_on ? actuators_pprz[1] : 0"/>
<set servo="RM" value="autopilot.motors_on ? actuators_pprz[2] : -MAX_PPRZ"/>
<set servo="LM" value="autopilot.motors_on ? actuators_pprz[3] : -MAX_PPRZ"/>
<!--<set servo="RM" value="autopilot_motors_on ? -MAX_PPRZ : -MAX_PPRZ"/>-->
<!--<set servo="LM" value="autopilot_motors_on ? -MAX_PPRZ : -MAX_PPRZ"/>-->
</command_laws>
<section name="IMU" prefix="IMU_">
<!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
<!--<define name="MAG_X_NEUTRAL" value="-37"/>-->
<!--<define name="MAG_Y_NEUTRAL" value="299"/>-->
<!--<define name="MAG_Z_NEUTRAL" value="197"/>-->
<!--<define name="MAG_X_SENS" value="7.25691459037" integer="16"/>-->
<!--<define name="MAG_Y_SENS" value="7.7018409319" integer="16"/>-->
<!--<define name="MAG_Z_SENS" value="8.16601148911" integer="16"/>-->
<!--<define name= "MAG_X_CURRENT_COEF" value="0.001455"/>-->
<!--<define name= "MAG_Y_CURRENT_COEF" value="0.012152"/>-->
<!--<define name= "MAG_Z_CURRENT_COEF" value="-0.0001088"/>-->
<!--Mag of gps-->
<define name="MAG_X_NEUTRAL" value="21"/>
<define name="MAG_Y_NEUTRAL" value="-206"/>
<define name="MAG_Z_NEUTRAL" value="104"/>
<define name="MAG_X_SENS" value="3.58153558634" integer="16"/>
<define name="MAG_Y_SENS" value="3.58800117886" integer="16"/>
<define name="MAG_Z_SENS" value="3.57182532939" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="94." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="LOW_BAT_LEVEL" value="13.9" units="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="13.2" unit="V"/>
</section>
<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}"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
</section>
<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}"/>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="107.0"/>
<define name="REF_ERR_Q" value="200.0"/>
<define name="REF_ERR_R" value="200.0"/>
<define name="REF_RATE_P" value="14.0"/>
<define name="REF_RATE_Q" value="15.0"/>
<define name="REF_RATE_R" value="15.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="100.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, 0.1, 0.045, 0.045}"/>
<define name="ACT_RATE_LIMIT" value="{170, 170, 9600, 9600}"/>
<define name="ACT_IS_SERVO" value="{1, 1, 0, 0}"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<!-- Gains for vertical navigation -->
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="175"/>
<define name="HOVER_KI" value="72"/>
<define name="NOMINAL_HOVER_THROTTLE" value ="0.4"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="NAV">
<define name="NAV_CLIMB_VSPEED" value="1.5"/>
<define name="NAV_DESCEND_VSPEED" value="-0.8"/>
</section>
<!--<section name="AHRS" prefix="AHRS_">-->
<!--<define name="H_X" value="0.5138"/>-->
<!--<define name="H_Y" value="0.00019"/>-->
<!--<define name="H_Z" value="0.8578"/>-->
<!--<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>-->
<!--</section>-->
<section name="INS" prefix="INS_">
<!-- Delft -->
<define name="H_X" value="0.3928"/>
<define name="H_Y" value="-0.0140"/>
<define name="H_Z" value="0.9195"/>
</section>
<!-- Gains for horizontal navigation-->
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
<!--The Quadshot uses (when TRUE) 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"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-75.0" unit="deg"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="COORDINATED_TURN_AIRSPEED" value="18.0"/>
<define name="BARO_PERIODIC_FREQUENCY" value="50"/>
<define name="GUIDANCE_H_MAX_BANK" value="60" unit="deg"/>
<define name="FWD_SIDESLIP_GAIN" value="0.32"/>
<define name="EFF_SCHED_USE_FUNCTION" value="TRUE"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="USE_AIRSPEED" value="TRUE"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="ele_left, ele_right, mot_right, mot_left" type="string[]"/>
<define name="JSBSIM_MODEL" value="cyclone" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
</section>
<firmware name="rotorcraft">
<target name="ap" board="chimera_1.0">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<module name="radio_control" type="sbus">
<!--[> Put the mode on channel AUX1<]-->
<define name="RADIO_KILL_SWITCH" value="5"/>
</module>
<module name="ctrl_effectiveness_scheduling">
<define name="SQUARED_ROLL_EFF" value="0.0018"/>
<define name="PITCH_EFF_AT_60" value="4.0"/>
<define name="YAW_EFF_AT_60" value="8.0"/>
<!--function of the form: A + B*airspeed^2-->
<define name="CE_PITCH_A" value="4.5"/>
<define name="CE_PITCH_B" value="0.01"/>
<define name="CE_YAW_A" value="5.631"/>
<define name="CE_YAW_B" value="0.0515"/>
</module>
<!--Switch advanced INDI scheduling functions on or off-->
<define name="INDI_FUNCTIONS_RC_CHANNEL" value="6"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
<module name="udp"/>
<module name="radio_control" type="datalink"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>
<!--Switch advanced INDI scheduling functions on or off-->
<!--Take the mode channel for simulation-->
<define name="INDI_FUNCTIONS_RC_CHANNEL" value="4"/>
</target>
<!--<module name="gps" type="ubx_ucenter"/>-->
<module name="tlsf"/>
<module name="pprzlog">
<define name="SDLOG_START_DELAY" value="10"/>
</module>
<module name="logger" type="sd_chibios"/>
<!--<module name="flight_recorder"/>-->
<!--<module name="logger_spi_link"/>-->
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<module name="airspeed" type="ms45xx_i2c">
<define name="MS45XX_I2C_DEV" value="i2c1"/>
<define name="MS45XX_AIRSPEED_SCALE" value="2.4"/>
<define name="USE_AIRSPEED_MS45XX" value="TRUE"/>
</module>
<module name="pwm_meas">
<define name="USE_PWM_INPUT1" value="PWM_PULSE_TYPE_ACTIVE_LOW"/>
<define name="USE_PWM_INPUT2" value="PWM_PULSE_TYPE_ACTIVE_LOW"/>
</module>
<module name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
<!--define name="USE_SERVOS_7AND8"/-->
</module>
<module name="mag" type="hmc58xx">
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
<define name="HMC58XX_CHAN_X" value="1"/>
<define name="HMC58XX_CHAN_Y" value="0"/>
<define name="HMC58XX_CHAN_Z" value="2"/>
<define name="HMC58XX_CHAN_X_SIGN" value="-"/>
<define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
<define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
<!--run the chimera mag once every 64 seconds to avoid message spam-->
<define name="MPU9250_MAG_PRESCALER" value="32768"/>
<define name="HMC58XX_MAG_TO_IMU_PHI" value="RadOfDeg\(6.5\)"/>
<define name="HMC58XX_MAG_TO_IMU_THETA" value="RadOfDeg\(28.5\)"/>
<define name="HMC58XX_MAG_TO_IMU_PSI" value="RadOfDeg\(181.0\)"/>
</module>
<module name="send_imu_mag_current">
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="30000"/>
<define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.5"/>
</module>
<module name="sys_mon"/>
<module name="telemetry" type="xbee_api"/>
<module name="imu" type="chimera">
<define name="IMU_MPU9250_GYRO_RANGE" value="MPU9250_GYRO_RANGE_2000" />
<define name="IMU_MPU9250_ACCEL_RANGE" value="MPU9250_ACCEL_RANGE_16G" />
</module>
<!--<module name="servo_tester"/>-->
<module name="air_data">
<define name="USE_AIRSPEED_AIR_DATA" value="FALSE"/>
</module>
<module name="gps" type="ublox">
<configure name="GPS_BAUD" value="B115200"/>
</module>
<module name="stabilization" type="indi">
<define name="INDI_THRUST_ON_PITCH_EFF" value="23.0"/>
</module>
<module name="guidance" type="indi_hybrid">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.5"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.8"/>
<define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="3000"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="15.0"/>
<!--<define name="KNIFE_EDGE_TEST" value="TRUE"/>-->
<!--Flap effectiveness on lift-->
<define name="FE_LIFT_A_PITCH" value="0.00018"/>
<define name="FE_LIFT_B_PITCH" value="0.00072"/>
<define name="FE_LIFT_A_AS" value="0.0008"/>
<define name="FE_LIFT_B_AS" value="0.00009"/>
</module>
<!--<module name="ahrs" type="int_cmpl_quat">-->
<!--<configure name="USE_MAGNETOMETER" value="TRUE"/>-->
<!--<define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID"/>-->
<!--<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/>-->
<!--<define name="AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION" value="TRUE"/>-->
<!--</module>-->
<!--<module name="ins"/>-->
<module name="ins" type="ekf2"/>
<!--<module name="ins" type="float_invariant">-->
<!--<define name="INS_PROPAGATE_FREQUENCY" value="500"/>-->
<!--<define name="INS_FINV_MAG_ID" value="MAG_HMC58XX_SENDER_ID"/>-->
<!--</module>-->
</firmware>
</airframe>
conf/flight_plans/tudelft/cyclone_valkenburg.xml
+76
View File
@@ -0,0 +1,76 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="40" ground_alt="0" lat0="52.169348" lon0="4.411563" max_dist_from_home="500" name="Rotorcraft Basic (Enac)" security_height="2">
<header>
#include "autopilot.h"
</header>
<waypoints>
<waypoint name="HOME" x="-15.8" y="16.5"/>
<waypoint name="CLIMB" x="-39.0" y="17.6"/>
<waypoint name="STDBY" x="-29.1" y="-47.5"/>
<waypoint name="p1" x="-4.7" y="-17.3"/>
<waypoint name="p2" x="40.0" y="-191.8"/>
<waypoint name="p3" x="-72.2" y="-217.0"/>
<waypoint name="p4" x="-120.9" y="-48.9"/>
<waypoint name="TD" x="-25.4" y="34.0"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 1)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="false" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<set value="false" var="force_forward"/>
<stay wp="p1"/>
</block>
<block name="go_p2">
<set value="false" var="force_forward"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="route">
<set value="true" var="force_forward"/>
<while cond="true">
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
</while>
<deroute block="stay_p1"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="false" var="force_forward"/>
<go wp="TD"/>
</block>
<block name="flare">
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/gcs/bottom_settings.xml
+1 -9
View File
@@ -66,14 +66,6 @@
<property name="size" value="15."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="926" y="2">
<property name="scale" value="1."/>
<property name="field" value="AIR_DATA:airspeed"/>
<property name="ac_id" value="replay6"/>
<property name="format" value="AS %.2f kts"/>
<property name="size" value="15."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="924" y="46">
<property name="scale" value="3.28084"/>
<property name="field" value="AIR_DATA:amsl_baro"/>
@@ -104,7 +96,7 @@
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="130" y="47">
<property name="scale" value="1.94384"/>
<property name="scale" value="1"/>
<property name="field" value="AIR_DATA:airspeed"/>
<property name="format" value="AS %.2f m/s"/>
<property name="size" value="15."/>
conf/modules/guidance_indi_hybrid.xml
+28
View File
@@ -0,0 +1,28 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="guidance_indi_hybrid" dir="guidance">
<doc>
<description>
Guidance controller for hybrids using INDI
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="guidance_indi_hybrid">
<dl_setting var="gih_params.pos_gain" min="0" step="0.1" max="10.0" shortname="kp" param="GUIDANCE_INDI_POS_GAIN" persistent="true"/>
<dl_setting var="gih_params.speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="guidance_indi_hybrid.h"/>
</header>
<init fun="guidance_indi_init()"/>
<init fun="guidance_indi_enter()"/>
<!--<periodic fun="guidance_indi_propagate_filters()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>-->
<makefile target="ap|nps" firmware="rotorcraft">
<file name="guidance_indi_hybrid.c" dir="$(SRC_FIRMWARE)/guidance"/>
<define name="GUIDANCE_INDI" value="TRUE"/>
<define name="GUIDANCE_INDI_HYBRID" value="TRUE"/>
</makefile>
</module>
conf/radios/sbus_delft.xml
+55
View File
@@ -0,0 +1,55 @@
<?xml version="1.0"?>
<!-- $Id$
--
-- (c) 2013 Gautier Hattenberger
--
-- 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, write to
-- the Free Software Foundation, 59 Temple Place - Suite 330,
-- Boston, MA 02111-1307, USA.
-->
<!--
-- Attributes of root (Radio) tag :
-- name: name of RC
-- data_min: min width of a pulse to be considered as a data pulse
-- data_max: max width of a pulse to be considered as a data pulse
-- sync_min: min width of a pulse to be considered as a synchro pulse
-- sync_max: max width of a pulse to be considered as a synchro pulse
-- pulse_type: POSITIVE ( Futaba and others) | NEGATIVE (JR)
-- min, max and sync are expressed in micro-seconds
-->
<!--
-- Attributes of channel tag :
-- ctl: name of the command on the transmitter - only for displaying
-- function: logical command
-- average: (boolean) channel filtered through several frames (for discrete commands)
-- min: minimum pulse length (micro-seconds)
-- max: maximum pulse length (micro-seconds)
-- neutral: neutral pulse length (micro-seconds)
-- Note: a command may be reversed by exchanging min and max values
-->
<!DOCTYPE radio SYSTEM "radio.dtd">
<radio name="Futaba T10CG with SBUS" data_min="900" data_max="2100" sync_min ="5000" sync_max ="15000" pulse_type="POSITIVE">
<channel ctl="A" function="THROTTLE" min="1000" neutral="1000" max="2000" average="0"/>
<channel ctl="B" function="ROLL" min="1000" neutral="1500" max="2000" average="0"/>
<channel ctl="C" function="PITCH" min="1000" neutral="1500" max="2000" average="0"/>
<channel ctl="D" function="YAW" min="1000" neutral="1500" max="2000" average="0"/>
<channel ctl="E" function="MODE" max="966" neutral="1520" min="2072" average="1"/>
<channel ctl="F" function="GAIN1" min="966" neutral="1520" max="2072" average="0"/>
<channel ctl="G" function="GAIN2" min="966" neutral="1520" max="2072" average="0"/>
</radio>
conf/simulator/jsbsim/aircraft/Systems/aerodynamics_cyfoam.xml
+32 -2
View File
@@ -1,5 +1,35 @@
<aerodynamics>
<axis name="YAW">
<function name="elevon_yaw" frame="BODY" unit="FT*LBS">
<sum>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.35 </value>
</product>
<product>
<property>fcs/ele_right_lag</property>
<value> -.35 </value>
</product>
</sum>
</function>
</axis>
<axis name="PITCH">
<function name="elevon_pitch" frame="BODY" unit="FT*LBS">
<sum>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.0417 </value>
</product>
<product>
<property>fcs/ele_right_lag</property>
<value> .0417 </value>
</product>
</sum>
</function>
</axis>
<axis name="LIFT">
<function name="aero/force/Lift_alpha">
@@ -12,8 +42,8 @@
<tableData>
-1.87 -0.35
-1.57 0.0
-1.27 0.35
-0.80 0.25
-1.27 0.65
-0.80 0.65
0.00 0.0
0.80 0.2
1.27 0.2
conf/simulator/jsbsim/aircraft/cyclone.xml
-163
View File
@@ -195,171 +195,8 @@
</direction>
</force>
<!-- Then the Moment Couples -->
<!--Pitch moments-->
<force name="ele_left_pitch1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.5 </value>
</product>
</function>
<location unit="IN">
<!-- Necessary arm in IN to produce a moment ten times
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
<x>0</x>
<y>0</y>
<z>1</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="ele_left_pitch2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.5 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="ele_right_pitch1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_right_lag</property>
<value> 0.5 </value>
</product>
</function>
<location unit="IN">
<!-- Necessary arm in IN to produce a moment ten times
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
<x>0</x>
<y>0</y>
<z>1</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="ele_right_pitch2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_right_lag</property>
<value> 0.5 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<!--Yaw moments-->
<force name="ele_left_yaw1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_left_lag</property>
<value> 4.2 </value>
</product>
</function>
<location unit="IN">
<!-- Necessary arm in IN to produce a moment ten times
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
<x>0</x>
<y>1</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="ele_left_yaw2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_left_lag</property>
<value> 4.2 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="ele_right_yaw1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_right_lag</property>
<value> 4.2 </value>
</product>
</function>
<location unit="IN">
<!-- Necessary arm in IN to produce a moment ten times
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
<x>0</x>
<y>1</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="ele_right_yaw2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/ele_right_lag</property>
<value> 4.2 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
</external_reactions>
<propulsion/>
conf/userconf/tudelft/conf.xml
+11
View File
@@ -144,6 +144,17 @@
settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue"
/>
<aircraft
name="CYFOAM"
ac_id="94"
airframe="airframes/tudelft/cyfoam.xml"
radio="radios/sbus_delft.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/cyclone_valkenburg.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/ins_ekf2.xml modules/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="#ffff7f7f0000"
/>
<aircraft
name="ChouChou_LisaS"
ac_id="135"
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+8 -1
View File
@@ -119,6 +119,13 @@ static void send_fp(struct transport_tx *trans, struct link_device *dev)
int32_t carrot_up = -guidance_v_z_sp;
int32_t carrot_heading = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
int32_t thrust = (int32_t)autopilot.throttle;
#if GUIDANCE_INDI_HYBRID
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
int32_t state_psi = ANGLE_BFP_OF_REAL(eulers_zxy.psi);
#else
int32_t state_psi = stateGetNedToBodyEulers_i()->psi;
#endif
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
@@ -128,7 +135,7 @@ static void send_fp(struct transport_tx *trans, struct link_device *dev)
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&state_psi,
&guidance_h.sp.pos.y,
&guidance_h.sp.pos.x,
&carrot_up,
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+15 -3
View File
@@ -29,12 +29,16 @@
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_flip.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h"
#include "subsystems/radio_control.h"
#if GUIDANCE_INDI_HYBRID
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#else
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#endif
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
@@ -77,6 +81,12 @@ PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF)
#define GUIDANCE_INDI FALSE
#endif
// Navigation can set heading freely
// This is false if sideslip is a problem
#ifndef GUIDANCE_HEADING_IS_FREE
#define GUIDANCE_HEADING_IS_FREE TRUE
#endif
struct HorizontalGuidance guidance_h;
int32_t transition_percentage;
@@ -598,12 +608,14 @@ void guidance_h_from_nav(bool in_flight)
guidance_h_update_reference();
#if GUIDANCE_HEADING_IS_FREE
/* set psi command */
guidance_h.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
#endif
#if GUIDANCE_INDI
guidance_indi_run(guidance_h.sp.heading);
guidance_indi_run(&guidance_h.sp.heading);
#else
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
@@ -683,7 +695,7 @@ void guidance_h_guided_run(bool in_flight)
guidance_h_update_reference();
#if GUIDANCE_INDI
guidance_indi_run(guidance_h.sp.heading);
guidance_indi_run(&guidance_h.sp.heading);
#else
/* compute x,y earth commands */
guidance_h_traj_run(in_flight);
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+2 -2
View File
@@ -151,7 +151,7 @@ void guidance_indi_enter(void)
*
* main indi guidance function
*/
void guidance_indi_run(float heading_sp)
void guidance_indi_run(float *heading_sp)
{
struct FloatEulers eulers_yxz;
struct FloatQuat * statequat = stateGetNedToBodyQuat_f();
@@ -236,7 +236,7 @@ void guidance_indi_run(float heading_sp)
guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;
guidance_euler_cmd.psi = heading_sp;
guidance_euler_cmd.psi = *heading_sp;
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
guidance_indi_filter_thrust();
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+1 -1
View File
@@ -33,7 +33,7 @@
#include "math/pprz_algebra_float.h"
extern void guidance_indi_enter(void);
extern void guidance_indi_run(float heading_sp);
extern void guidance_indi_run(float *heading_sp);
extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, float heading);
extern void guidance_indi_init(void);
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+651
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+53
View File
@@ -0,0 +1,53 @@
/*
* Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
*
* A guidance mode based on Incremental Nonlinear Dynamic Inversion
* Come to ICRA2016 to learn more!
*
*/
#ifndef GUIDANCE_INDI_HYBRID_H
#define GUIDANCE_INDI_HYBRID_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "filters/high_pass_filter.h"
extern void guidance_indi_enter(void);
extern void guidance_indi_run(float *heading_sp);
extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading);
extern void guidance_indi_init(void);
extern void guidance_indi_propagate_filters(void);
struct guidance_indi_hybrid_params {
float pos_gain;
float pos_gainz;
float speed_gain;
float speed_gainz;
};
extern struct guidance_indi_hybrid_params gih_params;
#endif /* GUIDANCE_INDI_HYBRID_H */
sw/airborne/firmwares/rotorcraft/navigation.c
+7 -2
View File
@@ -76,6 +76,10 @@
#define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC)
#define CARROT_DIST (12 << INT32_POS_FRAC)
bool force_forward = false;
struct FloatVect2 line_vect, to_end_vect;
const float max_dist_from_home = MAX_DIST_FROM_HOME;
const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
@@ -118,10 +122,8 @@ int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
/* nav_route variables */
struct EnuCoor_i nav_segment_start, nav_segment_end;
static inline void nav_set_altitude(void);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -209,6 +211,9 @@ void nav_init(void)
dist2_to_home = 0;
dist2_to_wp = 0;
FLOAT_VECT2_ZERO(line_vect);
FLOAT_VECT2_ZERO(to_end_vect);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WP_MOVED, send_wp_moved);
sw/airborne/firmwares/rotorcraft/navigation.h
+17
View File
@@ -113,6 +113,12 @@ extern void nav_run(void);
extern void set_exception_flag(uint8_t flag_num);
extern float nav_max_speed;
extern bool force_forward;
extern struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain);
extern struct FloatVect3 nav_get_speed_setpoint(float pos_gain);
extern struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v, struct FloatVect2 to_end_v, struct EnuCoor_i target, float pos_gain);
extern float get_dist2_to_waypoint(uint8_t wp_id);
extern float get_dist2_to_point(struct EnuCoor_i *p);
extern void compute_dist2_to_home(void);
@@ -268,11 +274,22 @@ extern uint8_t nav_oval_count;
/*********** Navigation along a line *************************************/
extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end);
extern struct FloatVect2 line_vect, to_end_vect;
#ifdef GUIDANCE_INDI_HYBRID
static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
{
VECT2_DIFF(line_vect, waypoints[wp_end].enu_f, waypoints[wp_start].enu_f);
VECT2_DIFF(to_end_vect, waypoints[wp_end].enu_f, *stateGetPositionEnu_f());
VECT3_COPY(navigation_target, waypoints[wp_end].enu_i);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
}
#else
static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
{
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&waypoints[wp_start].enu_i, &waypoints[wp_end].enu_i);
}
#endif
/** Nav glide routine */
static inline void NavGlide(uint8_t start_wp, uint8_t wp)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+22 -2
View File
@@ -119,6 +119,9 @@ float indi_u[INDI_NUM_ACT];
float indi_du[INDI_NUM_ACT];
float g2_times_du;
float q_filt = 0.0;
float r_filt = 0.0;
// variables needed for estimation
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
@@ -365,10 +368,13 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
struct FloatRates *body_rates = stateGetBodyRates_f();
q_filt = (q_filt*3+body_rates->q)/4;
r_filt = (r_filt*3+body_rates->r)/4;
//calculate the virtual control (reference acceleration) based on a PD controller
angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
angular_accel_ref.q = (rate_ref.q - body_rates->q) * reference_acceleration.rate_q;
angular_accel_ref.r = (rate_ref.r - body_rates->r) * reference_acceleration.rate_r;
angular_accel_ref.q = (rate_ref.q - q_filt) * reference_acceleration.rate_q;
angular_accel_ref.r = (rate_ref.r - r_filt) * reference_acceleration.rate_r;
g2_times_du = 0.0;
int8_t i;
@@ -417,6 +423,20 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
du_min[i] = -MAX_PPRZ * act_is_servo[i] - actuator_state_filt_vect[i];
du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
#ifdef GUIDANCE_INDI_MIN_THROTTLE
float airspeed = stateGetAirspeed_f();
//limit minimum thrust ap can give
if(!act_is_servo[i]) {
if((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
if(airspeed < 8.0) {
du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - actuator_state_filt_vect[i];
} else {
du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - actuator_state_filt_vect[i];
}
}
}
#endif
}
// WLS Control Allocator
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+3
View File
@@ -32,9 +32,12 @@
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
extern float actuator_state_filt_vect[INDI_NUM_ACT];
extern bool indi_use_adaptive;
extern float *Bwls[INDI_OUTPUTS];
struct ReferenceSystem {
float err_p;
float err_q;
sw/airborne/math/pprz_algebra_float.c
+20
View File
@@ -959,3 +959,23 @@ float float_mat_norm_li(float **a, int m, int n)
}
return value;
}
/* Scale a 3D vector to within a 2D bound */
void vect_bound_in_2d(struct FloatVect3 *vect3, float bound) {
float norm = FLOAT_VECT2_NORM(*vect3);
if(norm>bound) {
float scale = bound/norm;
vect3->x *= scale;
vect3->y *= scale;
}
}
/* Scale a 3D vector to a certain length in 2D */
void vect_scale(struct FloatVect3 *vect3, float norm_des) {
float norm = FLOAT_VECT2_NORM(*vect3);
if(norm>0.1) {
float scale = norm_des/norm;
vect3->x *= scale;
vect3->y *= scale;
}
}
sw/airborne/math/pprz_algebra_float.h
+3
View File
@@ -863,6 +863,9 @@ extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
extern bool float_mat_inv_4d(float invOut[16], float mat_in[16]);
extern void vect_bound_in_2d(struct FloatVect3 *vect3, float bound);
extern void vect_scale(struct FloatVect3 *vect3, float norm_des);
#ifdef __cplusplus
} /* extern "C" */
#endif
sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c
+73 -1
View File
@@ -34,6 +34,9 @@
#error "You need to use WLS control allocation for this module"
#endif
#ifndef INDI_FUNCTIONS_RC_CHANNEL
#error "You need to define an RC channel to switch between simple and advanced scheduling"
#endif
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {FWD_G1_ROLL,
FWD_G1_PITCH, FWD_G1_YAW, FWD_G1_THRUST
@@ -65,6 +68,30 @@ void ctrl_eff_scheduling_init(void)
}
void ctrl_eff_scheduling_periodic(void)
{
if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) {
ctrl_eff_scheduling_periodic_b();
} else {
ctrl_eff_scheduling_periodic_a();
}
#ifdef INDI_THRUST_ON_PITCH_EFF
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] < -7000) && (actuator_state_filt_vect[1] > 7000)) {
Bwls[1][2] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
Bwls[1][3] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
} else if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] > 7000) && (actuator_state_filt_vect[1] < -7000)) {
Bwls[1][2] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
Bwls[1][3] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
} else {
Bwls[1][2] = 0.0;
Bwls[1][3] = 0.0;
}
#endif
}
void ctrl_eff_scheduling_periodic_a(void)
{
// Go from transition percentage to ratio
float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
@@ -73,7 +100,52 @@ void ctrl_eff_scheduling_periodic(void)
int8_t j;
for (i = 0; i < INDI_OUTPUTS; i++) {
for (j = 0; j < INDI_NUM_ACT; j++) {
g1g2[i][j] = (g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio);
g1g2[i][j] = g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio;
}
}
}
void ctrl_eff_scheduling_periodic_b(void)
{
float airspeed = stateGetAirspeed_f();
struct FloatEulers eulers_zxy;
if(airspeed < 6.0) {
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
float pitch_interp = DegOfRad(eulers_zxy.theta);
Bound(pitch_interp, -60.0, -30.0);
float ratio = (pitch_interp + 30.0)/(-30.);
/*pitch*/
g1g2[1][0] = g1g2_hover[1][0]*(1-ratio) + -PITCH_EFF_AT_60/1000*ratio;
g1g2[1][1] = g1g2_hover[1][1]*(1-ratio) + PITCH_EFF_AT_60/1000*ratio;
/*yaw*/
g1g2[2][0] = g1g2_hover[2][0]*(1-ratio) + -YAW_EFF_AT_60/1000*ratio;
g1g2[2][1] = g1g2_hover[2][1]*(1-ratio) + -YAW_EFF_AT_60/1000*ratio;
} else {
// calculate squared airspeed
Bound(airspeed, 0.0, 30.0);
float airspeed2 = airspeed*airspeed;
float pitch_eff = CE_PITCH_A + CE_PITCH_B*airspeed2;
g1g2[1][0] = -pitch_eff/1000;
g1g2[1][1] = pitch_eff/1000;
float yaw_eff = CE_YAW_A + CE_YAW_B*airspeed2;
g1g2[2][0] = -yaw_eff/1000;
g1g2[2][1] = -yaw_eff/1000;
}
g1g2[0][2] = -actuator_state_filt_vect[2]/1000*SQUARED_ROLL_EFF;
g1g2[0][3] = actuator_state_filt_vect[3]/1000*SQUARED_ROLL_EFF;
Bound(g1g2[0][2], -30.0/1000, -2.0/1000);
Bound(g1g2[0][3], 2.0/1000, 30.0/1000);
/*Make pitch gain equal to roll gain for turns forward flight*/
if(airspeed > 12.0) {
reference_acceleration.err_q = 107.0;
} else {
reference_acceleration.err_q = 200.0;
}
}
sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.h
+2
View File
@@ -38,6 +38,8 @@ extern void ctrl_eff_scheduling_init(void);
* Periodic function that interpolates between gain sets depending on the scheduling variable.
*/
extern void ctrl_eff_scheduling_periodic(void);
extern void ctrl_eff_scheduling_periodic_a(void);
extern void ctrl_eff_scheduling_periodic_b(void);
#endif /* CTRL_EFFECTIVENESS_SCHEDULING_H */
sw/simulator/nps/nps_autopilot_rotorcraft.c
+1
View File
@@ -126,6 +126,7 @@ void nps_autopilot_run_step(double time)
#if USE_AIRSPEED
if (nps_sensors_airspeed_available()) {
stateSetAirspeed_f((float)sensors.airspeed.value);
AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
}
#endif