예제 #1
0
파일: mc_interface.c 프로젝트: 1ee7/bldc
void mc_interface_fault_stop(mc_fault_code fault) {
	if (mc_interface_dccal_done() && m_fault_now == FAULT_CODE_NONE) {
		// Sent to terminal fault logger so that all faults and their conditions
		// can be printed for debugging.
		chSysLock();
		volatile int val_samp = TIM8->CCR1;
		volatile int current_samp = TIM1->CCR4;
		volatile int tim_top = TIM1->ARR;
		chSysUnlock();

		fault_data fdata;
		fdata.fault = fault;
		fdata.current = mc_interface_get_tot_current();
		fdata.current_filtered = mc_interface_get_tot_current_filtered();
		fdata.voltage = GET_INPUT_VOLTAGE();
		fdata.duty = mc_interface_get_duty_cycle_now();
		fdata.rpm = mc_interface_get_rpm();
		fdata.tacho = mc_interface_get_tachometer_value(false);
		fdata.cycles_running = m_cycles_running;
		fdata.tim_val_samp = val_samp;
		fdata.tim_current_samp = current_samp;
		fdata.tim_top = tim_top;
		fdata.comm_step = mcpwm_get_comm_step();
		fdata.temperature = NTC_TEMP(ADC_IND_TEMP_MOS1);
		terminal_add_fault_data(&fdata);
	}

	m_ignore_iterations = m_conf.m_fault_stop_time_ms;

	switch (m_conf.motor_type) {
	case MOTOR_TYPE_BLDC:
	case MOTOR_TYPE_DC:
		mcpwm_stop_pwm();
		break;

	case MOTOR_TYPE_FOC:
		mcpwm_foc_stop_pwm();
		break;

	default:
		break;
	}

	m_fault_now = fault;
}
예제 #2
0
파일: commands.c 프로젝트: 451506709/bldc
/**
 * 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;
	static mc_configuration mcconf, mcconf_old; // Static to save some stack space
	app_configuration appconf;
	uint16_t flash_res;
	uint32_t new_app_offset;
	chuck_data chuck_d_tmp;

	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_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS1), 1e1, &ind);
		buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS2), 1e1, &ind);
		buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS3), 1e1, &ind);
		buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS4), 1e1, &ind);
		buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS5), 1e1, &ind);
		buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_MOS6), 1e1, &ind);
		buffer_append_float16(send_buffer, NTC_TEMP(ADC_IND_TEMP_PCB), 1e1, &ind);
		buffer_append_float32(send_buffer, mc_interface_read_reset_avg_motor_current(), 1e2, &ind);
		buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
		buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
		buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
		buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
		buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
		buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
		buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
		buffer_append_float32(send_buffer, mc_interface_get_watt_hours_charged(false), 1e4, &ind);
		buffer_append_int32(send_buffer, mc_interface_get_tachometer_value(false), &ind);
		buffer_append_int32(send_buffer, mc_interface_get_tachometer_abs_value(false), &ind);
		send_buffer[ind++] = mc_interface_get_fault();
		// TODO: send FOC values id, iq, abs
		commands_send_packet(send_buffer, ind);
		break;

	case COMM_SET_DUTY:
		ind = 0;
		mc_interface_set_duty((float)buffer_get_int32(data, &ind) / 100000.0);
		timeout_reset();
		break;

	case COMM_SET_CURRENT:
		ind = 0;
		mc_interface_set_current((float)buffer_get_int32(data, &ind) / 1000.0);
		timeout_reset();
		break;

	case COMM_SET_CURRENT_BRAKE:
		ind = 0;
		mc_interface_set_brake_current((float)buffer_get_int32(data, &ind) / 1000.0);
		timeout_reset();
		break;

	case COMM_SET_RPM:
		ind = 0;
		mc_interface_set_pid_speed((float)buffer_get_int32(data, &ind));
		timeout_reset();
		break;

	case COMM_SET_POS:
		ind = 0;
		mc_interface_set_pid_pos((float)buffer_get_int32(data, &ind) / 1000000.0);
		timeout_reset();
		break;

	case COMM_SET_DETECT:
		mcconf = *mc_interface_get_configuration();

		ind = 0;
		display_position_mode = data[ind++];

		if (mcconf.motor_type == MOTOR_TYPE_BLDC) {
			if (display_position_mode == DISP_POS_MODE_NONE) {
				mc_interface_release_motor();
			} else if (display_position_mode == DISP_POS_MODE_INDUCTANCE) {
				mcpwm_set_detect();
			}
		}

		timeout_reset();
		break;

	case COMM_SET_SERVO_POS:
#if SERVO_OUT_ENABLE
		ind = 0;
#if SERVO_OUT_SIMPLE
		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
#else
		servos[0].pos = (int16_t)(buffer_get_float16(data, 1000.0, &ind) * 255.0);
#endif
#endif
		break;

	case COMM_SET_MCCONF:
		mcconf = *mc_interface_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 = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_current_min = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_in_current_max = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_in_current_min = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_abs_current_max = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_min_erpm = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_erpm = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_erpm_fbrake = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_erpm_fbrake_cc = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_min_vin = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_vin = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_battery_cut_start = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_battery_cut_end = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_slow_abs_current = data[ind++];
		mcconf.l_rpm_lim_neg_torque = data[ind++];
		mcconf.l_temp_fet_start = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_temp_fet_end = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_temp_motor_start = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_temp_motor_end = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_min_duty = buffer_get_float32(data, 1000000.0, &ind);
		mcconf.l_max_duty = buffer_get_float32(data, 1000000.0, &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_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.foc_current_kp = buffer_get_float32(data, 1e5, &ind);
		mcconf.foc_current_ki = buffer_get_float32(data, 1e5, &ind);
		mcconf.foc_f_sw = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_dt_us = buffer_get_float32(data, 1e6, &ind);
		mcconf.foc_encoder_inverted = data[ind++];
		mcconf.foc_encoder_offset = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_encoder_ratio = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_sensor_mode = data[ind++];
		mcconf.foc_pll_kp = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_pll_ki = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_motor_l = buffer_get_float32(data, 1e8, &ind);
		mcconf.foc_motor_r = buffer_get_float32(data, 1e5, &ind);
		mcconf.foc_motor_flux_linkage = buffer_get_float32(data, 1e5, &ind);
		mcconf.foc_observer_gain = buffer_get_float32(data, 1e0, &ind);
		mcconf.foc_duty_dowmramp_kp = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_duty_dowmramp_ki = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_openloop_rpm = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_sl_openloop_hyst = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_sl_openloop_time = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_sl_d_current_duty = buffer_get_float32(data, 1e3, &ind);
		mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
		memcpy(mcconf.foc_hall_table, data + ind, 8);
		ind += 8;
		mcconf.foc_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_erpm = (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.p_pid_ang_div = (float)buffer_get_int32(data, &ind) / 100000.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 = buffer_get_float32(data, 1e5, &ind);

		mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
		mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
		mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
		mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
		mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
		mcconf.m_sensor_port_mode = data[ind++];

		conf_general_store_mc_configuration(&mcconf);
		mc_interface_set_configuration(&mcconf);

		ind = 0;
		send_buffer[ind++] = packet_id;
		commands_send_packet(send_buffer, ind);
		break;

	case COMM_GET_MCCONF:
	case COMM_GET_MCCONF_DEFAULT:
		if (packet_id == COMM_GET_MCCONF) {
			mcconf = *mc_interface_get_configuration();
		} else {
			conf_general_get_default_mc_configuration(&mcconf);
		}

		ind = 0;
		send_buffer[ind++] = packet_id;

		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);
		buffer_append_float32(send_buffer, mcconf.l_battery_cut_start, 1000.0, &ind);
		buffer_append_float32(send_buffer, mcconf.l_battery_cut_end, 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_float32(send_buffer, mcconf.foc_current_kp, 1e5, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_current_ki, 1e5, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_f_sw, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_dt_us, 1e6, &ind);
		send_buffer[ind++] = mcconf.foc_encoder_inverted;
		buffer_append_float32(send_buffer, mcconf.foc_encoder_offset, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_encoder_ratio, 1e3, &ind);
		send_buffer[ind++] = mcconf.foc_sensor_mode;
		buffer_append_float32(send_buffer, mcconf.foc_pll_kp, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_pll_ki, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_motor_l, 1e8, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_motor_r, 1e5, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_motor_flux_linkage, 1e5, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_observer_gain, 1e0, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_duty_dowmramp_kp, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_duty_dowmramp_ki, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_openloop_rpm, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_sl_openloop_hyst, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_sl_openloop_time, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_duty, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
		memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
		ind += 8;
		buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_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_erpm * 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_float32(send_buffer, mcconf.p_pid_ang_div, 1e5, &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);
		buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step, 1000000.0, &ind);
		buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
		buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
		buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
		send_buffer[ind++] = mcconf.m_sensor_port_mode;

		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.cc_button_inverted = data[ind++];
		appconf.app_adc_conf.rev_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 = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.rpm_lim_start = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.rpm_lim_end = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.ramp_time_pos = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.ramp_time_neg = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.stick_erpm_per_s_in_cc = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.multi_esc = data[ind++];
		appconf.app_chuk_conf.tc = data[ind++];
		appconf.app_chuk_conf.tc_max_diff = buffer_get_float32(data, 1000.0, &ind);

		appconf.app_nrf_conf.speed = data[ind++];
		appconf.app_nrf_conf.power = data[ind++];
		appconf.app_nrf_conf.crc_type = data[ind++];
		appconf.app_nrf_conf.retry_delay = data[ind++];
		appconf.app_nrf_conf.retries = data[ind++];
		appconf.app_nrf_conf.channel = data[ind++];
		memcpy(appconf.app_nrf_conf.address, data + ind, 3);
		ind += 3;
		appconf.app_nrf_conf.send_crc_ack = data[ind++];

		conf_general_store_app_configuration(&appconf);
		app_set_configuration(&appconf);
		timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);

		ind = 0;
		send_buffer[ind++] = packet_id;
		commands_send_packet(send_buffer, ind);
		break;

	case COMM_GET_APPCONF:
	case COMM_GET_APPCONF_DEFAULT:
		if (packet_id == COMM_GET_APPCONF) {
			appconf = *app_get_configuration();
		} else {
			conf_general_get_default_app_configuration(&appconf);
		}

		ind = 0;
		send_buffer[ind++] = packet_id;
		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.cc_button_inverted;
		send_buffer[ind++] = appconf.app_adc_conf.rev_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_float32(send_buffer, appconf.app_chuk_conf.hyst, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.rpm_lim_start, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.rpm_lim_end, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.ramp_time_pos, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.ramp_time_neg, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.stick_erpm_per_s_in_cc, 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);

		send_buffer[ind++] = appconf.app_nrf_conf.speed;
		send_buffer[ind++] = appconf.app_nrf_conf.power;
		send_buffer[ind++] = appconf.app_nrf_conf.crc_type;
		send_buffer[ind++] = appconf.app_nrf_conf.retry_delay;
		send_buffer[ind++] = appconf.app_nrf_conf.retries;
		send_buffer[ind++] = appconf.app_nrf_conf.channel;
		memcpy(send_buffer + ind, appconf.app_nrf_conf.address, 3);
		ind += 3;
		send_buffer[ind++] = appconf.app_nrf_conf.send_crc_ack;

		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++];
		mc_interface_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 = buffer_get_float32(data, 1e3, &ind);
		detect_min_rpm = buffer_get_float32(data, 1e3, &ind);
		detect_low_duty = buffer_get_float32(data, 1e3, &ind);

		chEvtSignal(detect_tp, (eventmask_t) 1);
		break;

	case COMM_DETECT_MOTOR_R_L: {
		mcconf = *mc_interface_get_configuration();
		mcconf_old = mcconf;

		mcconf.motor_type = MOTOR_TYPE_FOC;
		mc_interface_set_configuration(&mcconf);

		float r = 0.0;
		float l = 0.0;
		bool res = mcpwm_foc_measure_res_ind(&r, &l);
		mc_interface_set_configuration(&mcconf_old);

		if (!res) {
			r = 0.0;
			l = 0.0;
		}

		ind = 0;
		send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
		buffer_append_float32(send_buffer, r, 1e6, &ind);
		buffer_append_float32(send_buffer, l, 1e3, &ind);
		commands_send_packet(send_buffer, ind);
	}
	break;

	case COMM_DETECT_MOTOR_FLUX_LINKAGE: {
		ind = 0;
		float current = buffer_get_float32(data, 1e3, &ind);
		float min_rpm = buffer_get_float32(data, 1e3, &ind);
		float duty = buffer_get_float32(data, 1e3, &ind);
		float resistance = buffer_get_float32(data, 1e6, &ind);

		float linkage;
		bool res = conf_general_measure_flux_linkage(current, duty, min_rpm, resistance, &linkage);

		if (!res) {
			linkage = 0.0;
		}

		ind = 0;
		send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE;
		buffer_append_float32(send_buffer, linkage, 1e7, &ind);
		commands_send_packet(send_buffer, ind);
	}
	break;

	case COMM_DETECT_ENCODER: {
		if (encoder_is_configured()) {
			mcconf = *mc_interface_get_configuration();
			mcconf_old = mcconf;

			ind = 0;
			float current = buffer_get_float32(data, 1e3, &ind);

			mcconf.motor_type = MOTOR_TYPE_FOC;
			mcconf.foc_f_sw = 10000.0;
			mcconf.foc_current_kp = 0.01;
			mcconf.foc_current_ki = 10.0;
			mc_interface_set_configuration(&mcconf);

			float offset = 0.0;
			float ratio = 0.0;
			bool inverted = false;
			mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
			mc_interface_set_configuration(&mcconf_old);

			ind = 0;
			send_buffer[ind++] = COMM_DETECT_ENCODER;
			buffer_append_float32(send_buffer, offset, 1e6, &ind);
			buffer_append_float32(send_buffer, ratio, 1e6, &ind);
			send_buffer[ind++] = inverted;
			commands_send_packet(send_buffer, ind);
		} else {
			ind = 0;
			send_buffer[ind++] = COMM_DETECT_ENCODER;
			buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
			buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
			send_buffer[ind++] = false;
			commands_send_packet(send_buffer, ind);
		}
	}
	break;

	case COMM_DETECT_HALL_FOC: {
		mcconf = *mc_interface_get_configuration();

		if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
			mcconf_old = mcconf;
			ind = 0;
			float current = buffer_get_float32(data, 1e3, &ind);

			mcconf.motor_type = MOTOR_TYPE_FOC;
			mcconf.foc_f_sw = 10000.0;
			mcconf.foc_current_kp = 0.01;
			mcconf.foc_current_ki = 10.0;
			mc_interface_set_configuration(&mcconf);

			uint8_t hall_tab[8];
			bool res = mcpwm_foc_hall_detect(current, hall_tab);
			mc_interface_set_configuration(&mcconf_old);

			ind = 0;
			send_buffer[ind++] = COMM_DETECT_HALL_FOC;
			memcpy(send_buffer + ind, hall_tab, 8);
			ind += 8;
			send_buffer[ind++] = res ? 0 : 1;

			commands_send_packet(send_buffer, ind);
		} else {
			ind = 0;
			send_buffer[ind++] = COMM_DETECT_HALL_FOC;
			memset(send_buffer, 255, 8);
			ind += 8;
			send_buffer[ind++] = 0;
		}
	}
	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);
		buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level2() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage2() * 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;

	case COMM_SET_CHUCK_DATA:
		ind = 0;
		chuck_d_tmp.js_x = data[ind++];
		chuck_d_tmp.js_y = data[ind++];
		chuck_d_tmp.bt_c = data[ind++];
		chuck_d_tmp.bt_z = data[ind++];
		chuck_d_tmp.acc_x = buffer_get_int16(data, &ind);
		chuck_d_tmp.acc_y = buffer_get_int16(data, &ind);
		chuck_d_tmp.acc_z = buffer_get_int16(data, &ind);
		app_nunchuk_update_output(&chuck_d_tmp);
		break;

	case COMM_CUSTOM_APP_DATA:
		if (appdata_func) {
			appdata_func(data, len);
		}
		break;

	default:
		break;
	}
}
예제 #3
0
파일: terminal.c 프로젝트: 451506709/bldc
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;
	}

	static mc_configuration mcconf; // static to save some stack
	static mc_configuration mcconf_old; // static to save some stack
	mcconf = *mc_interface_get_configuration();
	mcconf_old = mcconf;

	if (strcmp(argv[0], "ping") == 0) {
		commands_printf("pong\n");
	} else if (strcmp(argv[0], "stop") == 0) {
		mc_interface_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)(mc_interface_get_last_inj_adc_isr_duration() * 1000.0));
		commands_printf("Latest sample ADC duration: %.4f ms\n", (double)(mc_interface_get_last_sample_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", chCoreGetStatusX());
		commands_printf("heap fragments   : %u", n);
		commands_printf("heap free total  : %u bytes\n", size);
	} else if (strcmp(argv[0], "threads") == 0) {
		thread_t *tp;
		static const char *states[] = {CH_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", mc_interface_fault_to_string(mc_interface_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", mc_interface_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("Cycles running   : %d", fault_vec[i].cycles_running);
				commands_printf("TIM duty         : %d", (int)((float)fault_vec[i].tim_top * fault_vec[i].duty));
				commands_printf("TIM val samp     : %d", fault_vec[i].tim_val_samp);
				commands_printf("TIM current samp : %d", fault_vec[i].tim_current_samp);
				commands_printf("TIM top          : %d", fault_vec[i].tim_top);
				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)mc_interface_get_rpm());
	} else if (strcmp(argv[0], "tacho") == 0) {
		commands_printf("Tachometer counts: %i\n", mc_interface_get_tachometer_value(0));
	} else if (strcmp(argv[0], "tim") == 0) {
		chSysLock();
		volatile int t1_cnt = TIM1->CNT;
		volatile int t8_cnt = TIM8->CNT;
		volatile int dir1 = !!(TIM1->CR1 & (1 << 4));
		volatile int dir8 = !!(TIM8->CR1 & (1 << 4));
		chSysUnlock();
		int duty1 = TIM1->CCR1;
		int duty2 = TIM1->CCR2;
		int duty3 = TIM1->CCR3;
		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 cycle1: %u", duty1);
		commands_printf("Duty cycle2: %u", duty2);
		commands_printf("Duty cycle3: %u", duty3);
		commands_printf("Top: %u", top);
		commands_printf("Dir1: %u", dir1);
		commands_printf("Dir8: %u", dir8);
		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", &current);
			sscanf(argv[2], "%f", &min_rpm);
			sscanf(argv[3], "%f", &low_duty);

			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;
				int8_t hall_table[8];
				int hall_res;
				if (conf_general_detect_motor_param(current, min_rpm, low_duty, &cycle_integrator, &coupling_k, hall_table, &hall_res)) {
					commands_printf("Cycle integrator limit: %.2f", (double)cycle_integrator);
					commands_printf("Coupling factor: %.2f", (double)coupling_k);

					if (hall_res == 0) {
						commands_printf("Detected hall sensor table:");
						commands_printf("%i, %i, %i, %i, %i, %i, %i, %i\n",
								hall_table[0], hall_table[1], hall_table[2], hall_table[3],
								hall_table[4], hall_table[5], hall_table[6], hall_table[7]);
					} else if (hall_res == -1) {
						commands_printf("Hall sensor detection failed:");
						commands_printf("%i, %i, %i, %i, %i, %i, %i, %i\n",
								hall_table[0], hall_table[1], hall_table[2], hall_table[3],
								hall_table[4], hall_table[5], hall_table[6], hall_table[7]);
					} else if (hall_res == -2) {
						commands_printf("WS2811 enabled. Hall sensors cannot be used.\n");
					} else if (hall_res == -3) {
						commands_printf("Encoder enabled. Hall sensors cannot be used.\n");
					}
				} 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);
	} else if (strcmp(argv[0], "can_devs") == 0) {
		commands_printf("CAN devices seen on the bus the past second:\n");
		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) < 1.0) {
				commands_printf("ID                 : %i", msg->id);
				commands_printf("RX Time            : %i", msg->rx_time);
				commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(msg->rx_time) * 1000.0));
				commands_printf("RPM                : %.2f", (double)msg->rpm);
				commands_printf("Current            : %.2f", (double)msg->current);
				commands_printf("Duty               : %.2f\n", (double)msg->duty);
			}
		}
	} else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
		if (argc == 2) {
			float current = -1.0;
			sscanf(argv[1], "%f", &current);

			if (current > 0.0 && current <= mcconf.l_current_max) {
				if (encoder_is_configured()) {
					mc_motor_type type_old = mcconf.motor_type;
					mcconf.motor_type = MOTOR_TYPE_FOC;
					mc_interface_set_configuration(&mcconf);

					float offset = 0.0;
					float ratio = 0.0;
					bool inverted = false;
					mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);

					mcconf.motor_type = type_old;
					mc_interface_set_configuration(&mcconf);

					commands_printf("Offset   : %.2f", (double)offset);
					commands_printf("Ratio    : %.2f", (double)ratio);
					commands_printf("Inverted : %s\n", inverted ? "true" : "false");
				} else {
					commands_printf("Encoder not enabled.\n");
				}
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_res") == 0) {
		if (argc == 2) {
			float current = -1.0;
			sscanf(argv[1], "%f", &current);

			if (current > 0.0 && current <= mcconf.l_current_max) {
				mcconf.motor_type = MOTOR_TYPE_FOC;
				mc_interface_set_configuration(&mcconf);

				commands_printf("Resistance: %.6f ohm\n", (double)mcpwm_foc_measure_resistance(current, 2000));

				mc_interface_set_configuration(&mcconf_old);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_ind") == 0) {
		if (argc == 2) {
			float duty = -1.0;
			sscanf(argv[1], "%f", &duty);

			if (duty > 0.0) {
				mcconf.motor_type = MOTOR_TYPE_FOC;
				mcconf.foc_f_sw = 3000.0;
				mc_interface_set_configuration(&mcconf);

				commands_printf("Inductance: %.2f microhenry\n", (double)(mcpwm_foc_measure_inductance(duty, 200, 0)));

				mc_interface_set_configuration(&mcconf_old);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_linkage") == 0) {
		if (argc == 5) {
			float current = -1.0;
			float duty = -1.0;
			float min_erpm = -1.0;
			float res = -1.0;
			sscanf(argv[1], "%f", &current);
			sscanf(argv[2], "%f", &duty);
			sscanf(argv[3], "%f", &min_erpm);
			sscanf(argv[4], "%f", &res);

			if (current > 0.0 && current <= mcconf.l_current_max && min_erpm > 0.0 && duty > 0.02 && res >= 0.0) {
				float linkage;
				conf_general_measure_flux_linkage(current, duty, min_erpm, res, &linkage);
				commands_printf("Flux linkage: %.7f\n", (double)linkage);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_res_ind") == 0) {
		mcconf.motor_type = MOTOR_TYPE_FOC;
		mc_interface_set_configuration(&mcconf);

		float res = 0.0;
		float ind = 0.0;
		mcpwm_foc_measure_res_ind(&res, &ind);
		commands_printf("Resistance: %.6f ohm", (double)res);
		commands_printf("Inductance: %.2f microhenry\n", (double)ind);

		mc_interface_set_configuration(&mcconf_old);
	} else if (strcmp(argv[0], "measure_linkage_foc") == 0) {
		if (argc == 2) {
			float duty = -1.0;
			sscanf(argv[1], "%f", &duty);

			if (duty > 0.0) {
				mcconf.motor_type = MOTOR_TYPE_FOC;
				mc_interface_set_configuration(&mcconf);
				const float res = (3.0 / 2.0) * mcconf.foc_motor_r;

				// Disable timeout
				systime_t tout = timeout_get_timeout_msec();
				float tout_c = timeout_get_brake_current();
				timeout_configure(60000, 0.0);

				for (int i = 0;i < 100;i++) {
					mc_interface_set_duty(((float)i / 100.0) * duty);
					chThdSleepMilliseconds(20);
				}

				float vq_avg = 0.0;
				float rpm_avg = 0.0;
				float samples = 0.0;
				float iq_avg = 0.0;
				for (int i = 0;i < 1000;i++) {
					vq_avg += mcpwm_foc_get_vq();
					rpm_avg += mc_interface_get_rpm();
					iq_avg += mc_interface_get_tot_current_directional();
					samples += 1.0;
					chThdSleepMilliseconds(1);
				}

				mc_interface_release_motor();
				mc_interface_set_configuration(&mcconf_old);

				// Enable timeout
				timeout_configure(tout, tout_c);

				vq_avg /= samples;
				rpm_avg /= samples;
				iq_avg /= samples;

				float linkage = (vq_avg - res * iq_avg) / (rpm_avg * ((2.0 * M_PI) / 60.0));

				commands_printf("Flux linkage: %.7f\n", (double)linkage);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "foc_state") == 0) {
		mcpwm_foc_print_state();
		commands_printf(" ");
	}

	// 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("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");

		commands_printf("can_devs");
		commands_printf("  Prints all CAN devices seen on the bus the past second");

		commands_printf("foc_encoder_detect [current]");
		commands_printf("  Run the motor at 1Hz on open loop and compute encoder settings");

		commands_printf("measure_res [current]");
		commands_printf("  Lock the motor with a current and calculate its resistance");

		commands_printf("measure_ind [duty]");
		commands_printf("  Send short voltage pulses, measure the current and calculate the motor inductance");

		commands_printf("measure_linkage [current] [duty] [min_rpm] [motor_res]");
		commands_printf("  Run the motor in BLDC delay mode and measure the flux linkage");
		commands_printf("  example measure_linkage 5 0.5 700 0.076");
		commands_printf("  tip: measure the resistance with measure_res first");

		commands_printf("measure_res_ind");
		commands_printf("  Measure the motor resistance and inductance with an incremental adaptive algorithm.");

		commands_printf("measure_linkage_foc [duty]");
		commands_printf("  Run the motor with FOC and measure the flux linkage.");

		commands_printf("foc_state");
		commands_printf("  Print some FOC state variables.\n");
	} else {
		commands_printf("Invalid command: %s\n"
				"type help to list all available commands\n", argv[0]);
	}
}
예제 #4
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;

	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)(mc_interface_read_reset_avg_motor_current() * 100.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(mc_interface_read_reset_avg_input_current() * 100.0), &ind);
		buffer_append_int16(send_buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)mc_interface_get_rpm(), &ind);
		buffer_append_int16(send_buffer, (int16_t)(GET_INPUT_VOLTAGE() * 10.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(mc_interface_get_amp_hours(false) * 10000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(mc_interface_get_amp_hours_charged(false) * 10000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(mc_interface_get_watt_hours(false) * 10000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(mc_interface_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++] = mc_interface_get_fault();
		commands_send_packet(send_buffer, ind);
		break;

	case COMM_SET_DUTY:
		ind = 0;
		mc_interface_set_duty((float)buffer_get_int32(data, &ind) / 100000.0);
		timeout_reset();
		break;

	case COMM_SET_CURRENT:
		ind = 0;
		mc_interface_set_current((float)buffer_get_int32(data, &ind) / 1000.0);
		timeout_reset();
		break;

	case COMM_SET_CURRENT_BRAKE:
		ind = 0;
		mc_interface_set_brake_current((float)buffer_get_int32(data, &ind) / 1000.0);
		timeout_reset();
		break;

	case COMM_SET_RPM:
		ind = 0;
		mc_interface_set_pid_speed((float)buffer_get_int32(data, &ind));
		timeout_reset();
		break;

	case COMM_SET_POS:
		ind = 0;
		mc_interface_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_POS:
#if SERVO_OUT_ENABLE
		ind = 0;
#if SERVO_OUT_SIMPLE
		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
#else
		servos[0].pos = (int16_t)(buffer_get_float16(data, 1000.0, &ind) * 255.0);
#endif
#endif
		break;

	case COMM_SET_MCCONF:
		mcconf = *mc_interface_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 = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_current_min = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_in_current_max = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_in_current_min = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_abs_current_max = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_min_erpm = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_erpm = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_erpm_fbrake = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_erpm_fbrake_cc = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_min_vin = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_max_vin = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_battery_cut_start = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_battery_cut_end = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_slow_abs_current = data[ind++];
		mcconf.l_rpm_lim_neg_torque = data[ind++];
		mcconf.l_temp_fet_start = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_temp_fet_end = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_temp_motor_start = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_temp_motor_end = buffer_get_float32(data, 1000.0, &ind);
		mcconf.l_min_duty = buffer_get_float32(data, 1000000.0, &ind);
		mcconf.l_max_duty = buffer_get_float32(data, 1000000.0, &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_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_erpm = (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);
		mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
		mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
		mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);

		conf_general_store_mc_configuration(&mcconf);
		mc_interface_set_configuration(&mcconf);

		ind = 0;
		send_buffer[ind++] = packet_id;
		commands_send_packet(send_buffer, ind);
		break;

	case COMM_GET_MCCONF:
	case COMM_GET_MCCONF_DEFAULT:
		if (packet_id == COMM_GET_MCCONF) {
			mcconf = *mc_interface_get_configuration();
		} else {
			conf_general_get_default_mc_configuration(&mcconf);
		}

		ind = 0;
		send_buffer[ind++] = packet_id;

		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);
		buffer_append_float32(send_buffer, mcconf.l_battery_cut_start, 1000.0, &ind);
		buffer_append_float32(send_buffer, mcconf.l_battery_cut_end, 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_erpm * 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);
		buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step, 1000000.0, &ind);
		buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
		buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &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.cc_button_inverted = data[ind++];
		appconf.app_adc_conf.rev_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 = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.rpm_lim_start = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.rpm_lim_end = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.ramp_time_pos = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.ramp_time_neg = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.stick_erpm_per_s_in_cc = buffer_get_float32(data, 1000.0, &ind);
		appconf.app_chuk_conf.multi_esc = data[ind++];
		appconf.app_chuk_conf.tc = data[ind++];
		appconf.app_chuk_conf.tc_max_diff = buffer_get_float32(data, 1000.0, &ind);

		conf_general_store_app_configuration(&appconf);
		app_set_configuration(&appconf);
		timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);

		ind = 0;
		send_buffer[ind++] = packet_id;
		commands_send_packet(send_buffer, ind);
		break;

	case COMM_GET_APPCONF:
	case COMM_GET_APPCONF_DEFAULT:
		if (packet_id == COMM_GET_APPCONF) {
			appconf = *app_get_configuration();
		} else {
			conf_general_get_default_app_configuration(&appconf);
		}

		ind = 0;
		send_buffer[ind++] = packet_id;
		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.cc_button_inverted;
		send_buffer[ind++] = appconf.app_adc_conf.rev_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_float32(send_buffer, appconf.app_chuk_conf.hyst, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.rpm_lim_start, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.rpm_lim_end, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.ramp_time_pos, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.ramp_time_neg, 1000.0, &ind);
		buffer_append_float32(send_buffer, appconf.app_chuk_conf.stick_erpm_per_s_in_cc, 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;
	}
}