From 8b2892c0052ac9b4da597e400e490fa117c44577 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 2 Nov 2011 15:59:30 +0100
Subject: [PATCH] Trim Fixedwing Inflight: Stop abusing ins_neutral/ir_neutral

---
 conf/settings/tuning.xml                   |  5 +++++
 conf/settings/tuning_ins.xml               |  5 +++++
 sw/airborne/commands.c                     | 11 +++++++++++
 sw/airborne/commands.h                     |  2 ++
 sw/airborne/firmwares/fixedwing/main_fbw.c | 13 ++++++++++++-
 5 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml
index 8e9538720e..c430c97e34 100644
--- a/conf/settings/tuning.xml
+++ b/conf/settings/tuning.xml
@@ -31,6 +31,11 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="3000" MIN="-3000" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="3000" MIN="-3000" 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"/>
diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml
index 1f092a4a56..0a5aea0529 100644
--- a/conf/settings/tuning_ins.xml
+++ b/conf/settings/tuning_ins.xml
@@ -31,6 +31,11 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="3000" MIN="-3000" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="3000" MIN="-3000" 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"/>
diff --git a/sw/airborne/commands.c b/sw/airborne/commands.c
index c73b50081f..3dd61cfdd3 100644
--- a/sw/airborne/commands.c
+++ b/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;
diff --git a/sw/airborne/commands.h b/sw/airborne/commands.h
index a824de01c9..4716959233 100644
--- a/sw/airborne/commands.h
+++ b/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];
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 40dcf65a58..7b70a8f900 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -150,7 +150,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] += command_roll_trim;
+    #endif
+    #ifdef COMMAND_PITCH
+    trimmed_commands[COMMAND_PITCH] += command_pitch_trim;
+    #endif
+
+    SetActuatorsFromCommands(trimmed_commands);
     fbw_new_actuators = 0;
   }
 #endif