static void update_control_non_running(void)
{
// Do not change anything while the motor is starting
const enum motor_rtctl_state rtctl_state = motor_rtctl_get_state();
if (rtctl_state == MOTOR_RTCTL_STATE_STARTING) {
return;
}
// Start if necessary
const float spinup_dc = _params.dc_min_voltage / _state.input_voltage;
const bool need_start =
(_state.mode == MOTOR_CONTROL_MODE_OPENLOOP && (_state.dc_openloop_setpoint > 0)) ||
(_state.mode == MOTOR_CONTROL_MODE_RPM && (_state.rpm_setpoint > 0));
if (need_start && (_state.num_unexpected_stops < _params.num_unexpected_stops_to_latch)) {
const uint64_t timestamp = motor_rtctl_timestamp_hnsec();
_state.dc_actual = spinup_dc;
motor_rtctl_start(spinup_dc, _params.reverse);
_state.rtctl_state = motor_rtctl_get_state();
// This HACK prevents the setpoint TTL expiration in case of protracted startup
const int elapsed_ms = (motor_rtctl_timestamp_hnsec() - timestamp) / HNSEC_PER_MSEC;
_state.setpoint_ttl_ms += elapsed_ms;
lowsyslog("Motor: Startup %i ms, DC %f, mode %i\n", elapsed_ms, spinup_dc, _state.mode);
if (_state.rtctl_state == MOTOR_RTCTL_STATE_IDLE) {
handle_unexpected_stop();
}
}
}
int motor_rtctl_test_hardware(void)
{
if (motor_rtctl_get_state() != MOTOR_RTCTL_STATE_IDLE) {
return -1;
}
motor_pwm_set_freewheeling();
usleep(INITIAL_DELAY_MS * 1000);
lowsyslog("Motor: Power stage test...\n");
{
int res = test_power_stage();
if (res != 0) {
return res;
}
}
lowsyslog("Motor: Cross phase test...\n");
{
int res = test_cross_phase_conductivity();
if (res != 0) {
return res;
}
}
lowsyslog("Motor: Sensors test...\n");
{
int res = test_sensors();
if (res != 0) {
return res;
}
}
return 0;
}
bool motor_is_idle(void)
{
chMtxLock(&_mutex);
bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE;
chMtxUnlock();
return ret;
}
bool motor_is_running(void)
{
chMtxLock(&_mutex);
bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_RUNNING;
chMtxUnlock();
return ret;
}
enum motor_rtctl_forced_rotation motor_rtctl_get_forced_rotation_state(void)
{
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
return motor_forced_rotation_detector_get_state();
} else {
return MOTOR_RTCTL_FORCED_ROT_NONE;
}
}
uint32_t motor_rtctl_get_comm_period_hnsec(void)
{
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
return 0;
}
const uint32_t val = _state.comm_period;
return val;
}
void motor_beep(int frequency, int duration_msec)
{
chMtxLock(&_mutex);
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
_state.beep_frequency = frequency;
_state.beep_duration_msec = duration_msec;
chMtxUnlock();
chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS); // Wake the control thread
} else {
chMtxUnlock();
}
}
static void update_setpoint_ttl(int dt_ms)
{
const enum motor_rtctl_state rtctl_state = motor_rtctl_get_state();
if (_state.setpoint_ttl_ms <= 0 || rtctl_state != MOTOR_RTCTL_STATE_RUNNING) {
return;
}
_state.setpoint_ttl_ms -= dt_ms;
if (_state.setpoint_ttl_ms <= 0) {
stop(true);
lowsyslog("Motor: Setpoint TTL expired, stop\n");
}
}
static void update_control(uint32_t comm_period, float dt)
{
/*
* Start/stop management
*/
const enum motor_rtctl_state new_rtctl_state = motor_rtctl_get_state();
const bool just_stopped =
new_rtctl_state == MOTOR_RTCTL_STATE_IDLE &&
_state.rtctl_state != MOTOR_RTCTL_STATE_IDLE;
if (just_stopped) {
handle_unexpected_stop();
}
_state.rtctl_state = new_rtctl_state;
if (comm_period == 0 || _state.rtctl_state != MOTOR_RTCTL_STATE_RUNNING) {
update_control_non_running();
return;
}
/*
* Primary control logic; can return NAN to stop the motor
*/
float new_duty_cycle = nan("");
if (_state.mode == MOTOR_CONTROL_MODE_OPENLOOP) {
new_duty_cycle = update_control_open_loop(comm_period);
}
else if (_state.mode == MOTOR_CONTROL_MODE_RPM) {
new_duty_cycle = update_control_rpm(comm_period, dt);
}
else { assert(0); }
if (!isfinite(new_duty_cycle)) {
stop(true);
return;
}
/*
* Limiters
*/
new_duty_cycle = update_control_current_limit(new_duty_cycle);
new_duty_cycle = update_control_dc_slope(new_duty_cycle, dt);
/*
* Update
*/
_state.dc_actual = new_duty_cycle;
motor_rtctl_set_duty_cycle(_state.dc_actual);
}
static void stop(bool expected)
{
motor_rtctl_stop();
_state.limit_mask = 0;
_state.dc_actual = 0.0;
_state.dc_openloop_setpoint = 0.0;
_state.rpm_setpoint = 0;
_state.setpoint_ttl_ms = 0;
_state.rtctl_state = motor_rtctl_get_state();
if (expected) {
_state.num_unexpected_stops = 0;
} else {
_state.num_unexpected_stops++;
}
rpmctl_reset();
}
static void update_filters(float dt)
{
float voltage = 0, current = 0;
motor_rtctl_get_input_voltage_current(&voltage, ¤t);
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
// Current sensor offset calibration, corner frequency is much lower.
const float offset_tau = _params.voltage_current_lowpass_tau * 100;
_state.input_curent_offset = lowpass(_state.input_curent_offset, current, offset_tau, dt);
}
current -= _state.input_curent_offset;
_state.input_voltage = lowpass(_state.input_voltage, voltage, _params.voltage_current_lowpass_tau, dt);
_state.input_current = lowpass(_state.input_current, current, _params.voltage_current_lowpass_tau, dt);
}
int motor_rtctl_test_motor(void)
{
if (motor_rtctl_get_state() != MOTOR_RTCTL_STATE_IDLE) {
return -1;
}
const int threshold = ((1 << MOTOR_ADC_RESOLUTION) * ANALOG_TOLERANCE_PERCENT) / 100;
struct motor_adc_sample sample;
int result = 0;
enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
MOTOR_PWM_MANIP_LOW,
MOTOR_PWM_MANIP_FLOATING,
MOTOR_PWM_MANIP_FLOATING
};
motor_pwm_set_freewheeling();
/*
* Test with low level
*/
manip_cmd[0] = MOTOR_PWM_MANIP_LOW;
motor_pwm_manip(manip_cmd);
usleep(SAMPLE_DELAY_MS * 1000);
sample = motor_adc_get_last_sample();
if (sample.phase_voltage_raw[1] > threshold || sample.phase_voltage_raw[2] > threshold) {
result++;
}
/*
* Test with high level
*/
manip_cmd[0] = MOTOR_PWM_MANIP_HIGH;
motor_pwm_manip(manip_cmd);
usleep(SAMPLE_DELAY_MS * 1000);
sample = motor_adc_get_last_sample();
if (abs(sample.phase_voltage_raw[0] - sample.phase_voltage_raw[1]) > threshold) {
result++;
}
if (abs(sample.phase_voltage_raw[0] - sample.phase_voltage_raw[2]) > threshold) {
result++;
}
motor_pwm_set_freewheeling();
return result;
}
static void poll_beep(void)
{
const bool do_beep =
(_state.beep_frequency > 0) &&
(_state.beep_duration_msec > 0) &&
(motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE);
if (do_beep) {
if (_state.beep_duration_msec > MAX_BEEP_DURATION_MSEC) {
_state.beep_duration_msec = MAX_BEEP_DURATION_MSEC;
}
motor_rtctl_beep(_state.beep_frequency, _state.beep_duration_msec);
}
_state.beep_frequency = 0;
_state.beep_duration_msec = 0;
}
void motor_rtctl_get_input_voltage_current(float* out_voltage, float* out_current)
{
int volt = 0, curr = 0;
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
const struct motor_adc_sample smpl = motor_adc_get_last_sample();
volt = smpl.input_voltage;
curr = smpl.input_current;
} else {
volt = _state.input_voltage;
curr = _state.input_current;
}
if (out_voltage) {
*out_voltage = motor_adc_convert_input_voltage(volt);
}
if (out_current) {
*out_current = motor_adc_convert_input_current(curr);
}
}
