fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

added optional CRC check for debugging Encoder SPI communication

Browse source
This commit is contained in:
0x23 2026-03-18 17:31:45 +01:00
parent 29e285dc15
commit 73fc46eb63
6 changed files with 58 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

28
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
View file

@ -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,10 +365,10 @@ 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;
return crc;
}
}

8
firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
View file

@ -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;

4
firmware/MotionControllerRP/src/hw_config.h
View file

@ -24,6 +24,10 @@ constexpr float ENCODER_MAGNET_PITCH = 3.0f; // [mm]
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 ------------------------------------------------------------------

10
firmware/MotionControllerRP/src/robot.cpp
View file

@ -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(" encoder_angle=") + std::to_string(angle) + " deg\n";
reply += std::string(", is_calibrated=") + std::to_string(joints[i]->is_calibrated);
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);

19
firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp
View file

@ -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)
@ -73,6 +78,14 @@ bool measure_calibration_data(
Constants::TWO_PI_F*40.0f, [&servo_controller]() {
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);

2
firmware/MotionControllerRP/src/version.h
View file

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