static void handle_res_packet(unsigned char *data, unsigned char len) {
COMM_RES_PACKET_ID car_res_packet;
uint8_t buffer2[256];
int32_t index;
(void)len;
car_res_packet = data[0];
data++;
len--;
switch (car_res_packet) {
case COMM_READ_VALUES:
index = 0;
buffer2[index++] = COMM_READ_VALUES;
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS1) * 10.0), &index);
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS2) * 10.0), &index);
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS3) * 10.0), &index);
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS4) * 10.0), &index);
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS5) * 10.0), &index);
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS6) * 10.0), &index);
buffer_append_int16(buffer2, (int16_t)(NTC_TEMP(ADC_IND_TEMP_PCB) * 10.0), &index);
buffer_append_int32(buffer2, (int32_t)(mcpwm_read_reset_avg_motor_current() * 100.0), &index);
buffer_append_int32(buffer2, (int32_t)(mcpwm_read_reset_avg_input_current() * 100.0), &index);
buffer_append_int16(buffer2, (int16_t)(mcpwm_get_duty_cycle_now() * 1000.0), &index);
buffer_append_int32(buffer2, (int32_t)mcpwm_get_rpm(), &index);
buffer_append_int16(buffer2, (int16_t)(GET_INPUT_VOLTAGE() * 10.0), &index);
packet_send_packet(buffer2, index);
break;
default:
break;
}
}
float mc_interface_get_rpm(void) {
float ret = 0.0;
switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_rpm();
break;
case MOTOR_TYPE_FOC:
ret = mcpwm_foc_get_rpm();
break;
default:
break;
}
return ret;
}
static void set_output(float output) {
output /= (1.0 - HYST);
if (output > HYST) {
output -= HYST;
} else if (output < -HYST) {
output += HYST;
} else {
output = 0.0;
}
const float rpm = mcpwm_get_rpm();
if (output > 0.0 && rpm > -MCPWM_MIN_RPM) {
float current = output * MCPWM_CURRENT_MAX;
// Soft RPM limit
if (rpm > RPM_MAX_2) {
current = -MCPWM_CURRENT_CONTROL_MIN;
} else if (rpm > RPM_MAX_1) {
current = utils_map(rpm, RPM_MAX_1, RPM_MAX_2, current, -MCPWM_CURRENT_CONTROL_MIN);
}
// Some low-pass filtering
static float current_p1 = 0.0;
static float current_p2 = 0.0;
current = (current + current_p1 + current_p2) / 3;
current_p2 = current_p1;
current_p1 = current;
if (fabsf(current) < MCPWM_CURRENT_CONTROL_MIN) {
current = -MCPWM_CURRENT_CONTROL_MIN;
}
mcpwm_set_current(current);
} else {
mcpwm_set_brake_current(output * MCPWM_CURRENT_MIN);
}
}
void terminal_process_string(char *str) {
enum { kMaxArgs = 64 };
int argc = 0;
char *argv[kMaxArgs];
char *p2 = strtok(str, " ");
while (p2 && argc < kMaxArgs) {
argv[argc++] = p2;
p2 = strtok(0, " ");
}
if (argc == 0) {
commands_printf("No command received\n");
return;
}
if (strcmp(argv[0], "ping") == 0) {
commands_printf("pong\n");
} else if (strcmp(argv[0], "stop") == 0) {
mcpwm_set_duty(0);
commands_printf("Motor stopped\n");
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
commands_printf("Latest injected ADC duration: %.4f ms", (double)(mcpwm_get_last_inj_adc_isr_duration() * 1000.0));
commands_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
} else if (strcmp(argv[0], "kv") == 0) {
commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
} else if (strcmp(argv[0], "mem") == 0) {
size_t n, size;
n = chHeapStatus(NULL, &size);
commands_printf("core free memory : %u bytes", chCoreStatus());
commands_printf("heap fragments : %u", n);
commands_printf("heap free total : %u bytes\n", size);
} else if (strcmp(argv[0], "threads") == 0) {
Thread *tp;
static const char *states[] = {THD_STATE_NAMES};
commands_printf(" addr stack prio refs state name time ");
commands_printf("-------------------------------------------------------------");
tp = chRegFirstThread();
do {
commands_printf("%.8lx %.8lx %4lu %4lu %9s %14s %lu",
(uint32_t)tp, (uint32_t)tp->p_ctx.r13,
(uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
states[tp->p_state], tp->p_name, (uint32_t)tp->p_time);
tp = chRegNextThread(tp);
} while (tp != NULL);
commands_printf("");
} else if (strcmp(argv[0], "fault") == 0) {
commands_printf("%s\n", mcpwm_fault_to_string(mcpwm_get_fault()));
} else if (strcmp(argv[0], "faults") == 0) {
if (fault_vec_write == 0) {
commands_printf("No faults registered since startup\n");
} else {
commands_printf("The following faults were registered since start:\n");
for (int i = 0;i < fault_vec_write;i++) {
commands_printf("Fault : %s", mcpwm_fault_to_string(fault_vec[i].fault));
commands_printf("Current : %.1f", (double)fault_vec[i].current);
commands_printf("Current filtered : %.1f", (double)fault_vec[i].current_filtered);
commands_printf("Voltage : %.2f", (double)fault_vec[i].voltage);
commands_printf("Duty : %.2f", (double)fault_vec[i].duty);
commands_printf("RPM : %.1f", (double)fault_vec[i].rpm);
commands_printf("Tacho : %d", fault_vec[i].tacho);
commands_printf("TIM PWM CNT : %d", fault_vec[i].tim_pwm_cnt);
commands_printf("TIM Samp CNT : %d", fault_vec[i].tim_samp_cnt);
commands_printf("Comm step : %d", fault_vec[i].comm_step);
commands_printf("Temperature : %.2f\n", (double)fault_vec[i].temperature);
}
}
} else if (strcmp(argv[0], "rpm") == 0) {
commands_printf("Electrical RPM: %.2f rpm\n", (double)mcpwm_get_rpm());
} else if (strcmp(argv[0], "tacho") == 0) {
commands_printf("Tachometer counts: %i\n", mcpwm_get_tachometer_value(0));
} else if (strcmp(argv[0], "tim") == 0) {
chSysLock();
volatile int t1_cnt = TIM1->CNT;
volatile int t8_cnt = TIM8->CNT;
chSysUnlock();
int duty = TIM1->CCR1;
int top = TIM1->ARR;
int voltage_samp = TIM8->CCR1;
int current1_samp = TIM1->CCR4;
int current2_samp = TIM8->CCR2;
commands_printf("Tim1 CNT: %i", t1_cnt);
commands_printf("Tim8 CNT: %u", t8_cnt);
commands_printf("Duty cycle: %u", duty);
commands_printf("Top: %u", top);
commands_printf("Voltage sample: %u", voltage_samp);
commands_printf("Current 1 sample: %u", current1_samp);
commands_printf("Current 2 sample: %u\n", current2_samp);
} else if (strcmp(argv[0], "volt") == 0) {
commands_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
} else if (strcmp(argv[0], "param_detect") == 0) {
// Use COMM_MODE_DELAY and try to figure out the motor parameters.
if (argc == 4) {
float current = -1.0;
float min_rpm = -1.0;
float low_duty = -1.0;
sscanf(argv[1], "%f", ¤t);
sscanf(argv[2], "%f", &min_rpm);
sscanf(argv[3], "%f", &low_duty);
const volatile mc_configuration *mcconf = mcpwm_get_configuration();
if (current > 0.0 && current < mcconf->l_current_max &&
min_rpm > 10.0 && min_rpm < 3000.0 &&
low_duty > 0.02 && low_duty < 0.8) {
float cycle_integrator;
float coupling_k;
if (conf_general_detect_motor_param(current, min_rpm, low_duty, &cycle_integrator, &coupling_k)) {
commands_printf("Cycle integrator limit: %.2f", (double)cycle_integrator);
commands_printf("Coupling factor: %.2f\n", (double)coupling_k);
} else {
commands_printf("Detection failed. Try again with different parameters.\n");
}
} else {
commands_printf("Invalid argument(s).\n");
}
} else {
commands_printf("This command requires three arguments.\n");
}
} else if (strcmp(argv[0], "rpm_dep") == 0) {
mc_rpm_dep_struct rpm_dep = mcpwm_get_rpm_dep();
commands_printf("Cycle int limit: %.2f", (double)rpm_dep.cycle_int_limit);
commands_printf("Cycle int limit running: %.2f", (double)rpm_dep.cycle_int_limit_running);
commands_printf("Cycle int limit max: %.2f\n", (double)rpm_dep.cycle_int_limit_max);
}
// Setters
else if (strcmp(argv[0], "set_hall_table") == 0) {
if (argc == 4) {
int dir = -1;
int fwd_add = -1;
int rev_add = -1;
sscanf(argv[1], "%i", &dir);
sscanf(argv[2], "%i", &fwd_add);
sscanf(argv[3], "%i", &rev_add);
if (dir >= 0 && fwd_add >= 0 && rev_add >= 0) {
mcpwm_init_hall_table(dir, fwd_add, rev_add);
commands_printf("New hall sensor dir: %i fwd_add %i rev_add %i\n",
dir, fwd_add, rev_add);
} else {
commands_printf("Invalid argument(s).\n");
}
} else {
commands_printf("This command requires three arguments.\n");
}
}
// The help command
else if (strcmp(argv[0], "help") == 0) {
commands_printf("Valid commands are:");
commands_printf("help");
commands_printf(" Show this help");
commands_printf("ping");
commands_printf(" Print pong here to see if the reply works");
commands_printf("stop");
commands_printf(" Stop the motor");
commands_printf("last_adc_duration");
commands_printf(" The time the latest ADC interrupt consumed");
commands_printf("kv");
commands_printf(" The calculated kv of the motor");
commands_printf("mem");
commands_printf(" Show memory usage");
commands_printf("threads");
commands_printf(" List all threads");
commands_printf("fault");
commands_printf(" Prints the current fault code");
commands_printf("faults");
commands_printf(" Prints all stored fault codes and conditions when they arrived");
commands_printf("rpm");
commands_printf(" Prints the current electrical RPM");
commands_printf("tacho");
commands_printf(" Prints tachometer value");
commands_printf("tim");
commands_printf(" Prints tim1 and tim8 settings");
commands_printf("set_hall_table [dir] [fwd_add] [rev_add]");
commands_printf(" Update the hall sensor lookup table");
commands_printf("volt");
commands_printf(" Prints different voltages");
commands_printf("param_detect [current] [min_rpm] [low_duty]");
commands_printf(" Spin up the motor in COMM_MODE_DELAY and compute its parameters.");
commands_printf(" This test should be performed without load on the motor.");
commands_printf(" Example: param_detect 5.0 600 0.06");
commands_printf("rpm_dep");
commands_printf(" Prints some rpm-dep values\n");
} else {
commands_printf("Invalid command: %s\n"
"type help to list all available commands\n", argv[0]);
}
}
/**
* Process a received buffer with commands and data.
*
* @param data
* The buffer to process.
*
* @param len
* The length of the buffer.
*/
void commands_process_packet(unsigned char *data, unsigned int len) {
if (!len) {
return;
}
COMM_PACKET_ID packet_id;
int32_t ind = 0;
uint16_t sample_len;
uint8_t decimation;
bool at_start;
mc_configuration mcconf;
app_configuration appconf;
uint16_t flash_res;
uint32_t new_app_offset;
(void)len;
packet_id = data[0];
data++;
len--;
switch (packet_id) {
case COMM_FW_VERSION:
ind = 0;
send_buffer[ind++] = COMM_FW_VERSION;
send_buffer[ind++] = FW_VERSION_MAJOR;
send_buffer[ind++] = FW_VERSION_MINOR;
commands_send_packet(send_buffer, ind);
break;
case COMM_JUMP_TO_BOOTLOADER:
flash_helper_jump_to_bootloader();
break;
case COMM_ERASE_NEW_APP:
ind = 0;
flash_res = flash_helper_erase_new_app(buffer_get_uint32(data, &ind));
ind = 0;
send_buffer[ind++] = COMM_ERASE_NEW_APP;
send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
commands_send_packet(send_buffer, ind);
break;
case COMM_WRITE_NEW_APP_DATA:
ind = 0;
new_app_offset = buffer_get_uint32(data, &ind);
flash_res = flash_helper_write_new_app_data(new_app_offset, data + ind, len - ind);
ind = 0;
send_buffer[ind++] = COMM_WRITE_NEW_APP_DATA;
send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_VALUES:
ind = 0;
send_buffer[ind++] = COMM_GET_VALUES;
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS1) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS2) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS3) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS4) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS5) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS6) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_PCB) * 10.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_read_reset_avg_motor_current() * 100.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_read_reset_avg_input_current() * 100.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(mcpwm_get_duty_cycle_now() * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)mcpwm_get_rpm(), &ind);
buffer_append_int16(send_buffer, (int16_t)(GET_INPUT_VOLTAGE() * 10.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_amp_hours(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_amp_hours_charged(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_watt_hours(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_watt_hours_charged(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, mcpwm_get_tachometer_value(false), &ind);
buffer_append_int32(send_buffer, mcpwm_get_tachometer_abs_value(false), &ind);
send_buffer[ind++] = mcpwm_get_fault();
commands_send_packet(send_buffer, ind);
break;
case COMM_SET_DUTY:
ind = 0;
mcpwm_set_duty((float)buffer_get_int32(data, &ind) / 100000.0);
timeout_reset();
break;
case COMM_SET_CURRENT:
ind = 0;
mcpwm_set_current((float)buffer_get_int32(data, &ind) / 1000.0);
timeout_reset();
break;
case COMM_SET_CURRENT_BRAKE:
ind = 0;
mcpwm_set_brake_current((float)buffer_get_int32(data, &ind) / 1000.0);
timeout_reset();
break;
case COMM_SET_RPM:
ind = 0;
mcpwm_set_pid_speed((float)buffer_get_int32(data, &ind));
timeout_reset();
break;
case COMM_SET_POS:
ind = 0;
mcpwm_set_pid_pos((float)buffer_get_int32(data, &ind) / 1000000.0);
timeout_reset();
break;
case COMM_SET_DETECT:
mcpwm_set_detect();
timeout_reset();
break;
case COMM_SET_SERVO_OFFSET:
servos[0].offset = data[0];
break;
case COMM_SET_MCCONF:
mcconf = *mcpwm_get_configuration();
ind = 0;
mcconf.pwm_mode = data[ind++];
mcconf.comm_mode = data[ind++];
mcconf.motor_type = data[ind++];
mcconf.sensor_mode = data[ind++];
mcconf.l_current_max = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_current_min = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_in_current_max = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_in_current_min = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_abs_current_max = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_erpm_fbrake = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_erpm_fbrake_cc = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_min_vin = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_vin = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_slow_abs_current = data[ind++];
mcconf.l_rpm_lim_neg_torque = data[ind++];
mcconf.l_temp_fet_start = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_temp_fet_end = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_temp_motor_start = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_temp_motor_end = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_min_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.l_max_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.lo_current_max = mcconf.l_current_max;
mcconf.lo_current_min = mcconf.l_current_min;
mcconf.lo_in_current_max = mcconf.l_in_current_max;
mcconf.lo_in_current_min = mcconf.l_in_current_min;
mcconf.sl_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_min_erpm_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_max_fullbreak_current_dir_change = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_phase_advance_at_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_rpm_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
memcpy(mcconf.hall_table, data + ind, 8);
ind += 8;
mcconf.hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_min_rpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.p_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_ramp_step_max = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
conf_general_store_mc_configuration(&mcconf);
mcpwm_set_configuration(&mcconf);
ind = 0;
send_buffer[ind++] = COMM_SET_MCCONF;
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_MCCONF:
mcconf = *mcpwm_get_configuration();
ind = 0;
send_buffer[ind++] = COMM_GET_MCCONF;
send_buffer[ind++] = mcconf.pwm_mode;
send_buffer[ind++] = mcconf.comm_mode;
send_buffer[ind++] = mcconf.motor_type;
send_buffer[ind++] = mcconf.sensor_mode;
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_current_min * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_in_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_in_current_min * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_abs_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm_fbrake * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm_fbrake_cc * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_vin * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_vin * 1000.0), &ind);
send_buffer[ind++] = mcconf.l_slow_abs_current;
send_buffer[ind++] = mcconf.l_rpm_lim_neg_torque;
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_fet_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_fet_end * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_motor_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_temp_motor_end * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_min_erpm_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_max_fullbreak_current_dir_change * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_phase_advance_at_br * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_rpm_br * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_bemf_coupling_k * 1000.0), &ind);
memcpy(send_buffer + ind, mcconf.hall_table, 8);
ind += 8;
buffer_append_int32(send_buffer, (int32_t)(mcconf.hall_sl_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_min_rpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kd * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_gain * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_ramp_step_max * 1000000.0), &ind);
buffer_append_int32(send_buffer, mcconf.m_fault_stop_time_ms, &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_SET_APPCONF:
appconf = *app_get_configuration();
ind = 0;
appconf.controller_id = data[ind++];
appconf.timeout_msec = buffer_get_uint32(data, &ind);
appconf.timeout_brake_current = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.send_can_status = data[ind++];
appconf.send_can_status_rate_hz = buffer_get_uint16(data, &ind);
appconf.app_to_use = data[ind++];
appconf.app_ppm_conf.ctrl_type = data[ind++];
appconf.app_ppm_conf.pid_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.pulse_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.pulse_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.median_filter = data[ind++];
appconf.app_ppm_conf.safe_start = data[ind++];
appconf.app_ppm_conf.rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_conf.multi_esc = data[ind++];
appconf.app_ppm_conf.tc = data[ind++];
appconf.app_ppm_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.ctrl_type = data[ind++];
appconf.app_adc_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.voltage_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.voltage_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.use_filter = data[ind++];
appconf.app_adc_conf.safe_start = data[ind++];
appconf.app_adc_conf.button_inverted = data[ind++];
appconf.app_adc_conf.voltage_inverted = data[ind++];
appconf.app_adc_conf.rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.multi_esc = data[ind++];
appconf.app_adc_conf.tc = data[ind++];
appconf.app_adc_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.update_rate_hz = buffer_get_uint16(data, &ind);
appconf.app_uart_baudrate = buffer_get_uint32(data, &ind);
appconf.app_chuk_conf.ctrl_type = data[ind++];
appconf.app_chuk_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_conf.rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_conf.rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_conf.ramp_time_pos = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_conf.ramp_time_neg = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_conf.multi_esc = data[ind++];
appconf.app_chuk_conf.tc = data[ind++];
appconf.app_chuk_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
conf_general_store_app_configuration(&appconf);
app_set_configuration(&appconf);
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
ind = 0;
send_buffer[ind++] = COMM_SET_APPCONF;
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_APPCONF:
appconf = *app_get_configuration();
ind = 0;
send_buffer[ind++] = COMM_GET_APPCONF;
send_buffer[ind++] = appconf.controller_id;
buffer_append_uint32(send_buffer, appconf.timeout_msec, &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.timeout_brake_current * 1000.0), &ind);
send_buffer[ind++] = appconf.send_can_status;
buffer_append_uint16(send_buffer, appconf.send_can_status_rate_hz, &ind);
send_buffer[ind++] = appconf.app_to_use;
send_buffer[ind++] = appconf.app_ppm_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pid_max_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pulse_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pulse_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_ppm_conf.median_filter;
send_buffer[ind++] = appconf.app_ppm_conf.safe_start;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.rpm_lim_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_ppm_conf.multi_esc;
send_buffer[ind++] = appconf.app_ppm_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.tc_max_diff * 1000.0), &ind);
send_buffer[ind++] = appconf.app_adc_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_adc_conf.use_filter;
send_buffer[ind++] = appconf.app_adc_conf.safe_start;
send_buffer[ind++] = appconf.app_adc_conf.button_inverted;
send_buffer[ind++] = appconf.app_adc_conf.voltage_inverted;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.rpm_lim_end * 1000.0), &ind);
send_buffer[ind++] = appconf.app_adc_conf.multi_esc;
send_buffer[ind++] = appconf.app_adc_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.tc_max_diff * 1000.0), &ind);
buffer_append_uint16(send_buffer, appconf.app_adc_conf.update_rate_hz, &ind);
buffer_append_uint32(send_buffer, appconf.app_uart_baudrate, &ind);
send_buffer[ind++] = appconf.app_chuk_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.rpm_lim_end * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.ramp_time_pos * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.ramp_time_neg * 1000.0), &ind);
send_buffer[ind++] = appconf.app_chuk_conf.multi_esc;
send_buffer[ind++] = appconf.app_chuk_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.tc_max_diff * 1000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_SAMPLE_PRINT:
ind = 0;
at_start = data[ind++];
sample_len = buffer_get_uint16(data, &ind);
decimation = data[ind++];
main_sample_print_data(at_start, sample_len, decimation);
break;
case COMM_TERMINAL_CMD:
data[len] = '\0';
terminal_process_string((char*)data);
break;
case COMM_DETECT_MOTOR_PARAM:
ind = 0;
detect_current = (float)buffer_get_int32(data, &ind) / 1000.0;
detect_min_rpm = (float)buffer_get_int32(data, &ind) / 1000.0;
detect_low_duty = (float)buffer_get_int32(data, &ind) / 1000.0;
chEvtSignal(detect_tp, (eventmask_t) 1);
break;
case COMM_REBOOT:
// Lock the system and enter an infinite loop. The watchdog will reboot.
__disable_irq();
for(;;){};
break;
case COMM_ALIVE:
timeout_reset();
break;
case COMM_GET_DECODED_PPM:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_PPM;
buffer_append_int32(send_buffer, (int32_t)(servodec_get_servo(0) * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(servodec_get_last_pulse_len(0) * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_DECODED_ADC:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_ADC;
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level() * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage() * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_GET_DECODED_CHUK:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_CHUK;
buffer_append_int32(send_buffer, (int32_t)(app_nunchuk_get_decoded_chuk() * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
case COMM_FORWARD_CAN:
comm_can_send_buffer(data[0], data + 1, len - 1, false);
break;
default:
break;
}
}
static msg_t adc_thread(void *arg) {
(void)arg;
chRegSetThreadName("APP_ADC");
// Set servo pin as an input with pullup
palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT_PULLUP);
for(;;) {
// Sleep for a time according to the specified rate
systime_t sleep_time = CH_FREQUENCY / config.update_rate_hz;
// At least one tick should be slept to not block the other threads
if (sleep_time == 0) {
sleep_time = 1;
}
chThdSleep(sleep_time);
// Read the external ADC pin and convert the value to a voltage.
float pwr = (float)ADC_Value[ADC_IND_EXT];
pwr /= 4095;
pwr *= V_REG;
read_voltage = pwr;
// Optionally apply a mean value filter
if (config.use_filter) {
static float filter_buffer[FILTER_SAMPLES];
static int filter_ptr = 0;
filter_buffer[filter_ptr++] = pwr;
if (filter_ptr >= FILTER_SAMPLES) {
filter_ptr = 0;
}
pwr = 0.0;
for (int i = 0; i < FILTER_SAMPLES; i++) {
pwr += filter_buffer[i];
}
pwr /= FILTER_SAMPLES;
}
// Map and truncate the read voltage
pwr = utils_map(pwr, config.voltage_start, config.voltage_end, 0.0, 1.0);
utils_truncate_number(&pwr, 0.0, 1.0);
// Optionally invert the read voltage
if (config.voltage_inverted) {
pwr = 1.0 - pwr;
}
decoded_level = pwr;
// Read the servo pin and optionally invert it.
bool button_val = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
if (config.button_inverted) {
button_val = !button_val;
}
switch (config.ctrl_type) {
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
// Scale the voltage and set 0 at the center
pwr *= 2.0;
pwr -= 1.0;
break;
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
// Invert the voltage if the button is pressed
if (button_val) {
pwr = -pwr;
}
break;
default:
break;
}
// Apply a deadband
utils_deadband(&pwr, config.hyst, 1.0);
float current = 0;
bool current_mode = false;
bool current_mode_brake = false;
const volatile mc_configuration *mcconf = mcpwm_get_configuration();
bool send_duty = false;
// Use the filtered and mapped voltage for control according to the configuration.
switch (config.ctrl_type) {
case ADC_CTRL_TYPE_CURRENT:
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
current_mode = true;
if (pwr >= 0.0) {
current = pwr * mcconf->l_current_max;
} else {
current = pwr * fabsf(mcconf->l_current_min);
}
if (fabsf(pwr) < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
}
break;
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
current_mode = true;
if (pwr >= 0.0) {
current = pwr * mcconf->l_current_max;
} else {
current = fabsf(pwr * mcconf->l_current_min);
current_mode_brake = true;
}
if (pwr < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
}
break;
case ADC_CTRL_TYPE_DUTY:
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
if (fabsf(pwr) < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
}
if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
mcpwm_set_duty(pwr);
send_duty = true;
}
break;
default:
continue;
}
// If safe start is enabled and the output has not been zero for long enough
if (ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
if (ms_without_power == pulses_without_power_before) {
ms_without_power = 0;
}
pulses_without_power_before = ms_without_power;
mcpwm_set_brake_current(timeout_get_brake_current());
continue;
}
// Reset timeout
timeout_reset();
// Find lowest RPM (for traction control)
float rpm_local = mcpwm_get_rpm();
float rpm_lowest = rpm_local;
if (config.multi_esc) {
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
float rpm_tmp = msg->rpm;
if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
rpm_lowest = rpm_tmp;
}
}
}
}
// Optionally send the duty cycles to the other ESCs seen on the CAN-bus
if (send_duty && config.multi_esc) {
float duty = mcpwm_get_duty_cycle_now();
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_duty(msg->id, duty);
}
}
}
if (current_mode) {
if (current_mode_brake) {
mcpwm_set_brake_current(current);
// Send brake command to all ESCs seen recently on the CAN bus
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake(msg->id, current);
}
}
} else {
// Apply soft RPM limit
if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
current = mcconf->cc_min_current;
} else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
} else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
current = mcconf->cc_min_current;
} else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
rpm_lowest = -rpm_lowest;
current = -current;
current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
current = -current;
rpm_lowest = -rpm_lowest;
}
float current_out = current;
bool is_reverse = false;
if (current_out < 0.0) {
is_reverse = true;
current_out = -current_out;
current = -current;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
}
// Traction control
if (config.multi_esc) {
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
if (config.tc) {
float rpm_tmp = msg->rpm;
if (is_reverse) {
rpm_tmp = -rpm_tmp;
}
float diff = rpm_tmp - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
if (current_out < mcconf->cc_min_current) {
current_out = 0.0;
}
}
if (is_reverse) {
comm_can_set_current(msg->id, -current_out);
} else {
comm_can_set_current(msg->id, current_out);
}
}
}
if (config.tc) {
float diff = rpm_local - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
if (current_out < mcconf->cc_min_current) {
current_out = 0.0;
}
}
}
if (is_reverse) {
mcpwm_set_current(-current_out);
} else {
mcpwm_set_current(current_out);
}
}
}
}
return 0;
}
void terminal_process_string(char *str) {
enum { kMaxArgs = 64 };
int argc = 0;
char *argv[kMaxArgs];
static char buffer[256];
char *p2 = strtok(str, " ");
while (p2 && argc < kMaxArgs) {
argv[argc++] = p2;
p2 = strtok(0, " ");
}
if (argc == 0) {
comm_print("No command received\n");
return;
}
if (strcmp(argv[0], "ping") == 0) {
comm_print("pong\n");
} else if (strcmp(argv[0], "stop") == 0) {
mcpwm_set_duty(0);
comm_print("Motor stopped\n");
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
sprintf(buffer, "Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
comm_print(buffer);
sprintf(buffer, "Latest injected ADC duration: %.4f ms", (double)(mcpwm_get_last_inj_adc_isr_duration() * 1000.0));
comm_print(buffer);
sprintf(buffer, "Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
comm_print(buffer);
} else if (strcmp(argv[0], "kv") == 0) {
sprintf(buffer, "Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
comm_print(buffer);
} else if (strcmp(argv[0], "mem") == 0) {
size_t n, size;
n = chHeapStatus(NULL, &size);
sprintf(buffer, "core free memory : %u bytes", chCoreStatus());
comm_print(buffer);
sprintf(buffer, "heap fragments : %u", n);
comm_print(buffer);
sprintf(buffer, "heap free total : %u bytes\n", size);
comm_print(buffer);
} else if (strcmp(argv[0], "threads") == 0) {
Thread *tp;
static const char *states[] = {THD_STATE_NAMES};
comm_print(" addr stack prio refs state name time ");
comm_print("-------------------------------------------------------------");
tp = chRegFirstThread();
do {
sprintf(buffer, "%.8lx %.8lx %4lu %4lu %9s %14s %lu",
(uint32_t)tp, (uint32_t)tp->p_ctx.r13,
(uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
states[tp->p_state], tp->p_name, (uint32_t)tp->p_time);
comm_print(buffer);
tp = chRegNextThread(tp);
} while (tp != NULL);
comm_print("");
} else if (strcmp(argv[0], "fault") == 0) {
comm_print_fault_code(mcpwm_get_fault());
} else if (strcmp(argv[0], "rpm") == 0) {
sprintf(buffer, "Electrical RPM: %.2f rpm\n", (double)mcpwm_get_rpm());
comm_print(buffer);
} else if (strcmp(argv[0], "tacho") == 0) {
sprintf(buffer, "Tachometer counts: %i\n", mcpwm_get_tachometer_value(0));
comm_print(buffer);
} else if (strcmp(argv[0], "tim") == 0) {
TIM_Cmd(TIM1, DISABLE);
int t1_cnt = TIM1->CNT;
int t8_cnt = TIM8->CNT;
int duty = TIM1->CCR1;
int top = TIM1->ARR;
int voltage_samp = TIM8->CCR1;
int current1_samp = TIM1->CCR4;
int current2_samp = TIM8->CCR2;
TIM_Cmd(TIM1, ENABLE);
sprintf(buffer, "Tim1 CNT: %i", t1_cnt);
comm_print(buffer);
sprintf(buffer, "Tim8 CNT: %u", t8_cnt);
comm_print(buffer);
sprintf(buffer, "Duty cycle: %u", duty);
comm_print(buffer);
sprintf(buffer, "Top: %u", top);
comm_print(buffer);
sprintf(buffer, "Voltage sample: %u", voltage_samp);
comm_print(buffer);
sprintf(buffer, "Current 1 sample: %u", current1_samp);
comm_print(buffer);
sprintf(buffer, "Current 2 sample: %u\n", current2_samp);
comm_print(buffer);
} else if (strcmp(argv[0], "help") == 0) {
comm_print("Valid commands are:");
comm_print("help");
comm_print(" Show this help");
comm_print("ping");
comm_print(" Print pong here to see if the reply works");
comm_print("stop");
comm_print(" Stop the motor");
comm_print("last_adc_duration");
comm_print(" The time the latest ADC interrupt consumed");
comm_print("kv");
comm_print(" The calculated kv of the motor");
comm_print("mem");
comm_print(" Show memory usage");
comm_print("threads");
comm_print(" List all threads");
comm_print("fault");
comm_print(" Prints the current fault code");
comm_print("rpm");
comm_print(" Prints the current electrical RPM");
comm_print("tacho");
comm_print(" Prints tachometer value");
comm_print("tim");
comm_print(" Prints tim1 and tim8 settings\n");
} else {
sprintf(buffer, "Invalid command: %s\n"
"type help to list all available commands\n", argv[0]);
comm_print(buffer);
}
}
/**
* Process a received buffer with commands and data.
*
* @param data
* The buffer to process.
*
* @param len
* The length of the buffer.
*/
void commands_process_packet(unsigned char *data, unsigned char len) {
if (!len) {
return;
}
COMM_PACKET_ID packet_id;
int32_t ind = 0;
uint16_t sample_len;
uint8_t decimation;
bool at_start;
mc_configuration mcconf;
app_configuration appconf;
(void)len;
packet_id = data[0];
data++;
len--;
switch (packet_id) {
case COMM_GET_VALUES:
ind = 0;
send_buffer[ind++] = COMM_GET_VALUES;
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS1) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS2) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS3) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS4) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS5) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_MOS6) * 10.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(NTC_TEMP(ADC_IND_TEMP_PCB) * 10.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_read_reset_avg_motor_current() * 100.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_read_reset_avg_input_current() * 100.0), &ind);
buffer_append_int16(send_buffer, (int16_t)(mcpwm_get_duty_cycle_now() * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)mcpwm_get_rpm(), &ind);
buffer_append_int16(send_buffer, (int16_t)(GET_INPUT_VOLTAGE() * 10.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_amp_hours(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_amp_hours_charged(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_watt_hours(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcpwm_get_watt_hours_charged(false) * 10000.0), &ind);
buffer_append_int32(send_buffer, mcpwm_get_tachometer_value(false), &ind);
buffer_append_int32(send_buffer, mcpwm_get_tachometer_abs_value(false), &ind);
send_buffer[ind++] = mcpwm_get_fault();
send_packet(send_buffer, ind);
break;
case COMM_SET_DUTY:
ind = 0;
mcpwm_set_duty((float)buffer_get_int32(data, &ind) / 100000.0);
timeout_reset();
break;
case COMM_SET_CURRENT:
ind = 0;
mcpwm_set_current((float)buffer_get_int32(data, &ind) / 1000.0);
timeout_reset();
break;
case COMM_SET_CURRENT_BRAKE:
ind = 0;
mcpwm_set_brake_current((float)buffer_get_int32(data, &ind) / 1000.0);
timeout_reset();
break;
case COMM_SET_RPM:
ind = 0;
mcpwm_set_pid_speed((float)buffer_get_int32(data, &ind));
timeout_reset();
break;
case COMM_SET_DETECT:
mcpwm_set_detect();
timeout_reset();
break;
case COMM_SET_SERVO_OFFSET:
servos[0].offset = data[0];
break;
case COMM_SET_MCCONF:
mcconf = *mcpwm_get_configuration();
ind = 0;
mcconf.pwm_mode = data[ind++];
mcconf.comm_mode = data[ind++];
mcconf.l_current_max = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_current_min = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_in_current_max = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_in_current_min = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_abs_current_max = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_erpm_fbrake = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_min_vin = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_max_vin = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.l_slow_abs_current = data[ind++];
mcconf.l_rpm_lim_neg_torque = data[ind++];
mcconf.lo_current_max = mcconf.l_current_max;
mcconf.lo_current_min = mcconf.l_current_min;
mcconf.lo_in_current_max = mcconf.l_in_current_max;
mcconf.lo_in_current_min = mcconf.l_in_current_min;
mcconf.sl_is_sensorless = data[ind++];
mcconf.sl_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_min_erpm_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_limit_high_fac = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_rpm_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.hall_dir = data[ind++];
mcconf.hall_fwd_add = data[ind++];
mcconf.hall_rev_add = data[ind++];
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_min_rpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
conf_general_store_mc_configuration(&mcconf);
mcpwm_set_configuration(&mcconf);
break;
case COMM_GET_MCCONF:
mcconf = *mcpwm_get_configuration();
ind = 0;
send_buffer[ind++] = COMM_GET_MCCONF;
send_buffer[ind++] = mcconf.pwm_mode;
send_buffer[ind++] = mcconf.comm_mode;
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_current_min * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_in_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_in_current_min * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_abs_current_max * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_erpm_fbrake * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_min_vin * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.l_max_vin * 1000.0), &ind);
send_buffer[ind++] = mcconf.l_slow_abs_current;
send_buffer[ind++] = mcconf.l_rpm_lim_neg_torque;
send_buffer[ind++] = mcconf.sl_is_sensorless;
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_min_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_min_erpm_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_limit_high_fac * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_cycle_int_rpm_br * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.sl_bemf_coupling_k * 1000.0), &ind);
send_buffer[ind++] = mcconf.hall_dir;
send_buffer[ind++] = mcconf.hall_fwd_add;
send_buffer[ind++] = mcconf.hall_rev_add;
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_min_rpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_gain * 1000000.0), &ind);
buffer_append_int32(send_buffer, mcconf.m_fault_stop_time_ms, &ind);
send_packet(send_buffer, ind);
break;
case COMM_SET_APPCONF:
appconf = *app_get_configuration();
ind = 0;
appconf.timeout_msec = buffer_get_uint32(data, &ind);
appconf.timeout_brake_current = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_to_use = data[ind++];
appconf.app_ppm_ctrl_type = data[ind++];
appconf.app_ppm_pid_max_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_pulse_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_pulse_width = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_ppm_rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_uart_baudrate = buffer_get_uint32(data, &ind);
appconf.app_chuk_ctrl_type = data[ind++];
appconf.app_chuk_hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_chuk_rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
conf_general_store_app_configuration(&appconf);
app_set_configuration(&appconf);
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
break;
case COMM_GET_APPCONF:
appconf = *app_get_configuration();
ind = 0;
send_buffer[ind++] = COMM_GET_APPCONF;
buffer_append_uint32(send_buffer, appconf.timeout_msec, &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.timeout_brake_current * 1000.0), &ind);
send_buffer[ind++] = appconf.app_to_use;
send_buffer[ind++] = appconf.app_ppm_ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pid_max_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pulse_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_pulse_width * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_rpm_lim_end * 1000.0), &ind);
buffer_append_uint32(send_buffer, appconf.app_uart_baudrate, &ind);
send_buffer[ind++] = appconf.app_chuk_ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_rpm_lim_start * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_rpm_lim_end * 1000.0), &ind);
send_packet(send_buffer, ind);
break;
case COMM_SAMPLE_PRINT:
ind = 0;
at_start = data[ind++];
sample_len = buffer_get_uint16(data, &ind);
decimation = data[ind++];
main_sample_print_data(at_start, sample_len, decimation);
break;
case COMM_TERMINAL_CMD:
data[len] = '\0';
terminal_process_string((char*)data);
break;
case COMM_DETECT_MOTOR_PARAM:
ind = 0;
detect_current = (float)buffer_get_int32(data, &ind) / 1000.0;
detect_min_rpm = (float)buffer_get_int32(data, &ind) / 1000.0;
detect_low_duty = (float)buffer_get_int32(data, &ind) / 1000.0;
chEvtSignal(detect_tp, (eventmask_t) 1);
break;
case COMM_REBOOT:
// Lock the system and enter an infinite loop. The watchdog will reboot.
__disable_irq();
for(;;){};
break;
case COMM_ALIVE:
timeout_reset();
break;
case COMM_GET_DECODED_PPM:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_PPM;
buffer_append_int32(send_buffer, (int32_t)(servodec_get_servo(0) * 1000000.0), &ind);
send_packet(send_buffer, ind);
break;
case COMM_GET_DECODED_CHUK:
ind = 0;
send_buffer[ind++] = COMM_GET_DECODED_CHUK;
buffer_append_int32(send_buffer, (int32_t)(app_nunchuk_get_decoded_chuk() * 1000000.0), &ind);
send_packet(send_buffer, ind);
break;
default:
break;
}
}
void terminal_process_string(char *str) {
enum { kMaxArgs = 64 };
int argc = 0;
char *argv[kMaxArgs];
char *p2 = strtok(str, " ");
while (p2 && argc < kMaxArgs) {
argv[argc++] = p2;
p2 = strtok(0, " ");
}
if (argc == 0) {
comm_printf("No command received\n");
return;
}
if (strcmp(argv[0], "ping") == 0) {
comm_printf("pong\n");
} else if (strcmp(argv[0], "stop") == 0) {
mcpwm_set_duty(0);
comm_printf("Motor stopped\n");
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
comm_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
comm_printf("Latest injected ADC duration: %.4f ms", (double)(mcpwm_get_last_inj_adc_isr_duration() * 1000.0));
comm_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
} else if (strcmp(argv[0], "kv") == 0) {
comm_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
} else if (strcmp(argv[0], "mem") == 0) {
size_t n, size;
n = chHeapStatus(NULL, &size);
comm_printf("core free memory : %u bytes", chCoreStatus());
comm_printf("heap fragments : %u", n);
comm_printf("heap free total : %u bytes\n", size);
} else if (strcmp(argv[0], "threads") == 0) {
Thread *tp;
static const char *states[] = {THD_STATE_NAMES};
comm_printf(" addr stack prio refs state name time ");
comm_printf("-------------------------------------------------------------");
tp = chRegFirstThread();
do {
comm_printf("%.8lx %.8lx %4lu %4lu %9s %14s %lu",
(uint32_t)tp, (uint32_t)tp->p_ctx.r13,
(uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
states[tp->p_state], tp->p_name, (uint32_t)tp->p_time);
tp = chRegNextThread(tp);
} while (tp != NULL);
comm_printf("");
} else if (strcmp(argv[0], "fault") == 0) {
comm_print_fault_code(mcpwm_get_fault());
} else if (strcmp(argv[0], "rpm") == 0) {
comm_printf("Electrical RPM: %.2f rpm\n", (double)mcpwm_get_rpm());
} else if (strcmp(argv[0], "tacho") == 0) {
comm_printf("Tachometer counts: %i\n", mcpwm_get_tachometer_value(0));
} else if (strcmp(argv[0], "tim") == 0) {
TIM_Cmd(TIM_PWM, DISABLE);
int t_pwm_cnt = TIM_PWM->CNT;
int t_adc_cnt = TIM_ADC->CNT;
int duty = TIM_PWM->CCR1;
int top = TIM_PWM->ARR;
int voltage_samp = TIM_ADC->CCR1;
int current1_samp = TIM_PWM->CCR4;
int current2_samp = TIM_ADC->CCR4;
TIM_Cmd(TIM_PWM, ENABLE);
comm_printf("TIM_PWM CNT: %i", t_pwm_cnt);
comm_printf("TIM_ADC CNT: %u", t_adc_cnt);
comm_printf("Duty cycle: %u", duty);
comm_printf("Top: %u", top);
comm_printf("Voltage sample: %u", voltage_samp);
comm_printf("Current 1 sample: %u", current1_samp);
comm_printf("Current 2 sample: %u\n", current2_samp);
} else if (strcmp(argv[0], "volt") == 0) {
comm_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
} else if (strcmp(argv[0], "reset_drv") == 0) {
comm_printf("reset driver\n");
mcpwm_reset_driver();
}
// Setters
else if (strcmp(argv[0], "set_hall_table") == 0) {
if (argc == 4) {
int dir = -1;
int fwd_add = -1;
int rev_add = -1;
sscanf(argv[1], "%i", &dir);
sscanf(argv[2], "%i", &fwd_add);
sscanf(argv[3], "%i", &rev_add);
if (dir >= 0 && fwd_add >= 0 && rev_add >= 0) {
mcpwm_init_hall_table(dir, fwd_add, rev_add);
comm_printf("New hall sensor dir: %i fwd_add %i rev_add %i\n",
dir, fwd_add, rev_add);
} else {
comm_printf("Invalid argument(s).\n");
}
} else {
comm_printf("This command requires three arguments.\n");
}
}
// The help command
else if (strcmp(argv[0], "help") == 0) {
comm_printf("Valid commands are:");
comm_printf("help");
comm_printf(" Show this help");
comm_printf("ping");
comm_printf(" Print pong here to see if the reply works");
comm_printf("stop");
comm_printf(" Stop the motor");
comm_printf("last_adc_duration");
comm_printf(" The time the latest ADC interrupt consumed");
comm_printf("kv");
comm_printf(" The calculated kv of the motor");
comm_printf("mem");
comm_printf(" Show memory usage");
comm_printf("threads");
comm_printf(" List all threads");
comm_printf("fault");
comm_printf(" Prints the current fault code");
comm_printf("rpm");
comm_printf(" Prints the current electrical RPM");
comm_printf("tacho");
comm_printf(" Prints tachometer value");
comm_printf("tim");
comm_printf(" Prints TIM_PWM and TIM_ADC settings");
comm_printf("reset_drv");
comm_printf(" Short pulse on EN_GATE to reset latched driver fault");
comm_printf("set_hall_table [dir] [fwd_add] [rev_add]");
comm_printf(" Update the hall sensor lookup table");
comm_printf("volt");
comm_printf(" Prints different voltages\n");
} else {
comm_printf("Invalid command: %s\n"
"type help to list all available commands\n", argv[0]);
}
}
