Merge pull request #23 from larus-breeze/nick_remover

Nick remover

Author: realtimepeople

NAV_Algorithms/AHRS.cpp

@@ -135,7 +135,7 @@ AHRS_type::AHRS_type (float sampling_time)
   body2nav(),
   euler(),
   slip_angle_averager( ANGLE_F_BY_FS),
-  nick_angle_averager( ANGLE_F_BY_FS),
+  pitch_angle_averager( ANGLE_F_BY_FS),
   turn_rate_averager( ANGLE_F_BY_FS),
   G_load_averager(     G_LOAD_F_BY_FS),
   compass_calibration(),
@@ -186,7 +186,7 @@ AHRS_type::update_attitude ( const float3vector &acc,
 			     const float3vector &mag)
 {
   attitude.rotate (gyro[ROLL] * Ts_div_2,
-		   gyro[NICK] * Ts_div_2,
+		   gyro[PITCH] * Ts_div_2,
 		   gyro[YAW]  * Ts_div_2);
 
   attitude.normalize ();
@@ -202,7 +202,7 @@ AHRS_type::update_attitude ( const float3vector &acc,
   turn_rate_averager.respond( nav_rotation[DOWN]);
 
   slip_angle_averager.respond( ATAN2( -acc[RIGHT], -acc[DOWN]));
-  nick_angle_averager.respond( ATAN2( +acc[FRONT], -acc[DOWN]));
+  pitch_angle_averager.respond( ATAN2( +acc[FRONT], -acc[DOWN]));
   G_load_averager.respond( acc.abs());
   magnetic_disturbance = (induction_nav_frame - expected_nav_induction).abs();
 }
@@ -408,20 +408,12 @@ void AHRS_type::write_calibration_into_EEPROM( void)
   if( !magnetic_calibration_updated)
     return;
 
-  EEPROM_initialize();
+  lock_EEPROM( false);
 
-#if 0 // todo unused, remove me some day
-  if ( automatic_earth_field_parameters)
-    {
-      float inclination=ATAN2(expected_nav_induction[DOWN], expected_nav_induction[NORTH]);
-      float declination=ATAN2(expected_nav_induction[EAST], expected_nav_induction[NORTH]);
-
-      write_EEPROM_value( (EEPROM_PARAMETER_ID)DECLINATION, declination);
-      write_EEPROM_value( (EEPROM_PARAMETER_ID)INCLINATION, inclination);
-    }
-#endif
   compass_calibration.write_into_EEPROM();
   magnetic_calibration_updated = false; // done ...
+
+  lock_EEPROM( true);
 }
 
 void AHRS_type::handle_magnetic_calibration ( char type)

NAV_Algorithms/AHRS.h

@@ -37,7 +37,7 @@
 
 extern float3vector nav_induction;
 
-enum { ROLL, NICK, YAW};
+enum { ROLL, PITCH, YAW};
 enum { FRONT, RIGHT, BOTTOM};
 enum { NORTH, EAST, DOWN};
 
@@ -155,9 +155,9 @@ class AHRS_type
   }
 
   float
-  getNickAngle () const
+  getPitchAngle () const
   {
-    return nick_angle_averager.get_output();
+    return pitch_angle_averager.get_output();
   }
 
   float get_turn_rate( void ) const
@@ -223,7 +223,7 @@ class AHRS_type
   float3matrix body2nav;
   eulerangle<ftype> euler;
   pt2<float,float> slip_angle_averager;
-  pt2<float,float> nick_angle_averager;
+  pt2<float,float> pitch_angle_averager;
   pt2<float,float> turn_rate_averager;
   pt2<float,float> G_load_averager;
   linear_least_square_fit<int64_t, float> mag_calibration_data_collector_right_turn[3];

NAV_Algorithms/data_structures.h

@@ -90,7 +90,7 @@ typedef struct
   float effective_vertical_acceleration;
   float turn_rate;
   float slip_angle;
-  float nick_angle;
+  float pitch_angle;
   float G_load;
   float pressure_altitude;
   float air_density;

NAV_Algorithms/navigator.cpp

@@ -131,7 +131,7 @@ void navigator_t::report_data( output_data_t &d)
     d.circle_mode 		= ahrs.get_circling_state();
     d.turn_rate			= ahrs.get_turn_rate();
     d.slip_angle		= ahrs.getSlipAngle();
-    d.nick_angle		= ahrs.getNickAngle();
+    d.pitch_angle		= ahrs.getPitchAngle();
     d.G_load			= ahrs.get_G_load();
     d.pressure_altitude		= - atmosphere.get_negative_altitude();
     d.magnetic_disturbance	= ahrs.getMagneticDisturbance();

NAV_Algorithms/organizer.h

@@ -52,7 +52,7 @@ class organizer_t
       {
         quaternion<float> q;
         q.from_euler (configuration (SENS_TILT_ROLL),
-  		      configuration (SENS_TILT_NICK),
+  		      configuration (SENS_TILT_PITCH),
   		      configuration (SENS_TILT_YAW));
         q.get_rotation_matrix (sensor_mapping);
       }

NAV_Algorithms/persistent_data.cpp

@@ -29,10 +29,8 @@
 
 ROM persistent_data_t PERSISTENT_DATA[]=
     {
-	{BOARD_ID, 	"Board_ID",		false, 0.0f, 0},	//! Board ID Hash to avoid board confusion
-
 	{SENS_TILT_ROLL,"SensTilt_Roll",	true,  0.0f, 0}, 	//! IMU Sensor tilt angle signed / degrees front right down frame
-	{SENS_TILT_NICK,"SensTilt_Nick",	true,  0.0f, 0}, 	//! IMU Sensor tilt angle signed
+	{SENS_TILT_PITCH,"SensTilt_Pitch",	true,  0.0f, 0}, 	//! IMU Sensor tilt angle signed
 	{SENS_TILT_YAW, "SensTilt_Yaw",		true,  0.0f, 0},  	//! IMU Sensor tilt angle signed
 
 	{PITOT_OFFSET,	"Pitot_Offset",		false,  0.0f, 0},	//! Pitot offset signed / Pa
@@ -52,7 +50,6 @@ ROM persistent_data_t PERSISTENT_DATA[]=
 	{VARIO_INT_TC,	"Vario_Int_TC",		false, 30.0f, 0},	//! Vario integrator time constant unsigned s / ( 100.0f / 65536 )
 	{WIND_TC,	"Wind_TC",		false, 5.0f, 0}, 	//! Wind fast time constant unsigned s / ( 100.0f / 65536 )
 	{MEAN_WIND_TC,	"Mean_Wind_TC",		false, 30.0f, 0},	//! Wind slow time constant unsigned s / ( 100.0f / 65536 )
-	{VETF,		"VrtclEnrgTuning",	false, 1.0f, 0},	//! Vertical Energy tuning factor s / ( 1.0f / 65536 )
 	{HORIZON,	"Horizon_active",	false, 1.0f, 0},	//! Horizon output is available
 
 	{GNSS_CONFIGURATION, "GNSS_CONFIG",	false, 1.0f, 0},	//! type of GNSS system
@@ -63,17 +60,26 @@ ROM persistent_data_t PERSISTENT_DATA[]=
 
 ROM unsigned PERSISTENT_DATA_ENTRIES = sizeof(PERSISTENT_DATA) / sizeof(persistent_data_t);
 
-bool all_EEPROM_parameters_existing( void)
+void ensure_EEPROM_parameter_integrity( void)
 {
+  bool EEPROM_has_been_unlocked = false;
   float dummy;
-  const persistent_data_t * parameter = PERSISTENT_DATA + 1; // skip BOARD_ID
+  const persistent_data_t * parameter = PERSISTENT_DATA;
   while( parameter < PERSISTENT_DATA + PERSISTENT_DATA_ENTRIES)
     {
-      if( true == read_EEPROM_value( parameter->id, dummy))
-	return false; // read error
+      if( true == read_EEPROM_value( parameter->id, dummy)) // parameter missing
+	{
+	  if( EEPROM_has_been_unlocked == false)
+	    {
+	      lock_EEPROM( false);
+	      EEPROM_has_been_unlocked = true;
+	    }
+	  (void) write_EEPROM_value( parameter->id, parameter->default_value);
+	}
       ++parameter;
     }
-  return true;
+  if( EEPROM_has_been_unlocked)
+    lock_EEPROM( true);
 }
 
 const persistent_data_t * find_parameter_from_ID( EEPROM_PARAMETER_ID id)
@@ -157,7 +163,7 @@ bool EEPROM_convert( EEPROM_PARAMETER_ID id, EEPROM_data_t & EEPROM_value, float
       break;
       break;
     case SENS_TILT_ROLL:
-    case SENS_TILT_NICK:
+    case SENS_TILT_PITCH:
     case SENS_TILT_YAW:
       if( read)
 	value = (float)(EEPROM_value.i16) / 32768.0f * M_PI_F;
@@ -182,17 +188,6 @@ bool EEPROM_convert( EEPROM_PARAMETER_ID id, EEPROM_data_t & EEPROM_value, float
       else
 	EEPROM_value.u16 = (uint16_t)(value * 655.36f);
       break;
-    case VETF:
-      if( read)
-	value = (float)(EEPROM_value.u16) / 65536.0f;
-      else
-	{
-	  unsigned uvalue = (unsigned) round( value * 65536.0f);
-	  if( uvalue >= 65536)
-	    uvalue = 65535;
-	  EEPROM_value.u16 = (uint16_t)uvalue;
-	}
-      break;
     case EEPROM_PARAMETER_ID_END:
     default:
 	value = 0.0f; // just to be sure ...

NAV_Algorithms/persistent_data.h

@@ -48,7 +48,7 @@ enum EEPROM_PARAMETER_ID
   BOARD_ID = 0,
 
   SENS_TILT_ROLL = 1,
-  SENS_TILT_NICK,
+  SENS_TILT_PITCH,
   SENS_TILT_YAW,
 
   PITOT_OFFSET=4,
@@ -68,7 +68,6 @@ enum EEPROM_PARAMETER_ID
   VARIO_INT_TC,
   WIND_TC,
   MEAN_WIND_TC,
-  VETF, // VERTICAL_ENERGY_TUNING_FACTOR
   HORIZON,
 
   GNSS_CONFIGURATION=40,
@@ -98,7 +97,7 @@ bool write_EEPROM_value( EEPROM_PARAMETER_ID id, float value);
 bool read_EEPROM_value( EEPROM_PARAMETER_ID id, float &value);
 bool lock_EEPROM( bool lockit);
 bool EEPROM_initialize( void);
-bool all_EEPROM_parameters_existing( void);
+void ensure_EEPROM_parameter_integrity(void);
 
 extern const persistent_data_t PERSISTENT_DATA[];
 extern const unsigned PERSISTENT_DATA_ENTRIES;

Output_Formatter/CAN_output.cpp

@@ -144,7 +144,7 @@ void CAN_output ( const output_data_t &x, bool horizon_activated)
 
   p.id=c_CAN_Id_Acceleration;		// 0x10b
   p.dlc=7;
-  p.data_sh[0] = (int16_t)(round(x.G_load * 1000.0f));	// G-Belastung mm/s^2 nach oben pos.
+  p.data_sh[0] = (int16_t)(round(x.G_load * 1000.0f));	// G-load mm/s^2
   p.data_sh[1] = (int16_t)(round(x.effective_vertical_acceleration * -1000.0f)); // mm/s^2
   p.data_sh[2] = (int16_t)(round(x.vario_uncompensated * -1000.0f)); // mm/s
   p.data_sb[6] = (int8_t)(x.circle_mode);
@@ -159,7 +159,7 @@ void CAN_output ( const output_data_t &x, bool horizon_activated)
   p.dlc=6;
   p.data_sh[0] = (int16_t)(round(x.slip_angle * 1000.0f));	// slip angle in radiant from body acceleration
   p.data_sh[1] = (int16_t)(round(x.turn_rate  * 1000.0f)); 	// turn rate rad/s
-  p.data_sh[2] = (int16_t)(round(x.nick_angle * 1000.0f));	// nick angle in radiant from body acceleration
+  p.data_sh[2] = (int16_t)(round(x.pitch_angle * 1000.0f));	// nick angle in radiant from body acceleration
   if( CAN_send(p, 1)) // check CAN for timeout this time
     system_state |= CAN_OUTPUT_ACTIVE;
   else

Output_Formatter/NMEA_format.cpp

@@ -312,15 +312,15 @@ void format_PLARB ( float voltage, char * &p)
 
 ROM char PLARA[]="$PLARA,";
 
-void format_PLARA ( float roll, float nick, float yaw, char * &p)
+void format_PLARA ( float roll, float pitch, float yaw, char * &p)
 {
   char * line_start = p;
   p = append_string( p, PLARA);
 
   p = to_ascii_1_decimal( round(roll * RAD_TO_DEGREE_10), p);
 
   p = append_string( p, ",");
-  p = to_ascii_1_decimal( round(nick * RAD_TO_DEGREE_10), p);
+  p = to_ascii_1_decimal( round(pitch * RAD_TO_DEGREE_10), p);
 
   if( yaw < 0.0f)
     yaw += 6.2832f;