more g4 filtering for cs + second sensor added

Author: askuric

src/common/base_classes/FOCMotor.cpp

@@ -42,6 +42,7 @@ FOCMotor::FOCMotor()
   //sensor 
   sensor_offset = 0.0f;
   sensor = nullptr;
+  position_loop_sensor = nullptr;
   //current sensor 
   current_sense = nullptr;
 }
@@ -52,8 +53,19 @@ FOCMotor::FOCMotor()
 */
 void FOCMotor::linkSensor(Sensor* _sensor) {
   sensor = _sensor;
+  if (!position_loop_sensor) position_loop_sensor = _sensor; // if position sensor not linked use the same sensor for position control as well
 }
 
+
+/**
+	Sensor linking method
+*/
+void FOCMotor::linkPositionLoopSensor(Sensor* _sensor, Direction direction) {
+  position_loop_sensor = _sensor;
+  position_loop_sensor_direction = direction;
+}
+
+
 /**
 	CurrentSense linking method
 */
@@ -64,8 +76,8 @@ void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
 // shaft angle calculation
 float FOCMotor::shaftAngle() {
   // if no sensor linked return previous value ( for open loop )
-  if(!sensor) return shaft_angle;
-  return sensor_direction*sensor->getAngle() - sensor_offset;
+  if(!position_loop_sensor) return shaft_angle;
+  return position_loop_sensor_direction*position_loop_sensor->getAngle();
 }
 // shaft velocity calculation
 float FOCMotor::shaftVelocity() {
@@ -355,8 +367,8 @@ void FOCMotor::monitor() {
     DQCurrent_s c = current;
     if( current_sense && torque_controller != TorqueControlType::foc_current ){
       c = current_sense->getFOCCurrents(electrical_angle);
-      c.q = LPF_current_q(c.q);
-      c.d = LPF_current_d(c.d);
+      // c.q = LPF_current_q(c.q);
+      // c.d = LPF_current_d(c.d);
     }
     if(monitor_variables & _MON_CURR_Q) {
       if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
@@ -381,7 +393,7 @@ void FOCMotor::monitor() {
   if(monitor_variables & _MON_ANGLE) {
     if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
     else if(printed) monitor_port->print(monitor_separator);
-    monitor_port->print(shaft_angle,monitor_decimals);
+    monitor_port->print(shaft_angle, monitor_decimals);
     printed= true;
   }
   if(printed){
@@ -624,7 +636,8 @@ void FOCMotor::loopFOC() {
       // constrain current setpoint
       current_sp = _constrain(current_sp, -current_limit, current_limit)  + feed_forward_current.q; // desired current is the setpoint
       // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
-      if (_isset(KV_rating)) voltage_bemf = estimateBEMF(shaft_velocity);
+      if (KV_rating &&_isset(KV_rating)) voltage_bemf = estimateBEMF(shaft_velocity);
+      else voltage_bemf = 0;
       // filter the value values
       current.q = LPF_current_q(current_sp);
       // calculate the phase voltage
@@ -700,6 +713,9 @@ void FOCMotor::move(float new_target) {
   // a specific frequency (or almost)
   updateMotionControlTime();
 
+  // update the additional sensor values
+  if (position_loop_sensor) position_loop_sensor->update();
+
   // read value even if motor is disabled to keep the monitoring updated
   // except for the open loop modes where the values are updated within angle/velocityOpenLoop functions
 
@@ -712,7 +728,7 @@ void FOCMotor::move(float new_target) {
   if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ){
     // read the values only if the motor is not in open loop
     // because in open loop the shaft angle/velocity is updated within angle/velocityOpenLoop functions
-    shaft_angle = shaftAngle(); 
+    shaft_angle = shaftAngle() - sensor_offset;
     shaft_velocity = shaftVelocity(); 
   }
 
@@ -797,6 +813,10 @@ int  FOCMotor::initFOC() {
     exit_flag *= alignSensor();
     // added the shaft_angle update
     sensor->update();
+    //if(position_loop_sensor_direction == Direction::UNKNOWN) position_loop_sensor_direction = sensor_direction;
+    SIMPLEFOC_MOTOR_DEBUG("Sensor aligned.");
+    position_loop_sensor->update();
+    SIMPLEFOC_MOTOR_DEBUG("Sensor aligned1.");
     shaft_angle = shaftAngle();
   } else {
     SIMPLEFOC_MOTOR_DEBUG("No sensor.");

src/common/base_classes/FOCMotor.h

@@ -160,10 +160,20 @@ class FOCMotor
     /**
      * Function linking a motor and a sensor 
      * 
-     * @param sensor Sensor class  wrapper for the FOC algorihtm to read the motor angle and velocity
+     * @param sensor Sensor class  wrapper for the FOC algorithm to read the motor angle and velocity
      */
     void linkSensor(Sensor* sensor);
 
+
+
+    /**
+     * Function linking a motor and a second sensor used only for position control
+     * If not provided, the motor will use the sensor linked with the linkSensor function for position control as well
+     * 
+     * @param sensor Sensor class  wrapper for the FOC algorithm to read the motor angle and velocity
+     */
+    void linkPositionLoopSensor(Sensor* sensor, Direction direction = Direction::CW);
+
     /**
      * Function linking a motor and current sensing 
      * 
@@ -265,12 +275,15 @@ class FOCMotor
     unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad
     unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad
 
-    // sensor related variabels
+    // sensor related variables
     float sensor_offset; //!< user defined sensor zero offset
+    float position_loop_sensor_direction = Direction::UNKNOWN; //!< direction of the position loop sensor compared to the sensor linked with linkSensor function - default is 1, if set to -1 the direction will be flipped
     float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available
     Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration
     bool pp_check_result = false; //!< the result of the PP check, if run during loopFOC
 
+
+
     /**
      * Function providing BLDCMotor class with the 
      * Serial interface and enabling monitoring mode
@@ -299,6 +312,8 @@ class FOCMotor
       * - HallSensor
     */
     Sensor* sensor; 
+    Sensor* position_loop_sensor; //!< optional second sensor used only for position control - if not provided the sensor linked with linkSensor will be used for position control as well
+    
     //!< CurrentSense link
     CurrentSense* current_sense; 
 

src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp

@@ -5,6 +5,9 @@
 #include "../../../../communication/SimpleFOCDebug.h"
 
 #define SIMPLEFOC_STM32_DEBUG
+#define SIMPLEFOC_STM32G4_CS_SAMPLING_TIME ADC_SAMPLETIME_24CYCLES_5
+#define SIMPLEFOC_STM32G4_CS_OVERSAMPLING_RATIO ADC_OVERSAMPLING_RATIO_8
+#define SIMPLEFOC_STM32G4_CS_OVERSAMPLING_SHIFT ADC_RIGHTBITSHIFT_3
 
 
 // pointer to the ADC handles used in the project
@@ -105,6 +108,7 @@ int _adc_init_regular(ADC_TypeDef* adc_instance){
   hadc[adc_num].Init.DMAContinuousRequests = DISABLE;
   hadc[adc_num].Init.EOCSelection = ADC_EOC_SINGLE_CONV;
   hadc[adc_num].Init.Overrun = ADC_OVR_DATA_PRESERVED;
+  hadc[adc_num].Init.OversamplingMode = DISABLE;
   if ( HAL_ADC_Init(&hadc[adc_num]) != HAL_OK){
 #ifdef SIMPLEFOC_STM32_DEBUG
     SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
@@ -149,14 +153,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive
       sConfigInjected.InjectedNbrOfConversion++;
     }
   }
-  sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5;
+  sConfigInjected.InjectedSamplingTime = SIMPLEFOC_STM32G4_CS_SAMPLING_TIME;
   sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;  
   sConfigInjected.AutoInjectedConv = DISABLE;
   sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;
   sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
   sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE;
   sConfigInjected.InjectedOffset = 0;
-  sConfigInjected.InjecOversamplingMode = DISABLE;
+  sConfigInjected.InjecOversamplingMode = ENABLE;
+  sConfigInjected.InjecOversampling.Ratio = SIMPLEFOC_STM32G4_CS_OVERSAMPLING_RATIO;
+  sConfigInjected.InjecOversampling.RightBitShift = SIMPLEFOC_STM32G4_CS_OVERSAMPLING_SHIFT;
   sConfigInjected.QueueInjectedContext = DISABLE;
 
   // automating TRGO flag finding - hardware specific