Add support for normalized control surface positions and right and left ailerons

src/FDM/JSBSim/FGFCS.cpp

@@ -65,14 +65,17 @@ CLASS IMPLEMENTATION
 
 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
 {
+  int i;
   Name = "FGFCS";
 
   DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0.0;
   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
-  DaPos = DePos = DrPos = DfPos = DsbPos = DspPos = 0.0;
+  DaLPos = DaRPos = DePos = DrPos = DfPos = DsbPos = DspPos = 0.0;
   GearCmd = GearPos = 1; // default to gear down
   LeftBrake = RightBrake = CenterBrake = 0.0;
+  DoNormalize=true;
   
+  for(i=0;i<6;i++) { ToNormalize[i]=-1;}
   Debug(0);
 }
 
@@ -104,6 +107,7 @@ bool FGFCS::Run(void)
     for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
     for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
     for (i=0; i<Components.size(); i++)  Components[i]->Run();
+    if(DoNormalize) Normalize();
   } else {
   }
 
@@ -249,9 +253,14 @@ void FGFCS::SetPropAdvance(int engineNum, double setting)
 bool FGFCS::Load(FGConfigFile* AC_cfg)
 {
   string token;
+  unsigned i;
   
   Name = Name + ":" + AC_cfg->GetValue("NAME");
   if (debug_lvl > 0) cout << "    Control System Name: " << Name << endl;
+  if( AC_cfg->GetValue("NORMALIZE") == "FALSE") {
+      DoNormalize=false;
+      cout << "    Automatic Control Surface Normalization Disabled" << endl;
+  }    
   AC_cfg->GetNextConfigLine();
   while ((token = AC_cfg->GetValue()) != string("/FLIGHT_CONTROL")) {
     if (token == "COMPONENT") {
@@ -288,6 +297,31 @@ bool FGFCS::Load(FGConfigFile* AC_cfg)
       AC_cfg->GetNextConfigLine();
     }
   }
+  //collect information for normalizing control surfaces
+  
+  for(i=0;i<Components.size();i++) {
+    
+    if(Components[i]->GetType() == "AEROSURFACE_SCALE" 
+        || Components[i]->GetType() == "KINEMAT"  ) {
+      if( Components[i]->GetOutputIdx() == FG_ELEVATOR_POS ) {
+        ToNormalize[iNDe]=i;
+      } else if ( Components[i]->GetOutputIdx() == FG_LEFT_AILERON_POS 
+                      || Components[i]->GetOutputIdx() == FG_AILERON_POS ) {
+        ToNormalize[iNDaL]=i;
+      } else if ( Components[i]->GetOutputIdx() == FG_RIGHT_AILERON_POS ) {
+        ToNormalize[iNDaR]=i;
+      } else if ( Components[i]->GetOutputIdx() == FG_RUDDER_POS ) {
+        ToNormalize[iNDr]=i;
+      } else if ( Components[i]->GetOutputIdx() == FG_SPDBRAKE_POS ) {
+        ToNormalize[iNDsb]=i;
+      } else if ( Components[i]->GetOutputIdx() == FG_SPOILERS_POS ) {
+        ToNormalize[iNDsp]=i;
+      } else if ( Components[i]->GetOutputIdx() == FG_FLAPS_POS ) {
+        ToNormalize[iNDf]=i;
+      }
+    }
+  }     
+  
   return true;
 }
 
@@ -370,6 +404,32 @@ void FGFCS::AddThrottle(void)
   PropAdvance.push_back(0.0);
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGFCS::Normalize(void) {
+  if( ToNormalize[iNDe] > -1 ) 
+    DePosN = Components[ToNormalize[iNDe]]->GetOutputPct();
+  
+  if( ToNormalize[iNDaL] > -1 ) 
+    DaLPosN = Components[ToNormalize[iNDaL]]->GetOutputPct();
+  
+  if( ToNormalize[iNDaR] > -1 ) 
+    DaRPosN = Components[ToNormalize[iNDaR]]->GetOutputPct();
+  
+  if( ToNormalize[iNDr] > -1 ) 
+    DrPosN = Components[ToNormalize[iNDr]]->GetOutputPct();
+       
+  if( ToNormalize[iNDsb] > -1 ) 
+    DsbPosN = Components[ToNormalize[iNDsb]]->GetOutputPct();
+  
+  if( ToNormalize[iNDsp] > -1 ) 
+    DspPosN = Components[ToNormalize[iNDsp]]->GetOutputPct();
+  
+  if( ToNormalize[iNDf] > -1 ) 
+    DfPosN = Components[ToNormalize[iNDf]]->GetOutputPct();
+   
+}  
+    
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 //    The bitmasked value choices are as follows:
 //    unset: In this case (the default) JSBSim would only print

src/FDM/JSBSim/FGFCS.h

@@ -153,6 +153,8 @@ CLASS DOCUMENTATION
 CLASS DECLARATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
+typedef enum { iNDe=0, iNDaL, iNDaR, iNDr, iNDsb, iNDsp, iNDf } NormalizeIdx;
+
 class FGFCS : public FGModel {
 
 public:
@@ -227,30 +229,68 @@ public:
 
   /// @name Aerosurface position retrieval
   //@{
-  /** Gets the aileron position.
+  /** Gets the left aileron position.
       @return aileron position in radians */
-  inline double GetDaPos(void) { return DaPos; }
+  inline double GetDaLPos(void) { return DaLPos; }
+
+  /// @name Aerosurface position retrieval
+  //@{
+  /** Gets the normalized left aileron position.
+      @return aileron position in radians */
+  inline double GetDaLPosN(void) { return DaLPosN; }
+
+  /// @name Aerosurface position retrieval
+  //@{
+  /** Gets the right aileron position.
+      @return aileron position in radians */
+  inline double GetDaRPos(void) { return DaRPos; }
+
+  /// @name Aerosurface position retrieval
+  //@{
+  /** Gets the normalized right aileron position.
+      @return right aileron position in percent (-1..1) */
+  inline double GetDaRPosN(void) { return DaRPosN; }
 
   /** Gets the elevator position.
       @return elevator position in radians */
   inline double GetDePos(void) { return DePos; }
  
+  /** Gets the normalized elevator position.
+      @return  elevator position in percent (-1..1) */
+  inline double GetDePosN(void) { return DePosN; }
+
   /** Gets the rudder position.
       @return rudder position in radians */
   inline double GetDrPos(void) { return DrPos; }
 
+  /** Gets the normalized rudder position.
+      @return rudder position in percent (-1..1) */
+  inline double GetDrPosN(void) { return DrPosN; }
+
   /** Gets the flaps position.
       @return flaps position in radians */
   inline double GetDfPos(void) { return DfPos; }
 
+  /** Gets the normalized flaps position.
+      @return flaps position in percent (-1..1) */
+  inline double GetDfPosN(void) { return DfPosN; }
+
   /** Gets the speedbrake position.
       @return speedbrake position in radians */
   inline double GetDsbPos(void) { return DsbPos; }
 
+  /** Gets the normalized speedbrake position.
+      @return speedbrake position in percent (-1..1) */
+  inline double GetDsbPosN(void) { return DsbPosN; }
+
   /** Gets the spoiler position.
       @return spoiler position in radians */
   inline double GetDspPos(void) { return DspPos; }
   
+  /** Gets the normalized spoiler position.
+      @return spoiler position in percent (-1..1) */
+  inline double GetDspPosN(void) { return DspPosN; }
+
   /** Gets the throttle position.
       @param engine engine ID number
       @return throttle position for the given engine in percent ( 0 - 100)*/
@@ -352,30 +392,62 @@ public:
 
   /// @name Aerosurface position setting
   //@{
-  /** Sets the aileron position
+  /** Sets the left aileron position
-      @param cmd aileron position in radians*/
+      @param cmd left aileron position in radians*/
-  inline void SetDaPos(double cmd) { DaPos = cmd; }
+  inline void SetDaLPos(double cmd) { DaLPos = cmd; }
+
+  /** Sets the normalized left aileron position
+      @param cmd left aileron position in percent (-1..1)*/
+  inline void SetDaLPosN(double cmd) { DaLPosN = cmd; }
+
+  /** Sets the right aileron position
+      @param cmd right aileron position in radians*/
+  inline void SetDaRPos(double cmd) { DaRPos = cmd; }
+
+  /** Sets the normalized right aileron position
+      @param cmd right aileron position in percent (-1..1)*/
+  inline void SetDaRPosN(double cmd) { DaRPosN = cmd; }
 
   /** Sets the elevator position
       @param cmd elevator position in radians*/
   inline void SetDePos(double cmd) { DePos = cmd; }
 
+  /** Sets the normalized elevator position
+      @param cmd elevator position in percent (-1..1) */
+  inline void SetDePosN(double cmd) { DePosN = cmd; }
+
   /** Sets the rudder position
       @param cmd rudder position in radians*/
   inline void SetDrPos(double cmd) { DrPos = cmd; }
  
+  /** Sets the normalized rudder position
+      @param cmd rudder position in percent (-1..1)*/
+  inline void SetDrPosN(double cmd) { DrPosN = cmd; }
+
   /** Sets the flaps position
       @param cmd flaps position in radians*/
   inline void SetDfPos(double cmd) { DfPos = cmd; }
   
+  /** Sets the flaps position
+      @param cmd flaps position in radians*/
+  inline void SetDfPosN(double cmd) { DfPosN = cmd; }
+
   /** Sets the speedbrake position
       @param cmd speedbrake position in radians*/
   inline void SetDsbPos(double cmd) { DsbPos = cmd; }
  
+  /** Sets the normalized speedbrake position
+      @param cmd normalized speedbrake position in percent (-1..1)*/
+  inline void SetDsbPosN(double cmd) { DsbPosN = cmd; }
+
   /** Sets the spoiler position
       @param cmd spoiler position in radians*/
   inline void SetDspPos(double cmd) { DspPos = cmd; }
  
+  /** Sets the normalized spoiler position
+      @param cmd normalized spoiler position in percent (-1..1)*/
+  inline void SetDspPosN(double cmd) { DspPosN = cmd; }
+
   /** Sets the actual throttle setting for the specified engine
       @param engine engine ID number
       @param cmd throttle setting in percent (0 - 100)*/
@@ -429,7 +501,8 @@ public:
 
 private:
   double DaCmd,   DeCmd,   DrCmd,  DfCmd,  DsbCmd, DspCmd;
-  double DaPos, DePos, DrPos, DfPos, DsbPos, DspPos;
+  double DaLPos,  DaRPos,  DePos,  DrPos,  DfPos,  DsbPos,  DspPos;
+  double DaLPosN, DaRPosN, DePosN, DrPosN, DfPosN, DsbPosN, DspPosN;
   double PTrimCmd, YTrimCmd, RTrimCmd;
   vector <double> ThrottleCmd;
   vector <double> ThrottlePos;
@@ -440,7 +513,11 @@ private:
   double LeftBrake, RightBrake, CenterBrake; // Brake settings
   double GearCmd,GearPos;
   
+  bool DoNormalize;
+  void Normalize(void);
+
   vector <FGFCSComponent*> Components;
+  int ToNormalize[7];
   void Debug(int from);
 };
 

src/FDM/JSBSim/FGJSBBase.h

@@ -115,13 +115,25 @@ enum eParam {
   FG_CI2VEL,
   FG_ELEVATOR_POS,
   FG_AELEVATOR_POS,
+  FG_NELEVATOR_POS,
   FG_AILERON_POS,
   FG_AAILERON_POS,
+  FG_NAILERON_POS,
+  FG_LEFT_AILERON_POS,
+  FG_ALEFT_AILERON_POS,
+  FG_NLEFT_AILERON_POS,
+  FG_RIGHT_AILERON_POS,
+  FG_ARIGHT_AILERON_POS,
+  FG_NRIGHT_AILERON_POS,
   FG_RUDDER_POS,
   FG_ARUDDER_POS,
+  FG_NRUDDER_POS,
   FG_SPDBRAKE_POS,
+  FG_NSPDBRAKE_POS,
   FG_SPOILERS_POS,
+  FG_NSPOILERS_POS,
   FG_FLAPS_POS,
+  FG_NFLAPS_POS,
   FG_ELEVATOR_CMD,
   FG_AILERON_CMD,
   FG_RUDDER_CMD,

src/FDM/JSBSim/FGOutput.cpp

@@ -149,7 +149,8 @@ void FGOutput::DelimitedOutput(string fname)
       outstream << "Elevator Cmd, ";
       outstream << "Rudder Cmd, ";
       outstream << "Flap Cmd, ";
-      outstream << "Aileron Pos, ";
+      outstream << "Left Aileron Pos, ";
+      outstream << "Right Aileron Pos, ";
       outstream << "Elevator Pos, ";
       outstream << "Rudder Pos, ";
       outstream << "Flap Pos"; 
@@ -228,7 +229,8 @@ void FGOutput::DelimitedOutput(string fname)
     outstream << FCS->GetDeCmd() << ", ";
     outstream << FCS->GetDrCmd() << ", ";
     outstream << FCS->GetDfCmd() << ", ";
-    outstream << FCS->GetDaPos() << ", ";
+    outstream << FCS->GetDaLPos() << ", ";
+    outstream << FCS->GetDaRPos() << ", ";
     outstream << FCS->GetDePos() << ", ";
     outstream << FCS->GetDrPos() << ", ";
     outstream << FCS->GetDfPos();
@@ -346,7 +348,8 @@ void FGOutput::SocketOutput(void)
     socket->Append("M");
     socket->Append("N");
     socket->Append("Throttle Position");
-    socket->Append("Aileron Position");
+    socket->Append("Left Aileron Position");
+    socket->Append("Right Aileron Position");
     socket->Append("Elevator Position");
     socket->Append("Rudder Position");
     sFirstPass = false;
@@ -390,7 +393,8 @@ void FGOutput::SocketOutput(void)
   socket->Append(Aircraft->GetMoments(eM));
   socket->Append(Aircraft->GetMoments(eN));
   socket->Append(FCS->GetThrottlePos(0));
-  socket->Append(FCS->GetDaPos());
+  socket->Append(FCS->GetDaLPos());
+  socket->Append(FCS->GetDaRPos());
   socket->Append(FCS->GetDePos());
   socket->Append(FCS->GetDrPos());
   socket->Send();

src/FDM/JSBSim/FGState.cpp

@@ -119,13 +119,25 @@ FGState::FGState(FGFDMExec* fdex)
   RegisterVariable(FG_CI2VEL,             " CI2Vel "              ); 
   RegisterVariable(FG_ELEVATOR_POS,       " elevator_pos "        ); 
   RegisterVariable(FG_AELEVATOR_POS,      " |elevator_pos| "      ); 
+  RegisterVariable(FG_NELEVATOR_POS,      " elevator_pos_n "      ); 
   RegisterVariable(FG_AILERON_POS,        " aileron_pos "         );
   RegisterVariable(FG_AAILERON_POS,       " |aileron_pos| "       );
+  RegisterVariable(FG_NAILERON_POS,       " aileron_pos_n "       );
+  RegisterVariable(FG_LEFT_AILERON_POS,   " left_aileron_pos "    );
+  RegisterVariable(FG_ALEFT_AILERON_POS,  " |left_aileron_pos| "  );
+  RegisterVariable(FG_NLEFT_AILERON_POS,  " left_aileron_pos_n "  );
+  RegisterVariable(FG_RIGHT_AILERON_POS,  " right_aileron_pos "   );
+  RegisterVariable(FG_ARIGHT_AILERON_POS, " |right_aileron_pos| " );
+  RegisterVariable(FG_NRIGHT_AILERON_POS, " right_aileron_pos_n " );
   RegisterVariable(FG_RUDDER_POS,         " rudder_pos "          );
   RegisterVariable(FG_ARUDDER_POS,        " |rudder_pos| "        );
+  RegisterVariable(FG_NRUDDER_POS,        " rudder_pos_n "        );
   RegisterVariable(FG_SPDBRAKE_POS,       " speedbrake_pos "      );
+  RegisterVariable(FG_NSPDBRAKE_POS,      " speedbrake_pos_n "    );
   RegisterVariable(FG_SPOILERS_POS,       " spoiler_pos "         );
+  RegisterVariable(FG_NSPOILERS_POS,      " spoiler_pos_n "       );
   RegisterVariable(FG_FLAPS_POS,          " flaps_pos "           );
+  RegisterVariable(FG_NFLAPS_POS,         " flaps_pos_n "         );
   RegisterVariable(FG_GEAR_POS,           " gear_pos "            );
   RegisterVariable(FG_ELEVATOR_CMD,       " elevator_cmd "        );
   RegisterVariable(FG_AILERON_CMD,        " aileron_cmd "         );
@@ -236,20 +248,44 @@ double FGState::GetParameter(eParam val_idx) {
     return FCS->GetDePos();
   case FG_AELEVATOR_POS:
     return fabs(FCS->GetDePos());
+  case FG_NELEVATOR_POS:
+    return FCS->GetDePosN();   
   case FG_AILERON_POS:
-    return FCS->GetDaPos();
+    return FCS->GetDaLPos();
   case FG_AAILERON_POS:
-    return fabs(FCS->GetDaPos());
+    return fabs(FCS->GetDaLPos());
+  case FG_NAILERON_POS:
+    return FCS->GetDaLPosN();
+  case FG_LEFT_AILERON_POS:
+    return FCS->GetDaLPos();
+  case FG_ALEFT_AILERON_POS:
+    return fabs(FCS->GetDaLPos());
+  case FG_NLEFT_AILERON_POS:
+    return FCS->GetDaLPosN();
+  case FG_RIGHT_AILERON_POS:
+    return FCS->GetDaRPos();
+  case FG_ARIGHT_AILERON_POS:
+    return fabs(FCS->GetDaRPos());
+  case FG_NRIGHT_AILERON_POS:
+    return FCS->GetDaRPosN();
   case FG_RUDDER_POS:
     return FCS->GetDrPos();
   case FG_ARUDDER_POS:
     return fabs(FCS->GetDrPos());
+  case FG_NRUDDER_POS:
+    return FCS->GetDrPosN();
   case FG_SPDBRAKE_POS:
     return FCS->GetDsbPos();
+  case FG_NSPDBRAKE_POS:
+    return FCS->GetDsbPosN();
   case FG_SPOILERS_POS:
     return FCS->GetDspPos();
+  case FG_NSPOILERS_POS:
+    return FCS->GetDspPosN();
   case FG_FLAPS_POS:
     return FCS->GetDfPos();
+  case FG_NFLAPS_POS:
+    return FCS->GetDfPosN();
   case FG_ELEVATOR_CMD:
     return FCS->GetDeCmd();
   case FG_AILERON_CMD:
@@ -341,21 +377,51 @@ void FGState::SetParameter(eParam val_idx, double val)
   case FG_ELEVATOR_POS:
     FCS->SetDePos(val);
     break;
+  case FG_NELEVATOR_POS:
+    FCS->SetDePosN(val);
+    break;
   case FG_AILERON_POS:
-    FCS->SetDaPos(val);
+    FCS->SetDaLPos(val);
+    break;
+  case FG_NAILERON_POS:
+    FCS->SetDaLPosN(val);
+    break;
+  case FG_LEFT_AILERON_POS:
+    FCS->SetDaLPos(val);
+    break;
+  case FG_NLEFT_AILERON_POS:
+    FCS->SetDaLPosN(val);
+    break;
+  case FG_RIGHT_AILERON_POS:
+    FCS->SetDaRPos(val);
+    break;
+  case FG_NRIGHT_AILERON_POS:
+    FCS->SetDaRPosN(val);
     break;
   case FG_RUDDER_POS:
     FCS->SetDrPos(val);
     break;
+  case FG_NRUDDER_POS:
+    FCS->SetDrPosN(val);
+    break;
   case FG_SPDBRAKE_POS:
     FCS->SetDsbPos(val);
     break;
+  case FG_NSPDBRAKE_POS:
+    FCS->SetDsbPosN(val);
+    break;
   case FG_SPOILERS_POS:
     FCS->SetDspPos(val);
     break;
+  case FG_NSPOILERS_POS:
+    FCS->SetDspPosN(val);
+    break;
   case FG_FLAPS_POS:
     FCS->SetDfPos(val);
     break;
+  case FG_NFLAPS_POS:
+    FCS->SetDfPosN(val);
+    break;
   case FG_THROTTLE_POS:
     if (ActiveEngine == -1) {
       for (i=0; i<Propulsion->GetNumEngines(); i++) {