Merge pull request #51 from larus-breeze/refresh_settings_from_EEPROM

realtimepeople · web-flow · commit d3398ac1d772 · 2025-05-05T07:43:24.000+02:00
Elaborate reset of all EEPROM parameters.
diff --git a/Generic_Algorithms/pt2.h b/Generic_Algorithms/pt2.h
@@ -48,7 +48,11 @@ template <class datatype, class basetype> class pt2
 	  old( datatype()),
 	  very_old( datatype())
 	{
-		basetype delta = SIN( M_PI * (DESIGN_FREQUENCY - fcutoff)) / SIN( M_PI * (fcutoff + DESIGN_FREQUENCY));
+	  tune( fcutoff);
+	}
+	void tune( basetype f_cutoff)
+	{
+		basetype delta = SIN( M_PI * (DESIGN_FREQUENCY - f_cutoff)) / SIN( M_PI * (f_cutoff + DESIGN_FREQUENCY));
 		basetype a0x = A2 * SQR(delta) - A1 + ONE;
 		basetype a1x = -2.0 * delta * A2 + (SQR(delta) + ONE) * A1 - 2.0 * delta;
 		basetype a2x = A2 - delta * A1 + SQR(delta);
diff --git a/NAV_Algorithms/AHRS.cpp b/NAV_Algorithms/AHRS.cpp
@@ -166,6 +166,13 @@ AHRS_type::AHRS_type (float sampling_time)
     compass_calibration.set_default();
 }
 
+void AHRS_type::tune( void)
+{
+  antenna_DOWN_correction = configuration( ANT_SLAVE_DOWN)  / configuration( ANT_BASELENGTH);
+  antenna_RIGHT_correction = configuration( ANT_SLAVE_RIGHT) / configuration( ANT_BASELENGTH);
+  automatic_magnetic_calibration = (magnetic_calibration_type)(round)(configuration(MAG_AUTO_CALIB));
+}
+
 void
 AHRS_type::update (const float3vector &gyro,
 		   const float3vector &acc,
diff --git a/NAV_Algorithms/AHRS.h b/NAV_Algorithms/AHRS.h
@@ -53,6 +53,8 @@ class AHRS_type
 	AHRS_type(float sampling_time);
 	void attitude_setup( const float3vector & acceleration, const float3vector & induction);
 
+	void tune( void);
+
 	void update_magnetic_induction_data( float declination, float inclination)
 	{
 	  declination *= (M_PI_F / 180.0f); // degrees to radiant
diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp
@@ -47,7 +47,7 @@ void navigator_t::update_at_100Hz (
 
   wind_observer.process_at_100_Hz( GNSS_velocity - heading_vector * TAS);
 
-  flight_observer.update_at_100Hz (
+  variometer.update_at_100Hz (
       GNSS_velocity,
       ahrs.get_nav_acceleration (),
       heading_vector,
@@ -90,12 +90,12 @@ bool navigator_t::update_at_10Hz ()
 
   wind_observer.process_at_10_Hz( ahrs);
 
-  vario_integrator.update (flight_observer.get_vario_GNSS(), // here because of the update rate 10Hz
+  vario_integrator.update (variometer.get_vario_GNSS(), // here because of the update rate 10Hz
 			   ahrs.get_euler ().yaw,
 			   ahrs.get_circling_state ());
 
   unsigned airborne_criteria_fulfilled = 0;
-  if( abs( flight_observer.get_speed_compensation_GNSS()) > AIRBORNE_TRIGGER_SPEED_COMP)
+  if( abs( variometer.get_speed_compensation_GNSS()) > AIRBORNE_TRIGGER_SPEED_COMP)
     ++ airborne_criteria_fulfilled;
 
   if( GNSS_velocity.abs() > AIRBORNE_TRIGGER_SPEED)
@@ -115,7 +115,7 @@ bool navigator_t::update_at_10Hz ()
   if( airborne_detector.is_airborne())
     atmosphere.air_density_metering(
 	air_pressure_resampler_100Hz_10Hz.get_output(),
-	flight_observer.get_filtered_GNSS_altitude());
+	variometer.get_filtered_GNSS_altitude());
 
 
   return landing_detected;
@@ -130,18 +130,18 @@ void navigator_t::report_data( output_data_t &d)
     d.euler			= ahrs.get_euler();
     d.q				= ahrs.get_attitude();
 
-    d.vario			= flight_observer.get_vario_GNSS(); // todo pick one vario
-    d.vario_pressure		= flight_observer.get_vario_pressure();
+    d.vario			= variometer.get_vario_GNSS(); // todo pick one vario
+    d.vario_pressure		= variometer.get_vario_pressure();
     d.integrator_vario		= vario_integrator.get_output();
-    d.vario_uncompensated 	= flight_observer.get_vario_uncompensated_GNSS();
+    d.vario_uncompensated 	= variometer.get_vario_uncompensated_GNSS();
 
     d.wind 			= wind_observer.get_instant_value();
     d.wind_average		= wind_observer.get_average_value();
 
-    d.speed_compensation_TAS 	= flight_observer.get_speed_compensation_IAS();
-    d.speed_compensation_GNSS 	= flight_observer.get_speed_compensation_GNSS();
+    d.speed_compensation_TAS 	= variometer.get_speed_compensation_IAS();
+    d.speed_compensation_GNSS 	= variometer.get_speed_compensation_GNSS();
     d.effective_vertical_acceleration
-				= flight_observer.get_effective_vertical_acceleration();
+				= variometer.get_effective_vertical_acceleration();
 
     d.circle_mode 		= ahrs.get_circling_state();
     d.turn_rate			= ahrs.get_turn_rate();
@@ -174,7 +174,7 @@ void navigator_t::report_data( output_data_t &d)
     d.inst_wind_corrected_N	= wind_observer.get_corrected_wind()[NORTH];
     d.inst_wind_corrected_E	= wind_observer.get_corrected_wind()[EAST];
     for( unsigned i=0; i<3; ++i)
-      d.speed_compensation[i]  	= flight_observer.get_speed_compensation(i);
+      d.speed_compensation[i]  	= variometer.get_speed_compensation(i);
     d.cross_acc_correction 	= ahrs_magnetic.get_cross_acc_correction();
     d.vario_wind_N		= wind_observer.get_speed_compensator_wind()[NORTH];
     d.vario_wind_E		= wind_observer.get_speed_compensator_wind()[EAST];
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
@@ -47,7 +47,7 @@ class navigator_t
 	 ahrs_magnetic (0.01f),
 #endif
 	 atmosphere (101325.0f),
-	 flight_observer(),
+	 variometer(),
 	 wind_observer(),
 	 airborne_detector(),
 	 air_pressure_resampler_100Hz_10Hz(0.025f), // = 2.5 Hz @ 100Hz
@@ -67,6 +67,19 @@ class navigator_t
 	 IAS_averager(1.0f / 1.0f / 100.0f)
   {};
 
+  void tune(void)
+  {
+    variometer.tune();
+    vario_integrator.tune( configuration( VARIO_INT_TC) < 0.25f
+	   ? configuration( VARIO_INT_TC) // normalized stop frequency given, old version
+	   : (FAST_SAMPLING_TIME / configuration( VARIO_INT_TC) ) ); // time-constant given, new version
+    wind_observer.tune();
+    ahrs.tune();
+#if DEVELOPMENT_ADDITIONS
+    ahrs_magnetic.tune();
+#endif
+  }
+
   void update_magnetic_induction_data( float declination, float inclination)
   {
     ahrs.update_magnetic_induction_data( declination, inclination);
@@ -118,7 +131,7 @@ class navigator_t
 
   void reset_altitude( void)
   {
-    flight_observer.reset( atmosphere.get_negative_pressure_altitude(), GNSS_negative_altitude);
+    variometer.reset( atmosphere.get_negative_pressure_altitude(), GNSS_negative_altitude);
   }
   /**
    * @brief update pitot pressure
@@ -186,18 +199,13 @@ class navigator_t
 #endif
   }
 
-  float get_IAS( void) const
-  {
-    return IAS;
-  }
-
 private:
   AHRS_type	ahrs;
 #if DEVELOPMENT_ADDITIONS
   AHRS_type	ahrs_magnetic;
 #endif
-  atmosphere_t 		atmosphere;
-  variometer_t 	flight_observer;
+  atmosphere_t 	atmosphere;
+  variometer_t 	variometer;
   wind_oberserver_t wind_observer;
   airborne_detector_t	airborne_detector;
 
diff --git a/NAV_Algorithms/organizer.h b/NAV_Algorithms/organizer.h
@@ -99,6 +99,8 @@ class organizer_t
 		  configuration (SENS_TILT_PITCH),
 		  configuration (SENS_TILT_YAW));
     q.get_rotation_matrix (sensor_mapping);
+
+    navigator.tune();
   }
 
   void update_magnetic_induction_data( double latitude, double longitude)
diff --git a/NAV_Algorithms/persistent_data.cpp b/NAV_Algorithms/persistent_data.cpp
@@ -61,6 +61,19 @@ ROM persistent_data_t PERSISTENT_DATA[]=
 
 ROM unsigned PERSISTENT_DATA_ENTRIES = sizeof(PERSISTENT_DATA) / sizeof(persistent_data_t);
 
+bool all_EEPROM_parameters_available( void)
+{
+  float dummy;
+  for( 	const persistent_data_t * parameter = PERSISTENT_DATA;
+	parameter < PERSISTENT_DATA + PERSISTENT_DATA_ENTRIES;
+	++parameter)
+    {
+      if( true == read_EEPROM_value( parameter->id, dummy)) // parameter missing
+	return false;
+    }
+  return true;
+}
+
 void ensure_EEPROM_parameter_integrity( void)
 {
   bool EEPROM_has_been_unlocked = false;
diff --git a/NAV_Algorithms/soaring_flight_averager.h b/NAV_Algorithms/soaring_flight_averager.h
@@ -51,6 +51,11 @@ template<class value_t, bool CLAMP_OUTPUT_FIRST_CIRCLE = false, bool SOFT_TAKEOF
 	  }
     };
 
+    void tune( float normalized_stop_frequency)
+    {
+	averager.tune( normalized_stop_frequency);
+    }
+
     const value_t & get_output (void) const
     {
       return present_output;
diff --git a/NAV_Algorithms/variometer.cpp b/NAV_Algorithms/variometer.cpp
@@ -1,5 +1,5 @@
 /***********************************************************************//**
- * @file		flight_observer.cpp
+ * @file		variometer.cpp
  * @brief		maintains important derived data for gliders
  * @author		Dr. Klaus Schaefer
  * @copyright 		Copyright 2021 Dr. Klaus Schaefer. All rights reserved.
@@ -29,7 +29,7 @@
 #define ONE_DIV_BY_GRAVITY_TIMES_2 0.0509684f
 #define RECIP_GRAVITY 0.1094f
 
-//! calculate instant windspeed and variometer data, update @ 100 Hz
+//! calculate variometer data, update @ 100 Hz
 void variometer_t::update_at_100Hz (
     const float3vector &gnss_velocity,
     const float3vector &ahrs_acceleration,
diff --git a/NAV_Algorithms/variometer.h b/NAV_Algorithms/variometer.h
@@ -1,5 +1,5 @@
 /***********************************************************************//**
- * @file		flight_observer.h
+ * @file		variometer.h
  * @brief		maintains important derived data for gliders
  * @author		Dr. Klaus Schaefer
  * @copyright 		Copyright 2021 Dr. Klaus Schaefer. All rights reserved.
@@ -73,6 +73,13 @@ class variometer_t
     speed_compensation_projected_4(0.0f)
   {
   };
+
+  void tune( void)
+  {
+    vario_averager_pressure.tune( FAST_SAMPLING_TIME / configuration( VARIO_TC));
+    vario_averager_GNSS.tune( FAST_SAMPLING_TIME / configuration( VARIO_TC));
+  }
+
     void update_at_100Hz
     (
 	const float3vector &gnss_velocity,
diff --git a/NAV_Algorithms/wind_observer.h b/NAV_Algorithms/wind_observer.h
@@ -55,6 +55,22 @@ class wind_oberserver_t
     wind_correction_nav()
   {}
 
+  void tune( void)
+  {
+    instant_wind_averager.tune( configuration( WIND_TC)  < 0.25f
+    ? configuration( WIND_TC)
+    : (FAST_SAMPLING_TIME / configuration( WIND_TC) ) );
+   wind_average_observer.tune( configuration( MEAN_WIND_TC) < 0.25f
+    ? configuration( MEAN_WIND_TC)
+    : (SLOW_SAMPLING_TIME / configuration( MEAN_WIND_TC) ) );
+   relative_wind_observer.tune( configuration( MEAN_WIND_TC) < 0.25f
+    ? configuration( MEAN_WIND_TC) * 10.0f
+    : (SLOW_SAMPLING_TIME / configuration( MEAN_WIND_TC) ) );
+   corrected_wind_averager.tune( configuration( MEAN_WIND_TC)  < 0.25f
+    ? configuration( MEAN_WIND_TC) * 10.0f
+    : (SLOW_SAMPLING_TIME / configuration( MEAN_WIND_TC) ) );
+  }
+
   // wind reported to the user
   float3vector get_instant_value( void) const
   {
diff --git a/Output_Formatter/CAN_output.cpp b/Output_Formatter/CAN_output.cpp
@@ -29,8 +29,6 @@
 #include "system_state.h"
 #include "CAN_gateway.h"
 
-extern uint32_t UNIQUE_ID[4]; // MPU silicon ID
-
 enum CAN_ID_SENSOR
 {
   // all values in SI-STD- (metric) units, angles in radians
@@ -88,12 +86,20 @@ void CAN_output ( const output_data_t &x, bool horizon_activated)
   CAN_send(p, 1);
 
   p.id=CAN_Id_Wind;
-  p.data_f[0] = ATAN2( - x.wind[EAST], - x.wind[NORTH]);
+  float direction = ATAN2( - x.wind[EAST], - x.wind[NORTH]);
+  // map into 0 .. 2 * pi
+  if( direction < 0)
+    direction += 2.0f * M_PI_F;
+  p.data_f[0] = direction;
   p.data_f[1] = x.wind.abs();
   CAN_send(p, 1);
 
   p.id=CAN_Id_Wind_Average;
-  p.data_f[0] = ATAN2( - x.wind_average[EAST], - x.wind_average[NORTH]);
+  direction = ATAN2( - x.wind_average[EAST], - x.wind_average[NORTH]);
+  // map into 0 .. 2 * pi
+  if( direction < 0)
+    direction += 2.0f * M_PI_F;
+  p.data_f[0] = direction;
   p.data_f[1] = x.wind_average.abs();
   CAN_send(p, 1);
 
@@ -130,21 +136,24 @@ void CAN_output ( const output_data_t &x, bool horizon_activated)
 
   p.id=CAN_Id_GPS_Lat;
   p.dlc=8;
-  p.data_d = x.c.latitude;
+  // latitude handled in degrees internally
+  p.data_d = x.c.latitude * M_PI / 180.0;
   CAN_send(p, 1);
 
   p.id=CAN_Id_GPS_Lon;
-  p.data_d = x.c.longitude;
+  // longitude handled in degrees internally
+  p.data_d = x.c.longitude * M_PI / 180.0;
   CAN_send(p, 1);
 
-  p.id=CAN_Id_GPS_Alt;		// 0x106
+  p.id=CAN_Id_GPS_Alt;
   p.dlc=8;
-  p.data_f[0] = x.c.position[DOWN] * -1.0f;
+  p.data_f[0] = - x.c.position[DOWN];
   p.data_f[1] = x.c.geo_sep_dm * 0.1f; // dm -> m
   CAN_send(p, 1);
 
   p.id=CAN_Id_GPS_Trk_Spd;
-  p.data_f[0] = x.c.heading_motion;
+  // heading handled in degrees
+  p.data_f[0] = x.c.heading_motion * M_PI_F / 180.0f;
   p.data_f[1] = x.c.speed_motion;
   CAN_send(p, 1);
 
@@ -165,6 +174,8 @@ void CAN_output ( const output_data_t &x, bool horizon_activated)
   CAN_send(p, 1);
 }
 
+extern uint32_t UNIQUE_ID[4]; // MPU silicon ID
+
 void CAN_heartbeat( void)
 {
   CANpacket p( CAN_Id_Heartbeat_Sens, 8);

Original file line number	Diff line number	Diff line change
`@@ -53,6 +53,8 @@ class AHRS_type`
`53`	`53`	`AHRS_type(float sampling_time);`
`54`	`54`	`void attitude_setup( const float3vector & acceleration, const float3vector & induction);`
`55`	`55`
	`56`	`+ void tune( void);`
	`57`	`+`
`56`	`58`	`void update_magnetic_induction_data( float declination, float inclination)`
`57`	`59`	`{`
`58`	`60`	`declination *= (M_PI_F / 180.0f); // degrees to radiant`
Original file line number	Diff line number	Diff line change
`@@ -99,6 +99,8 @@ class organizer_t`
`99`	`99`	`configuration (SENS_TILT_PITCH),`
`100`	`100`	`configuration (SENS_TILT_YAW));`
`101`	`101`	`q.get_rotation_matrix (sensor_mapping);`
	`102`	`+`
	`103`	`+ navigator.tune();`
`102`	`104`	`}`
`103`	`105`
`104`	`106`	`void update_magnetic_induction_data( double latitude, double longitude)`
Original file line number	Diff line number	Diff line change
`@@ -51,6 +51,11 @@ template<class value_t, bool CLAMP_OUTPUT_FIRST_CIRCLE = false, bool SOFT_TAKEOF`
`51`	`51`	`}`
`52`	`52`	`};`
`53`	`53`
	`54`	`+ void tune( float normalized_stop_frequency)`
	`55`	`+ {`
	`56`	`+ averager.tune( normalized_stop_frequency);`
	`57`	`+ }`
	`58`	`+`
`54`	`59`	`const value_t & get_output (void) const`
`55`	`60`	`{`
`56`	`61`	`return present_output;`