static msg_t periodic_thread(void *arg) { (void)arg; chRegSetThreadName("Main periodic"); int fault_print = 0; for(;;) { if (mcpwm_get_state() == MC_STATE_RUNNING) { ledpwm_set_intensity(LED_GREEN, 1.0); } else { ledpwm_set_intensity(LED_GREEN, 0.2); } if (mcpwm_get_fault() != FAULT_CODE_NONE) { ledpwm_set_intensity(LED_RED, 1.0); if (!fault_print && AUTO_PRINT_FAULTS) { fault_print = 1; comm_print_fault_code(mcpwm_get_fault()); } } else { ledpwm_set_intensity(LED_RED, 0.0); fault_print = 0; } if (mcpwm_get_state() == MC_STATE_DETECTING) { comm_send_rotor_pos(mcpwm_get_detect_pos()); } chThdSleepMilliseconds(25); } return 0; }
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; } }
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]); } }