Error_motor_disarmed & error_control_deadline_missed

Hello community.

We are using MAXON 200142 motor.
As an encoder we are using 3 built-in HALL sensors. As the motor has 8 poles we’ve got 48 cpr.

We are using POSITION_CONTROL mode. The motor executes an oscillating movement (go forward for X steps without speed limit then go backwards to the original position again without speed limit, only the current is limited), basically we are using it for doing injections.
The motor calibrates successfully and works fine for X amount of time. If a load (syringe)is attached then it works for about 80 -100 injections, without the syringe it can run into the error after 2 injections. The errors are:
Axis::ERROR_MOTOR_DISARMED;
Motor::ERROR_CONTROL_DEADLINE_MISSED;

The ERROR_CONTROL_DEADLINE_MISSED is set inside the pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) function, and I can assume that it happens because the other_axis.motor_.next_timings_valid_ were not valid in time.

What could be the reason for that? What else can I check in the software?

I have to remind that the error is always triggered when the backwards movement is started, as in the folowing traces:

Code:

configureMotor()

{
        Controller::Config_t &c = axis->controller_.config_;
	c.pos_gain = 5;
	c.vel_gain = 0.001;
	c.vel_integrator_gain = 0.1;
	c.vel_limit = 3000;
	c.vel_limit_tolerance = 2;
	c.vel_ramp_rate = 10000;
	c.setpoints_in_cpr = false;
	c.control_mode = Controller::CTRL_MODE_VELOCITY_CONTROL;


	Motor::Config_t &m = axis->motor_.config_;
	m.pre_calibrated = true;		// Calibration is not required for gimball typ
	m.pole_pairs = 8;
	m.calibration_current = 1;
	m.resistance_calib_max_voltage = 2;
	m.phase_inductance = 0;
	m.phase_resistance = 0;
	m.direction = 1;
	m.motor_type = Motor::MOTOR_TYPE_GIMBAL;
	m.current_lim = 6;
	m.current_lim_tolerance = 1.25;
	m.inverter_temp_limit_lower = 100;
	m.inverter_temp_limit_upper = 120;
	m.requested_current_range = 25;
	m.current_control_bandwidth = 100;

	Encoder::Config_t &e = axis->encoder_.config_;
	e.mode = Encoder::MODE_HALL;
	e.use_index = false;
	e.find_idx_on_lockin_only = false;
	e.pre_calibrated = false;
	e.zero_count_on_find_idx = true;
	e.cpr = 48;
	e.offset = 72;
	e.offset_float = 1.4792187213897705;
	e.enable_phase_interpolation = true;
	e.bandwidth = 100.0;
	e.calib_range = 0.02;
	e.calib_scan_distance = 50.26548385620117;
	e.calib_scan_omega = 12.566370964050293;
	e.idx_search_unidirectional = false;
	e.ignore_illegal_hall_state = false;

	board_config.dc_bus_undervoltage_trip_level = 4.0f;

	osDelay(100);

	osDelay(1000);

	m.current_lim = 2;
	m.requested_current_range = 25;
	c.vel_limit = 3000;
	m.calibration_current = 1;
	m.pole_pairs = 8;
	m.motor_type = Motor::MOTOR_TYPE_GIMBAL;
}
//Move backward funtcion
moveSlow(float target)
{
	axes[0]->requested_state_ = Axis::AXIS_STATE_CLOSED_LOOP_CONTROL;
	axes[0]->controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
	axes[0]->controller_.config_.vel_limit = 3000;
	axes[0]->motor_.config_.current_lim = 2;
	axes[0]->controller_.config_.pos_gain = 20;
	axes[0]->controller_.config_.vel_gain = 0.01;
	axes[0]->controller_.config_.vel_integrator_gain = 0.1;

	osDelay(100);

	axes[0]->controller_.move_to_pos(target);

	// Wait for motor to reach position
	float error;
	int count = 0;
	int vel;

	do
	{
		vel = axes[0]->encoder_.vel_estimate_;

		bkw_speed = vel;

		if(vel < minSpeed)
			minSpeed = vel;

		osDelay(10);
		error = axes[0]->encoder_.pos_estimate_ - target;

		bkw_error = error;

		fabs(error) > MOVE_SLOW_POS_DELTA_ERR ? count = 0 : count++;
	}
	while(count < 2);

	axes[0]->requested_state_ = Axis::AXIS_STATE_IDLE;

	bkw_speed = 0;
	bkw_error = 0;

	return axes[0]->error_ == Axis::ERROR_NONE;

}
//Move forward function
moveFast(float target)
{
	axes[0]->requested_state_ = Axis::AXIS_STATE_CLOSED_LOOP_CONTROL;
	axes[0]->controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;

	axes[0]->controller_.config_.vel_limit = 3000;
	axes[0]->motor_.config_.current_lim = 5;
	axes[0]->controller_.config_.pos_gain = 20;
	axes[0]->controller_.config_.vel_gain = 0.01;
	axes[0]->controller_.config_.vel_integrator_gain = 0.1;

	osDelay(100);

	axes[0]->controller_.move_to_pos((int)target);

	// Wait for motor to reach position
	float error;
	int count = 0;
	int vel;

	do
	{
		vel = axes[0]->encoder_.vel_estimate_;

		fw_speed = vel;

		if(vel > maxSpeed)
			maxSpeed = vel;

		osDelay(10);
		error = axes[0]->encoder_.pos_estimate_ - (int)target;

		fw_error = error;

		error = abs(error);

		if(error > MOVE_FAST_POS_DELTA_ERR)
			count = 0;
		else
			count++;

	}
	while(count < 5);

	axes[0]->requested_state_ = Axis::AXIS_STATE_IDLE;

	fw_speed = 0;
	fw_error = 0;

	return axes[0]->error_ == Axis::ERROR_NONE;
}

Some additional info. We want to use only one motor ( M0 ), that’s why we modified pwm_trig_adc_cb() and also disabled TIM8.
In this scenario what would be the correct way of driving only the M0?

According to motor timing diagram in the original ODrive TIM8 is used to control M0 in terms of (copied from timing diagram explanation):

  • 3.Trigger interrupt that will write to PWM hardware (A0 labeled corner)
  • 4.After short interrupt code execution time, we write to registers

Why it is not possible to use the same TIM1 for this propose, doing points 3. and 4. on DC_CAL of TIM1?

This is what changed inside pwm_trig_adc_cb():

This was replaced by what follows
//    Axis& axis = injected ? *axes[0] : *axes[1];
//    int axis_num = injected ? 0 : 1;
//    Axis& other_axis = injected ? *axes[1] : *axes[0];

    Axis& axis = *axes[0];
    int axis_num = 0;
    Axis& other_axis = *axes[0];

void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
#define calib_tau 0.2f  //@TOTO make more easily configurable
    static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;

    // Ensure ADCs are expected ones to simplify the logic below
    if (!(hadc == &hadc2 || hadc == &hadc3)) {
        low_level_fault(Motor::ERROR_ADC_FAILED);
        return;
    };

    // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion
    // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
    // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
    // If we are counting down, we just sampled in SVM vector 7, with zero current
//    Axis& axis = injected ? *axes[0] : *axes[1];
//    int axis_num = injected ? 0 : 1;
//    Axis& other_axis = injected ? *axes[1] : *axes[0];

    Axis& axis = *axes[0];
    int axis_num = 0;
    Axis& other_axis = *axes[0];

    bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
    bool current_meas_not_DC_CAL = !counting_down;

    // Check the timing of the sequencing
    if (current_meas_not_DC_CAL)
        axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_I);
    else
        axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_DC);

    bool update_timings = false;
    if (hadc == &hadc2) {
        if (&axis == axes[1] && counting_down)
            update_timings = true; // update timings of M0
        else if (&axis == axes[0] && !counting_down)
            update_timings = true; // update timings of M1
    }

    // Load next timings for the motor that we're not currently sampling
    if (update_timings) {
        if (!other_axis.motor_.next_timings_valid_) {
            // the motor control loop failed to update the timings in time
            // we must assume that it died and therefore float all phases
            bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_);
            if (was_armed) {
                other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED;
            }
        } else {
            other_axis.motor_.next_timings_valid_ = false;
            safety_critical_apply_motor_pwm_timings(
                other_axis.motor_, other_axis.motor_.next_timings_
            );
        }
        update_brake_current();
    }

    uint32_t ADCValue;
    if (injected) {
        ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
    } else {
        ADCValue = HAL_ADC_GetValue(hadc);
    }
    float current = axis.motor_.phase_current_from_adcval(ADCValue);

    if (current_meas_not_DC_CAL) {
        // ADC2 and ADC3 record the phB and phC currents concurrently,
        // and their interrupts should arrive on the same clock cycle.
        // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
        // Therefore we store the value from ADC2 and signal the thread that the
        // measurement is ready when we receive the ADC3 measurement

        // return or continue
        if (hadc == &hadc2) {
            axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB;
            return;
        } else {
            axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
        }
        // Prepare hall readings
        // TODO move this to inside encoder update function
        decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]);
        // Trigger axis thread
        axis.signal_current_meas();
    } else {
        // DC_CAL measurement
        if (hadc == &hadc2) {
            axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k;
        } else {
            axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
        }
    }
}

According to this changes we are applying new timings only when TIM1 is up-counting and this somehow works until the error is triggered.

I wouldn’t modify the firmware yet… can you check dump_errors(odrv0) next time you get an error? I have a sneaking suspicion it’s because of the controller overspeed error

Thanks for your response. The thing is that the firmware is modified a lot because we are using a custom PCB and only one motor. I solved the issue by enabling back the TIM8 and update the timings on a DC_CALL of the TIM8.
What I found during all this procedure is that sometimes a DC_CAL is interpreted as a real phase current measurement and the other way around, it mainly happens for ADC3 (phC) this is because this line:

bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;

is called when processing the ADC callback but we are interested to know the counting direction when the timer event is triggered, so I made it global and update it in the timer callback function. Works perfect.