added optional CRC check for debugging Encoder SPI communication

firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp

@@ -22,7 +22,7 @@ void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint
   sleep_ms(10);
 }
 
-MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
+MT6835Encoder::MT6835Encoder(spi_inst_t* spi, int32_t cs_pin) : spi(spi), cs_pin(cs_pin) {
   if (cs_pin >= 0) {
       gpio_init(cs_pin);
       gpio_set_dir(cs_pin, GPIO_OUT);
@@ -46,6 +46,7 @@ bool MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
 
   last_raw_angle = 0;
   abs_raw_angle = 0;
+  crc_error_count = 0;
   initialized = true;
 
   return true;
@@ -99,7 +100,9 @@ MT6835Encoder::AbsRawAngleType MT6835Encoder::read_abs_angle_raw() {
   if (check_crc) {
       if (last_crc != calc_crc(raw_angle, last_status)) {
           last_status |= MT6835_CRC_ERROR;
-          return -1.0f; // CRC error indicator
+          crc_error_count++;
+          // LOG_ERROR("chip_crc: %i - calc_crc: %i", last_crc, calc_crc(raw_angle, last_status));
+     //     return -1.0f; // CRC error indicator
       }
   }
 
@@ -122,6 +125,21 @@ uint8_t MT6835Encoder::get_status() {
     return last_status;
 }
 
+void MT6835Encoder::set_crc_enabled(bool enable) {
+  check_crc = enable;
+}
+
+bool MT6835Encoder::is_crc_enabled() {
+  return check_crc;
+}
+
+uint32_t MT6835Encoder::get_crc_error_count(bool reset) {
+  uint32_t result = crc_error_count;
+  if(reset)
+    crc_error_count = 0;
+  return result;
+}
+
 uint8_t MT6835Encoder::get_calibration_status() {
     uint8_t data[3] = {0};
     data[0] = (MT6835_OP_READ << 4) | (MT6835_REG_CAL_STATUS >> 8);
@@ -337,7 +355,7 @@ uint8_t MT6835Encoder::calc_crc(uint32_t angle, uint8_t status) {
     uint8_t crc = 0x00;
     uint8_t input;
 
-    input = angle >> 13;
+    input = (angle >> 13) & 0xFF;
     crc ^= input;
     for (int k = 8; k > 0; k--)
         crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
@@ -347,7 +365,7 @@ uint8_t MT6835Encoder::calc_crc(uint32_t angle, uint8_t status) {
     for (int k = 8; k > 0; k--)
         crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
 
-    input = ((angle << 3) & 0xFF) | (status & 0x07);
+    input = ((angle & 0x1F) << 3) | (status & 0x07);
     crc ^= input;
     for (int k = 8; k > 0; k--)
         crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;

firmware/MotionControllerRP/src/hardware/MT6835_encoder.h

@@ -142,7 +142,7 @@ class MT6835Encoder {
       static void setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz);
 
       // Constructor: pass SPI instance (spi0 or spi1), CS pin
-      MT6835Encoder(spi_inst_t *spi, uint cs_pin);
+      MT6835Encoder(spi_inst_t *spi, int32_t cs_pin);
       virtual ~MT6835Encoder();
 
       bool init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
@@ -192,6 +192,9 @@ class MT6835Encoder {
       void set_options4(MT6835Options4 opts);
 
       uint8_t get_status();
+      void set_crc_enabled(bool enable);
+      bool is_crc_enabled();
+      uint32_t get_crc_error_count(bool reset=false);
 
       uint8_t get_calibration_status();
 
@@ -203,9 +206,10 @@ class MT6835Encoder {
   private:
       bool initialized=false;
       spi_inst_t *spi;
-      uint cs_pin;
+      int32_t cs_pin;
       uint8_t last_status = 0;
       uint8_t last_crc = 0;
+      uint32_t crc_error_count = 0;
 
       int32_t last_raw_angle = 0;
       AbsRawAngleType abs_raw_angle = 0;

firmware/MotionControllerRP/src/hw_config.h

@@ -25,6 +25,10 @@ constexpr float ENCODER_MAGNET_RADIUS = 30.0f;  // [mm]
 constexpr float ENCODER_ANGLE_TO_ROTOR_ANGLE = (ENCODER_MAGNET_PITCH*2.0f) / 
                                                (ENCODER_MAGNET_RADIUS * Constants::TWO_PI_F);
 											   
+// enables error checking for encoders (slow) - useful for debugging
+// Note: some chips seem to return always a crc of 0 producing massiv false errors       
+constexpr bool ENABLE_ENCODER_CRC = false;
+
 //--- HOMING ------------------------------------------------------------------
 
 constexpr float HOMING_VELOCITY   = 1.0f;        // rad per s

firmware/MotionControllerRP/src/robot.cpp

@@ -82,6 +82,7 @@ void Robot::init() {
       PIN_MOTOR_EN, PIN_M1_PWM_A_POS, PIN_M1_PWM_A_NEG, PIN_MOTOR_PWMAB,
       PIN_MOTOR_EN, PIN_M1_PWM_B_POS, PIN_M1_PWM_B_NEG, PIN_MOTOR_PWMAB
     );
+	encoder->set_crc_enabled(ENABLE_ENCODER_CRC);
     joints[0] = new RobotJoint(encoder, motor_driver, MOTOR1_POLE_PAIRS);
   }
 
@@ -92,6 +93,7 @@ void Robot::init() {
       PIN_MOTOR_EN, PIN_M2_PWM_A_POS, PIN_M2_PWM_A_NEG, PIN_MOTOR_PWMAB,
       PIN_MOTOR_EN, PIN_M2_PWM_B_POS, PIN_M2_PWM_B_NEG, PIN_MOTOR_PWMAB
     );
+	encoder->set_crc_enabled(ENABLE_ENCODER_CRC);
     joints[1] = new RobotJoint(encoder, motor_driver, MOTOR2_POLE_PAIRS);
   }
 
@@ -102,6 +104,7 @@ void Robot::init() {
       PIN_MOTOR_EN, PIN_M3_PWM_A_POS, PIN_M3_PWM_A_NEG, PIN_MOTOR_PWMAB,
       PIN_MOTOR_EN, PIN_M3_PWM_B_POS, PIN_M3_PWM_B_NEG, PIN_MOTOR_PWMAB
     );
+	encoder->set_crc_enabled(ENABLE_ENCODER_CRC);
     joints[2] = new RobotJoint(encoder, motor_driver, MOTOR3_POLE_PAIRS);
   }
 
@@ -540,11 +543,14 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     spin_lock_unsafe_blocking(joints_spin_lock);
     for(int i=0; i<NUM_JOINTS; i++) {
       float angle = joints[i]->encoder->read_abs_angle()*Constants::RAD2DEG;
+      int32_t crc_errors = joints[i]->encoder->is_crc_enabled() ? joints[i]->encoder->get_crc_error_count(false) : -1;
 
       reply += std::string("Joint ") + std::to_string(i)+":";
       reply += std::string("  is_homed=") + std::to_string(joints[i]->is_homed);
-      reply += std::string("  is_calibrated=") + std::to_string(joints[i]->is_calibrated);
+      reply += std::string(",  is_calibrated=") + std::to_string(joints[i]->is_calibrated);
-      reply += std::string("  encoder_angle=") + std::to_string(angle) + " deg\n";
+      reply += std::string(",  encoder_angle=") + std::to_string(angle) + " deg";
+      reply += std::string(",  crc_errors=") + std::to_string(crc_errors); 
+      reply += std::string(",  enc_status=") + std::to_string(joints[i]->encoder->get_status()) + "\n"; 
     }
     spin_unlock_unsafe(joints_spin_lock);
 

firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp

@@ -13,7 +13,6 @@
 
 #include "actuator_calibration.h"
 
-
 //*** FUNCTION ***********************************************************************************/
 
 bool measure_calibration_data(
@@ -39,18 +38,24 @@ bool measure_calibration_data(
   float start_field_angle = motor_driver.get_field_angle();
   float field_angle_step = calibration_range*pole_pair_count/(sample_count-1);
 
+  auto& encoder = servo_controller.get_encoder();
+  
+  encoder.get_crc_error_count(true); // reset crc error count
+  bool was_crc_enabled = encoder.is_crc_enabled();
+  encoder.set_crc_enabled(true);
+
   auto run_measurement = [&](int sample_count, float field_angle_step, int& weak_field_measurements) {
     // Measure in increasing direction
     for (size_t i = 0; i < sample_count; ++i) {
       if(i>0)
         motor_driver.rotate_field(field_angle_step, field_velocity, nullptr);
 
-      float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
+      float encoder_angle_raw = encoder.read_abs_angle_raw();
       float field_angle = motor_driver.get_field_angle();
       float motor_pos = (field_angle-start_field_angle)/pole_pair_count;
       // TODO: read motor_pos from precise reference encoder
   
-      if(servo_controller.get_encoder().get_status() & MT6835_STATUS_WEAKFIELD)
+      if(encoder.get_status() & MT6835_STATUS_WEAKFIELD)
         weak_field_measurements++;
  
       if(print_measurements)
@@ -74,6 +79,14 @@ bool measure_calibration_data(
       servo_controller.get_encoder().read_abs_angle_raw();
   });
   
+  encoder.set_crc_enabled(was_crc_enabled);
+
+  uint32_t encorder_crc_errors = encoder.get_crc_error_count(false);
+  if(encorder_crc_errors > 0) {
+    LOG_WARNING("Data error (CRC) in %i of %i measurements", encorder_crc_errors, sample_count);
+    // return false;
+  }
+
   if(weak_field_measurements > 0) {
     LOG_ERROR("Magnetic field too weak for %i of %i measurements", weak_field_measurements, sample_count);
     return false;

firmware/MotionControllerRP/src/version.h

@@ -1 +1 @@
-static const char* FIRMWARE_VERSION = "v1.0.4";
+static const char* FIRMWARE_VERSION = "v1.0.5";