Eff_scheduling update

paparazzi · Sep 9, 2023 · b908c09 · b908c09
1 parent 13f2d21
commit b908c09
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 0 deletions.
diff --git a/conf/modules/eff_scheduling_nederdrone.xml b/conf/modules/eff_scheduling_nederdrone.xml
@@ -18,6 +18,7 @@ not use this module at the same time!</description>
         <dl_setting shortname="all_act_fwd_sched" var="all_act_fwd_sched" min="0" step="1" max="1" values="OFF|ON"/>
         <dl_setting shortname="trim_elevator" var="trim_elevator" min="-5000" step="1" max="5000"/>
         <dl_setting shortname="trim_flaps" var="trim_flaps" min="-5000" step="1" max="5000"/>
+        <dl_setting shortname="pref_flaps_factor" var="trim_flaps" min="0.1" step="0.1" max="100"/>
       </dl_settings>
     </dl_settings>
   </settings>

diff --git a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
@@ -56,6 +56,10 @@ not use this module at the same time!
 float trim_elevator = INDI_SCHEDULING_TRIM_ELEVATOR;
 float trim_flaps = INDI_SCHEDULING_TRIM_FLAPS;
 
+float pref_flaps_factor = INDI_SCHEDULING_PREF_FLAPS_FACTOR;
+
+float indi_Wu_original[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
+
 bool all_act_fwd_sched = false;
 
 int32_t use_scheduling = 1;
@@ -206,4 +210,19 @@ void schdule_control_effectiveness(void) {
     sched_ratio_tip_props = pitch_offset / pitch_range_deg;
   }
   Bound(sched_ratio_tip_props, 0.0, 1.0);
+
+  if(airspeed > 15.0) {
+    uint8_t i;
+    for (i = 0; i < 4; i++) {
+      indi_Wu[i] = indi_Wu_original*pref_flaps_factor;
+    }
+    for (i = 4; i < 8; i++) {
+      indi_Wu[i] = indi_Wu_original/pref_flaps_factor;
+    }
+  } else {
+    uint8_t i;
+    for (i = 0; i < 8; i++) {
+      indi_Wu[i] = indi_Wu_original;
+    }
+  }
 }
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.h b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.h
@@ -54,4 +54,6 @@ extern bool all_act_fwd_sched;
 extern float trim_elevator;
 extern float trim_flaps;
 
+extern float pref_flaps_factor;
+
 #endif  // EFF_SCHEDULING_NEDERDRONE_H