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

[tudelft] Rot Wing Updates V3 (#3278)

Browse Source 
* max bank in deg

* takeoff no attitude msec timer

* Prepared elevator moment compensation fix

* [EHVB_rotwing fp] Updated takeoff stratgey with roll and pitch check and added standby_free to flightplan

* [fp EHVB] Takeoff 3 seconds on att 0,0

* increase filter freq and setting for max acc

* [rot_wing_eff_sched] Elevator 5 degrees higher

* [rot_wing] Decreased cutoff frequencies of filters

* [modules] Support dual ublox GPS modules

* [ekf2] Add failsafe remove yaw

* Reverted acceleration limits

* takeoff procedure update

* [flight_plan] Takeoff when hover motors are running

* scale elevator ctrl eff in transition

* [conf] Fix takeoff

* Higher pitch gains

* Fix conf

* Add extra throttle for spinup

* Update calibration

* max_bank in Radians only except in xml/gcs with auto-conversion

* cleanup

* Use flightplan variables instead...

* fix test

* revert debugging action

* cleanup unused

* cleanup more

* Fix compile bug

* [pfc] Fix actuators

* Add follow tests

* reduce pitch weight in forward flight

* correctly set cmd thrust in INDI

* fix takeoff unequal roll effectiveness and not in_flight

* Fix heading in approach

* fix double define and roll scaling setting

* settings names and roll scaling in right settings

* moving simulator stuff

* [flight_plan] Update angel for takeoff

* Update conf

* Conf update

* [conf] Update checks

* land in approach

* Fix flightplan

* Update sw/ground_segment/python/moving_base/moving_base.py

* input params for moving base sim

* no elevator compensation

---------

Co-authored-by: Ewoud Smeur <e.j.j.smeur@tudelft.nl>
Co-authored-by: Dennis van Wijngaarden <32736330+Dennis-Wijngaarden@users.noreply.github.com>
Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
This commit is contained in:
Christophe De Wagter
2024-06-05 14:58:07 +02:00
committed by GitHub
parent 827a468e8a
commit 308a698bf4
22 changed files with 436 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+3 -2
View File
@@ -281,7 +281,8 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -446,7 +447,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
conf/airframes/tudelft/rot_wing_v3b.xml
+5 -4
View File
@@ -338,7 +338,8 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -426,10 +427,10 @@
<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="7.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
@@ -490,7 +491,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
conf/airframes/tudelft/rot_wing_v3d.xml
+5 -4
View File
@@ -326,7 +326,8 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -414,10 +415,10 @@
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
@@ -477,7 +478,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
conf/airframes/tudelft/rot_wing_v3e.xml
+5 -4
View File
@@ -326,7 +326,8 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -414,10 +415,10 @@
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
@@ -477,7 +478,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
conf/airframes/tudelft/rot_wing_v3f.xml
+5 -4
View File
@@ -326,7 +326,8 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -414,10 +415,10 @@
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
@@ -477,7 +478,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
conf/airframes/tudelft/rot_wing_v3g.xml
+16 -8
View File
@@ -30,6 +30,7 @@
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
@@ -323,14 +324,15 @@
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
<!-- Preflight check actuators (ELE, RUD, AIL_L, FLAP_L, AIL_R, FLAP_R, ROT_M, M_FRONT, M_RIGHT_, M_BACK, M_LEFT, M_PUSH) -->
<define name="PFC_ACTUATORS" value="{{.feedback_id=SERVO_SERVO_ELEVATOR_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=0.85, .high_feedback=0.3, .timeout=1},{.feedback_id=SERVO_SERVO_RUDDER_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_ROTATION_MECH_IDX, .feedback_id2=SERVO_BROTATION_MECH_IDX, .low=-9600, .high=9600, .low_feedback=1.57, .high_feedback=0, .timeout=5},{.feedback_id=SERVO_MOTOR_FRONT_IDX, .feedback_id2=SERVO_BMOTOR_FRONT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_RIGHT_IDX, .feedback_id2=SERVO_BMOTOR_RIGHT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_BACK_IDX, .feedback_id2=SERVO_BMOTOR_BACK_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_LEFT_IDX, .feedback_id2=SERVO_BMOTOR_LEFT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_PUSH_IDX, .feedback_id2=255, .low=-9600, .high=2000, .low_feedback=0, .high_feedback=2200, .timeout=3}}"/>
<define name="PFC_ACTUATORS" value="{{.feedback_id=SERVO_SERVO_ELEVATOR_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=0.56, .high_feedback=0.09, .timeout=1},{.feedback_id=SERVO_SERVO_RUDDER_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.25, .high_feedback=0.25, .timeout=1},{.feedback_id=SERVO_AIL_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_ROTATION_MECH_IDX, .feedback_id2=SERVO_BROTATION_MECH_IDX, .low=-9600, .high=9600, .low_feedback=1.57, .high_feedback=0, .timeout=5},{.feedback_id=SERVO_MOTOR_FRONT_IDX, .feedback_id2=SERVO_BMOTOR_FRONT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_RIGHT_IDX, .feedback_id2=SERVO_BMOTOR_RIGHT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_BACK_IDX, .feedback_id2=SERVO_BMOTOR_BACK_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_LEFT_IDX, .feedback_id2=SERVO_BMOTOR_LEFT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_PUSH_IDX, .feedback_id2=255, .low=-9600, .high=2000, .low_feedback=0, .high_feedback=2200, .timeout=3}}"/>
<!-- Others -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -339,7 +341,6 @@
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="MULTI_GPS_MODE" value="PRIMARY_GPS"/>
</section>
<section name="GROUND_DETECT">
@@ -421,10 +422,10 @@
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
@@ -484,7 +485,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
@@ -502,6 +503,7 @@
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<define name="ACCEL_FWD_BX_LIM" value="3.0"/>
</section>
<section name="FORWARD">
@@ -512,12 +514,16 @@
<section name="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.1" />
<define name="SLOPE" value="50.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="TARGET_POS">
<define name="TARGET_OFFSET_DISTANCE" value="20.0"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
@@ -553,7 +559,9 @@
<item name="tail connection">Check tail connection</item>
<item name="wing tape">Check wings taped and secured</item>
<item name="attitude">Check attitude and heading</item>
<item name="airspeed">Airspeed sensor calibration</item>
<item name="takeoff location">Put UAV on take-off location</item>
<item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item>
<item name="camera">Switch on camera</item>
conf/airframes/tudelft/rot_wing_v3h.xml
+7 -6
View File
@@ -30,6 +30,7 @@
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
@@ -330,7 +331,8 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -339,7 +341,6 @@
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="MULTI_GPS_MODE" value="PRIMARY_GPS"/>
</section>
<section name="GROUND_DETECT">
@@ -354,7 +355,7 @@
<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-19,0,11}, .scale={{36573,4111,12385},{3737,420,1269}}, .filter_sample_freq=1130, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-1,-5,24}, .scale={{21205,53606,11977},{4326,10919,2440}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-63,-14,21}, .scale={{32790,18121,37501},{6697,3698,7659}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={2658,2634,1176}, .scale={{20085,2361,16413},{34633,4348,28912}}, .body_to_sensor={{0,16384,0,16384,0,0,0,0,-16384}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={11770,9014,-7862}, .scale={{1879,14000,16295},{3057,25229,28216}}, .body_to_sensor={{0,16384,0,16384,0,0,0,0,-16384}}}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1130, .filter_freq=30}}"/>
<!-- Define axis in hover frame -->
@@ -421,10 +422,10 @@
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
@@ -484,7 +485,7 @@
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
conf/flight_plans/tudelft/rotating_wing_EHR8.xml
+26 -5
View File
@@ -41,9 +41,14 @@
<corner name="C9"/>
</sector>
</sectors>
<variables>
<variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/>
<variable var="stage_timer_msec"/>
</variables>
<modules>
<module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/>
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_TD"/>
</module>
</modules>
<exceptions>
@@ -59,7 +64,7 @@
<block name="Geo init">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
<call_once fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
@@ -68,16 +73,25 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block>
<block name="Wait takeoff">
<exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/>
<set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
<set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
<attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Climb">
@@ -92,6 +106,13 @@
</block>
<block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="Approach Flare"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<block name="Approach Flare">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<!-- <block name="follow_module">
conf/flight_plans/tudelft/rotating_wing_EHVB.xml
+27 -9
View File
@@ -1,5 +1,5 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="70" ground_alt="0" lat0="52.1681551" lon0="4.4126468" max_dist_from_home="1070" name="Rotating Wing Valkenburg" security_height="2">
<flight_plan alt="80" ground_alt="0" lat0="52.1681551" lon0="4.4126468" max_dist_from_home="1070" name="Rotating Wing Valkenburg" security_height="2">
<header/>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
@@ -15,8 +15,8 @@
<waypoint name="p4" x="120" y="-75"/> -->
<waypoint name="circ" x="160" y="25"/>
<waypoint name="TD" x="10" y="-1"/>
<!-- <waypoint name="APP" x="70" y="-25"/> -->
<!-- <waypoint name="FOLLOW" x="300" y="80"/> -->
<waypoint name="APP" x="70" y="-25"/>
<waypoint name="FOLLOW" x="300" y="80"/>
<!-- EHVB -->
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
<waypoint lat="52.168049" lon="4.406923" name="C2"/>
@@ -41,10 +41,15 @@
<corner name="C9"/>
</sector>
</sectors>
<variables>
<variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/>
<variable var="stage_timer_msec"/>
</variables>
<modules>
<!--<module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/>
</module> -->
<module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_FOLLOW"/>
</module>
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosHeight() @GT 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
@@ -68,16 +73,25 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block>
<block name="Wait takeoff">
<exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 3.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 3.0) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/>
<set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
<set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
<attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Climb">
@@ -162,6 +176,10 @@
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block> -->
<block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<block name="Transition_quad">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<go wp="STDBY"/>
conf/modules/eff_scheduling_rot_wing.xml
+2 -1
View File
@@ -22,6 +22,7 @@
<dl_settings>
<dl_settings NAME="eff_sched">
<dl_setting var="eff_sched_pusher_time" min="0.002" step="0.002" max="3." shortname="push_time"/>
<dl_setting shortname="roll_scaling" var="roll_eff_scaling" min="0.3" step="0.01" max="1.0"/>
</dl_settings>
</dl_settings>
</settings>
@@ -32,7 +33,7 @@
<periodic fun="eff_scheduling_rot_wing_periodic()" freq="10.0"/>
<makefile>
<file name="eff_scheduling_rot_wing.c"/>
<test>
<test firmware="rotorcraft">
<define name="ACTUATORS_NB" value="10"/>
<define name="SERVO_ROTATION_MECH_IDX" value="8"/>
<define name="SERVO_MOTOR_FRONT" value="0"/>
conf/modules/ins_ekf2.xml
+1 -1
View File
@@ -49,7 +49,7 @@
<dl_setting var="ekf2.qnh" min="500" step="0.1" max="1500" shortname="QNH"/>
<dl_setting var="ekf2.temp" min="-50" step="0.1" max="80" shortname="temperature"/>
<dl_setting var="ekf2.mag_fusion_type" min="0" step="1" max="5" shortname="mag_fusion" values="AUTO|HEADING|3D|AUTOFW|INDOOR|NONE" module="modules/ins/ins_ekf2" handler="change_param"/>
<dl_setting var="ekf2.fusion_mode" min="0" max="1" step="1" shortname="remove_gps" values="FALSE|TRUE" module="modules/ins/ins_ekf2" handler="remove_gps" type="bool"/>
<dl_setting var="ekf2.fusion_mode" min="0" max="1" step="1" shortname="remove_gps_yaw" values="FALSE|TRUE" module="modules/ins/ins_ekf2" handler="remove_gps" type="bool"/>
</dl_settings>
</dl_settings>
</settings>
conf/modules/parachute.xml
+1 -1
View File
@@ -7,7 +7,7 @@
</doc>
<settings>
<dl_settings>
<dl_settings NAME="RotWingState">
<dl_settings NAME="Parachute">
<dl_setting var="parachute.arming_method" min="0" step="1" max="2" values="OFF|AUTO|ALWAYS ON" shortname="arming"/>
</dl_settings>
</dl_settings>
conf/modules/pfc_actuators.xml
+1 -1
View File
@@ -12,7 +12,7 @@
</doc>
<settings>
<dl_settings>
<dl_settings name="Checks">
<dl_settings name="pfc_actuators">
<dl_setting var="act_start" min="0" step="1" max="1" values="OFF|START" handler="start" module="checks/pfc_actuators" type="fun"/>
</dl_settings>
</dl_settings>
conf/userconf/tudelft/conf.xml
+37 -4
View File
@@ -557,7 +557,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
@@ -568,7 +568,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
@@ -579,7 +579,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
@@ -590,7 +590,40 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3F"
ac_id="10"
airframe="airframes/tudelft/rot_wing_v3f.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3G"
ac_id="11"
airframe="airframes/tudelft/rot_wing_v3g.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHR8.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3H"
ac_id="33"
airframe="airframes/tudelft/rot_wing_v3h.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
</conf>
conf/userconf/tudelft/control_panel.xml
+185 -172
View File
@@ -66,6 +66,66 @@
<program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
</section>
<section name="sessions">
<session name="Bluegiga">
<program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM2" constant="all"/>
<arg flag="4242" constant="4252"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="NatNet3">
<arg flag="-ac" constant="1"/>
<arg flag="12"/>
<arg flag="-sm"/>
<arg flag="-f" constant="5"/>
<arg flag="-o"/>
<arg flag="-zf"/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="Bluegiga">
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="NatNet3">
<arg flag="-ac" constant="7"/>
<arg flag="114"/>
<arg flag="-sm"/>
<arg flag="-f" constant="5"/>
<arg flag="-o"/>
<arg flag="-zf"/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
<program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM2"/>
<arg flag="00:07:80:2d:e0:4b"/>
<arg flag="4242"/>
<arg flag="4252"/>
</program>
</session>
<session name="EKF Full">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
@@ -138,110 +198,6 @@
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:distance_compensated"/>
</program>
</session>
<session name="helidd">
<program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM0" constant="00:07:80:2d:d6:d9"/>
<arg flag="4242" constant="4252"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="GeniusDD"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="Flight XBeeAPI USB0 @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-transport" constant="xbee"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-layout" constant="bottom_settings.xml"/>
</program>
<program name="Messages"/>
</session>
<session name="Bluegiga">
<program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM2" constant="all"/>
<arg flag="4242" constant="4252"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="NatNet3">
<arg flag="-ac" constant="1"/>
<arg flag="12"/>
<arg flag="-sm"/>
<arg flag="-f" constant="5"/>
<arg flag="-o"/>
<arg flag="-zf"/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="Scaled Sensors">
<program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/>
<arg flag="-t" constant="ACC"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="9.81"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="-9.81"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ax:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ay:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:az:0.0009766"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+250"/>
<arg flag="-t" constant="GYRO"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gp:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gq:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gr:0.0139882"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+500"/>
<arg flag="-t" constant="MAG"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mx:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:my:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mz:0.0004883"/>
</program>
</session>
<session name="Joystick Hobbyking">
<program name="Joystick">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="PythonTools">
<program name="IridiumDialer"/>
<program name="PayloadForward"/>
<program name="SVInfo"/>
<program name="IridiumDialer"/>
</session>
<session name="Flight /dev/paparazzi/link Transparent @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/paparazzi/link"/>
@@ -264,15 +220,19 @@
<arg flag="-layout" constant="bottom_settings.xml"/>
</program>
</session>
<session name="Messages and Settings">
<program name="Messages"/>
<program name="Settings">
<arg flag="-ac" constant="@AIRCRAFT"/>
<session name="Flight ACM0 Transparent @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
</session>
<session name="Flight ttyUSB0 Transparent @57600">
<session name="Flight ACM1 Transparent @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-d" constant="/dev/ttyACM1"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
@@ -299,7 +259,7 @@
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<program name="Joystick">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
<arg flag="-d 0"/>
@@ -310,57 +270,6 @@
<arg flag="-zf"/>
</program>
</session>
<session name="Flight ACM0 Transparent @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
</session>
<session name="Flight ACM1 Transparent @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM1"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
</session>
<session name="Bluegiga">
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="NatNet3">
<arg flag="-ac" constant="7"/>
<arg flag="114"/>
<arg flag="-sm"/>
<arg flag="-f" constant="5"/>
<arg flag="-o"/>
<arg flag="-zf"/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
<program name="BluegigaUsbDongle" command="sw/tools/bluegiga_usb_dongle/bluegiga_usb_driver">
<arg flag="/dev/ttyACM2"/>
<arg flag="00:07:80:2d:e0:4b"/>
<arg flag="4242"/>
<arg flag="4252"/>
</program>
</session>
<session name="Flight UDP/WiFi">
<program name="Server"/>
<program name="GCS"/>
@@ -368,6 +277,59 @@
<arg flag="-udp"/>
</program>
</session>
<session name="Flight XBeeAPI USB0 @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-transport" constant="xbee"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-layout" constant="bottom_settings.xml"/>
</program>
<program name="Messages"/>
</session>
<session name="Flight ttyUSB0 Transparent @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
</session>
<session name="Hydrogen">
<program name="Attitude Visualizer"/>
<program name="HydrogenStatus"/>
<program name="NederdroneStatus"/>
<program name="ATC"/>
</session>
<session name="Joystick Hobbyking">
<program name="Joystick">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="Joystick X3D Pro">
<program name="Joystick">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="extreme_3d_pro.xml"/>
</program>
</session>
<session name="Messages and Settings">
<program name="Messages"/>
<program name="Settings">
<arg flag="-ac" constant="@AIRCRAFT"/>
</program>
</session>
<session name="PythonTools">
<program name="IridiumDialer"/>
<program name="PayloadForward"/>
<program name="SVInfo"/>
<program name="IridiumDialer"/>
</session>
<session name="Raw Sensors">
<program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/>
@@ -401,10 +363,33 @@
<arg flag="-c" constant="*:telemetry:BARO_RAW:abs"/>
</program>
</session>
<session name="Joystick X3D Pro">
<program name="Joystick">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="extreme_3d_pro.xml"/>
<session name="Scaled Sensors">
<program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/>
<arg flag="-t" constant="ACC"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="9.81"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="-9.81"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ax:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ay:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:az:0.0009766"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+250"/>
<arg flag="-t" constant="GYRO"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gp:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gq:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gr:0.0139882"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+500"/>
<arg flag="-t" constant="MAG"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mx:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:my:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mz:0.0004883"/>
</program>
</session>
<session name="Simulation - Gazebo">
@@ -449,18 +434,46 @@
<arg flag="-track_size" constant="200"/>
<arg flag="-zoom" constant="40."/>
</program>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<program name="Joystick">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
<arg flag="-d 0"/>
</program>
<program name="Gazebo"/>
</session>
<session name="Hydrogen">
<program name="Attitude Visualizer"/>
<program name="HydrogenStatus"/>
<program name="NederdroneStatus"/>
<program name="ATC"/>
<session name="helidd">
<program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM0" constant="00:07:80:2d:d6:d9"/>
<arg flag="4242" constant="4252"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech"/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="GeniusDD"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="Ship">
<program name="Data Link">
<arg flag="-udp"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="Settings"/>
<program name="ATC">
<arg flag="-i" constant="11"/>
<arg flag="-e" constant="udp://0.0.0.0:12001:10.19.127.1:12000"/>
<arg flag="-r"/>
</program>
</session>
</section>
</control_panel>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+21 -1
View File
@@ -282,6 +282,7 @@ Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT];
Butterworth2LowPass measurement_lowpass_filters[3];
Butterworth2LowPass estimation_output_lowpass_filters[3];
Butterworth2LowPass acceleration_lowpass_filter;
Butterworth2LowPass acceleration_body_x_filter;
#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
Butterworth2LowPass rates_filt_so[3];
#else
@@ -462,6 +463,9 @@ void init_filters(void)
init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
}
// Filtering the bodyx acceleration with same cutoff as gyroscope
init_butterworth_2_low_pass(&acceleration_body_x_filter, tau, sample_time, 0.0);
// Filtering of the accel body z
init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
@@ -520,11 +524,19 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s
/* Propagate the filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
// Get the acceleration in body axes
struct Int32Vect3 *body_accel_i;
body_accel_i = stateGetAccelBody_i();
ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
int8_t i;
for (i = 0; i < 3; i++) {
update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
update_butterworth_2_low_pass(&acceleration_body_x_filter, body_accel_f.x);
//Calculate the angular acceleration via finite difference
angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
- measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
@@ -620,6 +632,14 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s
// This term compensates for the spinup torque in the yaw axis
float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING;
if (in_flight) {
// Limit the estimated disturbance in yaw for drones that are stable in sideslip
BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
} else {
// Not in flight, so don't estimate disturbance
float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
}
// The control objective in array format
indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]);
indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]);
@@ -672,7 +692,7 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s
//update thrust command such that the current is correctly estimated
cmd[COMMAND_THRUST] = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
}
cmd[COMMAND_THRUST] /= num_thrusters;
sw/airborne/modules/ctrl/approach_moving_target.c
+2 -2
View File
@@ -201,7 +201,7 @@ void follow_diagonal_approach(void)
// just filtering the heading will go wrong if it wraps, instead do:
// take the diff with current heading_filtered, wrap that, add to current, insert in filter and wrap again?
// example: target_heading - target_heading_filt (170 - -170 = 340), -> -20 -> -190 -> into filter -> wrap
float heading_diff = target_heading - target_heading_filt.o[0];
float heading_diff = RadOfDeg(target_heading) - target_heading_filt.o[0];
FLOAT_ANGLE_NORMALIZE(heading_diff);
float target_heading_input = target_heading_filt.o[0] + heading_diff;
update_butterworth_2_low_pass(&target_heading_filt, target_heading_input);
@@ -220,7 +220,7 @@ void follow_diagonal_approach(void)
// Reference model
float gamma_ref = RadOfDeg(amt.slope_ref);
float psi_ref = RadOfDeg(target_heading_filt_wrapped + amt.psi_ref);
float psi_ref = target_heading_filt_wrapped + RadOfDeg(amt.psi_ref);
amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref);
amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref);
sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
+23 -5
View File
@@ -30,6 +30,8 @@
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "autopilot.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
@@ -144,7 +146,9 @@ struct rot_wing_eff_sched_param_t eff_sched_p = {
.k_lift_tail = ROT_WING_EFF_SCHED_K_LIFT_TAIL
};
float eff_sched_pusher_time = 0.002;
float eff_sched_pusher_time = 0.002;
float roll_eff_scaling = 1.f;
struct rot_wing_eff_sched_var_t eff_sched_var;
@@ -282,9 +286,17 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy;
float cmd_right = actuator_state_filt_vect[1];
float cmd_left = actuator_state_filt_vect[3];
Bound(cmd_right, 3500, MAX_PPRZ);
Bound(cmd_left, 3500, MAX_PPRZ);
// Roll motor effectiveness
float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[1] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[3] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_right * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_left * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
dM_dpprz_right = dM_dpprz_right * roll_eff_scaling;
dM_dpprz_left = dM_dpprz_left * roll_eff_scaling;
// Bound dM_dpprz to half and 2 times the hover effectiveness
Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
@@ -294,8 +306,10 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
Bound(roll_motor_p_eff_right, -1, -0.00001);
float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx;
roll_motor_p_eff_left += roll_motor_airspeed_compensation;
if(autopilot.in_flight) {
float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx;
roll_motor_p_eff_left += roll_motor_airspeed_compensation;
}
Bound(roll_motor_p_eff_left, 0.00001, 1);
float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy;
@@ -324,6 +338,10 @@ void eff_scheduling_rot_wing_update_elevator_effectiveness(void)
eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed +
eff_sched_p.k_elevator[2] * eff_sched_var.airspeed2) / 10000.;
// scale the effectiveness of the elevator down if it has a large deflection to encourage it to become flat quickly (pragmatic, not physically inpsired)
float elevator_ineffectiveness_scaling = (50-de)/40;
Bound(elevator_ineffectiveness_scaling, 0.5, 1.0);
float dMydpprz = dMyde * eff_sched_p.k_elevator_deflection[1];
// Convert moment to effectiveness
sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h
+2
View File
@@ -77,6 +77,8 @@ struct rot_wing_eff_sched_var_t {
float airspeed2;
};
extern float roll_eff_scaling;
extern float rotation_angle_setpoint_deg;
extern int16_t rotation_cmd;
sw/airborne/modules/rot_wing_drone/rotwing_state.c
+22 -4
View File
@@ -78,6 +78,9 @@
#endif
// FW hov mot off state identification
#ifndef ROTWING_HOV_MOT_RUN_RPM_TH
#define ROTWING_HOV_MOT_RUN_RPM_TH 800
#endif
#ifndef ROTWING_HOV_MOT_OFF_RPM_TH
#define ROTWING_HOV_MOT_OFF_RPM_TH 50
#endif
@@ -137,6 +140,7 @@ float rotwing_state_max_hover_speed = 7;
bool hover_motors_active = true;
bool bool_disable_hover_motors = false;
float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
inline void rotwing_check_set_current_state(void);
inline void rotwing_switch_state(void);
@@ -220,7 +224,7 @@ void periodic_rotwing_state(void)
bool_disable_hover_motors = false;
}
}
// Function to request a state
@@ -604,7 +608,7 @@ void rotwing_state_free_processor(void)
/*
Calculations
*/
*/
// speed over pos_error projection
struct FloatVect2 pos_error_norm;
VECT2_COPY(pos_error_norm, pos_error);
@@ -612,9 +616,9 @@ void rotwing_state_free_processor(void)
float dist_to_target = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp);
float max_speed_decel2 = fabsf(2.f * dist_to_target * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
float max_speed_decel = sqrtf(max_speed_decel2);
// Check if speed setpoint above set airspeed
struct FloatVect2 desired_airspeed_v;
struct FloatVect2 desired_airspeed_v;
struct FloatVect2 groundspeed_sp;
groundspeed_sp.x = pos_error.x * nav_hybrid_pos_gain;
groundspeed_sp.y = pos_error.y * nav_hybrid_pos_gain;
@@ -708,6 +712,18 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
}
}
bool rotwing_state_hover_motors_running(void) {
// Check if hover motors are running
if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_RUN_RPM_TH
&& rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_RUN_RPM_TH
&& rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_RUN_RPM_TH
&& rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_RUN_RPM_TH) {
return true;
} else {
return false;
}
}
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
{
// adjust weights
@@ -756,9 +772,11 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl
float pitch_angle_range = 3.;
if (rotwing_state_skewing.wing_angle_deg < 55) {
scheduled_pitch_angle = 0;
Wu_gih[1] = Wu_gih_original[1];
} else {
float pitch_progression = (rotwing_state_skewing.wing_angle_deg - 55) / 35.;
scheduled_pitch_angle = pitch_angle_range * pitch_progression;
Wu_gih[1] = Wu_gih_original[1] * (1.f - pitch_angle_range*0.9);
}
if (!hover_motors_active) {
scheduled_pitch_angle = 8.;
sw/airborne/modules/rot_wing_drone/rotwing_state.h
+1
View File
@@ -92,5 +92,6 @@ extern void periodic_rotwing_state(void);
extern void request_rotwing_state(uint8_t state);
extern void rotwing_request_configuration(uint8_t configuration);
extern void rotwing_state_skew_actuator_periodic(void);
extern bool rotwing_state_hover_motors_running(void);
#endif // ROTWING_STATE_H
sw/ground_segment/python/moving_base/moving_base.py
+39 -7
View File
@@ -52,19 +52,22 @@ class UAV:
self.timeout = 0
class Base:
def __init__(self, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False):
def __init__(self, freq=10., use_ground_ref=False, verbose=False, speed=0, heading=0, turn_rate=0, id=[]):
self.step = 1. / freq
self.use_ground_ref = use_ground_ref
self.enabled = True # run sim by default
self.verbose = verbose
self.ids = [5]
self.ids = []
self.uavs = [UAV(i) for i in self.ids]
self.time = time.mktime(time.gmtime())
self.speed = 1 # m/s
self.speed = speed # m/s
self.course = -90 # deg
self.heading = -90 # deg
self.lat = 38.08000040764657 #deg
self.lon = -9.1 #deg
self.heading = heading # deg
self.turn_rate = turn_rate #deg/s
# self.lat = 38.08000040764657 #deg
# self.lon = -9.1 #deg
self.lat = 52.926 #deg
self.lon = 4.499 #deg
self.altitude = 2.0 # starts from 1 m high
# Start IVY interface
@@ -72,6 +75,9 @@ class Base:
# bind to GPS_INT message
def ins_cb(ac_id, msg):
if ac_id not in self.ids:
self.ids.append(ac_id)
self.uavs.append(UAV(ac_id))
if ac_id in self.ids and msg.name == "GPS_INT":
uav = self.uavs[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
@@ -153,6 +159,23 @@ class Base:
msg['heading'] = self.heading
self._interface.send(msg)
msg2 = PprzMessage("ground", "FLIGHT_PARAM")
msg2['ac_id'] = "GCS"
msg2['roll'] = 0.0
msg2['pitch'] = 0.0
msg2['heading'] = self.heading
msg2['lat'] = self.lat
msg2['long'] = self.lon
msg2['speed'] = self.speed
msg2['course'] = self.course
msg2['alt'] = 0.0
msg2['climb'] = 0.0
msg2['agl'] = 0.0
msg2['unix_time'] = 0.0
msg2['itow'] = 0
msg2['airspeed'] = self.speed
self._interface.send(msg2)
def run(self):
try:
# The main loop
@@ -165,6 +188,9 @@ class Base:
# Send base position
if self.enabled:
#integrate derivatives
self.heading += self.step*self.turn_rate
self.course = self.heading
dn = self.speed*m.cos(self.course/180.0*m.pi)
de = self.speed*m.sin(self.course/180.0*m.pi)
self.move_base(self.step*dn,self.step*de)
@@ -177,7 +203,13 @@ class Base:
if __name__ == '__main__':
import argparse
parser = argparse.ArgumentParser(description="Moving base simulator")
parser.add_argument('-s', '--speed', dest='speed', default=1, type=float, help="Speed of the ship (m/s).")
parser.add_argument('-he', '--heading', dest='heading', default=0, type=float, help="Heading of the ship (deg).")
parser.add_argument('-t', '--turn_rate', dest='turn_rate', default=0, type=float, help="Turn rate of the ship (deg/s).")
parser.add_argument('-i', '--id', dest='id', default=0, type=float, help="Aircraft ID.")
args = parser.parse_args()
base = Base()
base = Base(speed=args.speed, heading=args.heading, turn_rate=args.turn_rate, id=args.id)
base.run()