Merge pull request #75 from paparazzi/trim_fixedwing

Trim Fixedwing Inflight: Stop abusing ins_neutral/ir_neutral

conf/settings/tuning.xml

@@ -31,6 +31,11 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT"/>

conf/settings/tuning_ctl_adaptive.xml

@@ -6,6 +6,11 @@
   <dl_settings>
 
     <dl_settings NAME="control">
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>

conf/settings/tuning_ctl_new.xml

@@ -6,6 +6,11 @@
   <dl_settings>
 
     <dl_settings NAME="control">
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>

conf/settings/tuning_ins.xml

@@ -31,6 +31,11 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="ins">
         <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
         <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>

conf/settings/tuning_ins_dcm.xml

@@ -31,6 +31,11 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="ins">
         <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="subsystems/ahrs" />
         <dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="subsystems/ahrs" />

conf/settings/tuning_loiter.xml

@@ -44,6 +44,10 @@
         <dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down"           shortname="corr_down"     param="IR_CORRECTION_DOWN"/>
       </dl_settings>
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
 
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>

sw/airborne/commands.c

@@ -28,5 +28,16 @@
 
 #include "commands.h"
 
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
+pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+
 pprz_t commands[COMMANDS_NB];
 const pprz_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;

sw/airborne/commands.h

@@ -32,6 +32,8 @@
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
+extern pprz_t command_roll_trim;
+extern pprz_t command_pitch_trim;
 
 extern pprz_t commands[COMMANDS_NB];
 extern const pprz_t commands_failsafe[COMMANDS_NB];

sw/airborne/firmwares/fixedwing/main_fbw.c

@@ -145,7 +145,18 @@ void event_task_fbw( void) {
 #ifdef ACTUATORS
   if (fbw_new_actuators > 0)
   {
-    SetActuatorsFromCommands(commands);
+    pprz_t trimmed_commands[COMMANDS_NB];
+    int i;
+    for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
+
+    #ifdef COMMAND_ROLL
+    trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
+    #endif
+    #ifdef COMMAND_PITCH
+    trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
+    #endif
+
+    SetActuatorsFromCommands(trimmed_commands);
     fbw_new_actuators = 0;
   }
 #endif