static void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[]) {
  static const char *states[] = {THD_STATE_NAMES};
  uint64_t busy = 0, total = 0;
  Thread *tp;

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: threads\r\n");
    return;
  }

  chprintf(chp, "name            addr    stack prio refs     state   time\r\n");
  chprintf(chp, "--------------------------------------------------------\r\n");
  tp = chRegFirstThread();
  do {
    chprintf(chp, "%12s %.8lx %.8lx %4lu %4lu %9s %lu\r\n",
            chRegGetThreadName(tp),
            (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
            (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
            states[tp->p_state], (uint32_t)tp->p_time);
    if(tp->p_prio != 1) {
        busy += tp->p_time;
    }
    total += tp->p_time;
    tp = chRegNextThread(tp);
  } while (tp != NULL);
  chprintf(chp, "CPU Usage: %ld%%\r\n", busy*100/total);
}

static void cmd_threads(BaseChannel *chp, int argc, char *argv[]) {
  static const char *states[] = {
    "READY",
    "CURRENT",
    "SUSPENDED",
    "WTSEM",
    "WTMTX",
    "WTCOND",
    "SLEEPING",
    "WTEXIT",
    "WTOREVT",
    "WTANDEVT",
    "SNDMSG",
    "WTMSG",
    "FINAL"
  };
  Thread *tp;
  char buf[60];

  (void)argv;
  if (argc > 0) {
    shellPrintLine(chp, "Usage: threads");
    return;
  }
  shellPrintLine(chp, "    addr    stack prio refs     state time");
  tp = chRegFirstThread();
  do {
    siprintf(buf, "%8lx %8lx %4u %4i %9s %u",
             (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
             (unsigned int)tp->p_prio, tp->p_refs - 1,
             states[tp->p_state], (unsigned int)tp->p_time);
    shellPrintLine(chp, buf);
    tp = chRegNextThread(tp);
  } while (tp != NULL);
}

static THD_FUNCTION(ThreadMonitor, arg)
{
  (void)arg;
  chRegSetThreadName("Monitor");
  uint32_t  run_offset, irq_ticks = 0, total_ticks;
  thread_t* tp = NULL;

  DWT->CTRL |= DWT_CTRL_EXCEVTENA_Msk;

  while (TRUE)
  {
    tp = chRegFirstThread();
    do {
        tp->runtime = 0;
        tp->irqtime = 0;
        tp = chRegNextThread(tp);
    } while (tp != NULL);

    irq_ticks = 0;
    run_offset = DWT->CYCCNT;

	/* Populate load data */
    chThdSleepMilliseconds(1000);

	/* Convert to systick time base */
	total_ticks = (DWT->CYCCNT - run_offset) / (STM32_SYSCLK/CH_CFG_ST_FREQUENCY);

    tp = chRegFirstThread();
    do {
        irq_ticks += tp->irqtime;
        tp = chRegNextThread(tp);
    } while (tp != NULL);

    tp = chRegFirstThread();
    do {
        tp->pct = ((tp->runtime*10000)/total_ticks) | RUNNING(tp);
        tp = chRegNextThread(tp);
    } while (tp != NULL);

    irq_pct = ((irq_ticks*10000)/total_ticks);
  }
  return;
}

static bool regfind(thread_t *tp) {
  thread_t *ftp;
  bool found = false;

  ftp = chRegFirstThread();
  do {
    found |= ftp == tp;
    ftp = chRegNextThread(ftp);
  } while (ftp != NULL);
  return found;
}

static bool_t regfind(Thread *tp) {
  Thread *ftp;
  bool_t found = FALSE;

  ftp = chRegFirstThread();
  do {
    found |= ftp == tp;
    ftp = chRegNextThread(ftp);
  } while (ftp != NULL);
  return found;
}

// Fill data structure
void rtos_mon_periodic_arch(void)
{
  int i;

  rtos_mon.core_free_memory = chCoreGetStatusX();
  rtos_mon.heap_free_memory = 0;
  rtos_mon.thread_counter = 0;

  // loop threads to find idle thread
  // store info on other threads
  thread_t *tp;
  float sum = 0.f;
  rtos_mon.thread_name_idx = 0;
  tp = chRegFirstThread();
  do {
    // add beginning of thread name to buffer
    for (i = 0; i < RTOS_MON_NAME_LEN-1 && tp->p_name[i] != '\0'; i++) {
      rtos_mon.thread_names[rtos_mon.thread_name_idx++] = tp->p_name[i];
    }
    rtos_mon.thread_names[rtos_mon.thread_name_idx++] = ';';

    // store free stack for this thread
    rtos_mon.thread_free_stack[i] = get_stack_free(tp);

    // store time spend in thread
    thread_p_time[rtos_mon.thread_counter] = tp->p_time;
    sum += (float)(tp->p_time);

    // if current thread is 'idle' thread, store its value separately
    if (tp == chSysGetIdleThreadX()) {
      idle_counter = (uint32_t)tp->p_time;
    }
    // get next thread
    tp = chRegNextThread(tp);
    // increment thread counter
    rtos_mon.thread_counter++;
  } while (tp != NULL && rtos_mon.thread_counter < RTOS_MON_MAX_THREADS);

  // store individual thread load (as centi-percent integer)
  for (i = 0; i < rtos_mon.thread_counter; i ++) {
    rtos_mon.thread_load[i] = (uint16_t)(10000.f * (float)thread_p_time[i] / sum);
  }

  // assume we call the counter once a second
  // so the difference in seconds is always one
  // NOTE: not perfectly precise
  // FIXME: add finer resolution than seconds?
  rtos_mon.cpu_load = (1 - (float)(idle_counter - last_idle_counter) / CH_CFG_ST_FREQUENCY) * 100;
  last_idle_counter = idle_counter;

}

/**
 * @brief   Confirms that a pointer is a valid thread pointer.
 * @note    The reference counter of the found thread is increased by one so
 *          it cannot be disposed incidentally after the pointer has been
 *          returned.
 *
 * @param[in] tp        pointer to the thread
 * @return              A pointer to the found thread.
 * @retval NULL         if a matching thread has not been found.
 *
 * @api
 */
thread_t *chRegFindThreadByPointer(thread_t *tp) {
  thread_t *ctp;

  /* Scanning registry.*/
  ctp = chRegFirstThread();
  do {
    if (ctp == tp) {
      return ctp;
    }
    ctp = chRegNextThread(ctp);
  } while (ctp != NULL);

  return NULL;
}

/**
 * This methods prints all threads and their total times
 */
static void cmd_threads(void) {
#if CH_DBG_THREADS_PROFILING || defined(__DOXYGEN__)
	static const char *states[] = { THD_STATE_NAMES };
	Thread *tp;

	print("    addr    stack prio refs     state time\r\n");
	tp = chRegFirstThread();
	do {
		print("%.8lx [%.8lx] %4lu %4lu %9s %lu %s\r\n", (uint32_t) tp, 0, (uint32_t) tp->p_prio,
				(uint32_t) (tp->p_refs - 1), states[tp->p_state], (uint32_t) tp->p_time, tp->p_name);
		tp = chRegNextThread(tp);
	} while (tp != NULL);
#endif
}

void send_thread_states()
{
  thread_t *tp = chRegFirstThread();
  while (tp) {
    msg_thread_state_t tp_state;
    u16 cpu = 1000.0f * tp->p_ctime / (float)g_ctime;
    tp_state.cpu = cpu;
    tp_state.stack_free = check_stack_free(tp);
    strncpy(tp_state.name, chRegGetThreadNameX(tp), sizeof(tp_state.name));
    sbp_send_msg(SBP_MSG_THREAD_STATE, sizeof(tp_state), (u8 *)&tp_state);

    tp->p_ctime = 0;  /* Reset thread CPU cycle count */
    tp = chRegNextThread(tp);
  }
  g_ctime = 0;
}

static void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[]) {
  static const char *states[] = {CH_STATE_NAMES};
  thread_t *tp;

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: threads\r\n");
    return;
  }
  chprintf(chp, "    addr    stack prio refs     state time\r\n");
  tp = chRegFirstThread();
  do {
    chprintf(chp, "%08lx %08lx %4lu %9s\r\n",
            (uint32_t)tp, (uint32_t)tp->ctx.sp,
            (uint32_t)tp->prio, states[tp->state]);
    tp = chRegNextThread(tp);
  } while (tp != NULL);
}

void send_thread_states()
{
  Thread *tp = chRegFirstThread();
  while (tp) {
    msg_thread_state_t tp_state;
    u16 cpu = 1000.0f * tp->p_ctime / (float)g_ctime;
    tp_state.cpu = cpu;
    tp_state.stack_free = check_stack_free(tp);
    strncpy(tp_state.name, chRegGetThreadName(tp), sizeof(tp_state.name));
    sbp_send_msg(MSG_THREAD_STATE, sizeof(tp_state), (u8 *)&tp_state);

    /* This works because chThdGetTicks is actually a define that pulls out a
     * value from a struct, hopefully if that fact changes then this statement
     * will no longer compile. */
    tp->p_ctime = 0;
    tp = chRegNextThread(tp);
  }
  g_ctime = 0;
}

static void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[]) {
  static const char *states[] = {THD_STATE_NAMES};
  Thread *tp;

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: threads\r\n");
    return;
  }
  chprintf(chp, "    addr    stack prio refs     state time\r\n");
  tp = chRegFirstThread();
  do {
    chprintf(chp, "%.8lx %.8lx %4lu %4lu %9s %lu\r\n",
            (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
            (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
            states[tp->p_state], (uint32_t)tp->p_time);
    tp = chRegNextThread(tp);
  } while (tp != NULL);
}

/*! \brief Show running threads
 *
 * @param chp
 * @param argc
 * @param argv
 */
void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[]) {
    (void)chp;
	static const char *states[] = {THD_STATE_NAMES};
	Thread *tp;

	(void)argv;
	if (argc > 0) {
		SHELLDBG("Usage: threads\r\n");
		return;
	}
	SHELLDBG("addr\t\tstack\t\tprio\trefs\tstate\t\ttime\tname\r\n");
	tp = chRegFirstThread();
	do {
		SHELLDBG("%.8lx\t%.8lx\t%4lu\t%4lu\t%9s\t%lu\t%s\r\n",
				(uint32_t)tp, (uint32_t)tp->p_ctx.r13,
				(uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
				states[tp->p_state], (uint32_t)tp->p_time, tp->p_name);
		tp = chRegNextThread(tp);
	} while (tp != NULL);
}

static void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[])
{
  static const char *states[] = {CH_STATE_NAMES};
  thread_t *tp;

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: threads\r\n");
    return;
  }
  chprintf(chp, " addr     pc       stack    prio refs state         name\r\n");
  tp = chRegFirstThread();
  do {
    chprintf(chp, " %-8lx %-8lx %-8lx %4lu %4lu %-12s  %-10s\r\n",
      (uint32_t)tp, (uint32_t)tp->p_ctx.r13->lr, (uint32_t)tp->p_ctx.r13,
      (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
      states[tp->p_state],
      tp->p_name);
    tp = chRegNextThread(tp);
  } while (tp != NULL);
}

static void cmd_threads( int argc, char *argv[] )
{
    static const char *states[] = { CH_STATE_NAMES };
    thread_t *tp;

    (void)argv;
    if( argc > 0 )
    {
        usage( "threads" );
        return;
    }

    chprint( "    addr    stack prio refs     state time\r\n" );
    tp = chRegFirstThread();
    do
    {
        chprint( "%08lx %08lx %4lu %4lu %9s\r\n",
                  (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 = chRegNextThread( tp );
    } while( tp != NULL );
}

static void cmd_threads(Stream *chp, int argc, char *argv[]) {
  static const char *states[] = {CH_STATE_NAMES};
  thread_t *tp;
  
  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: threads\r\n");
    return;
  }
  chprintf(chp, "stklimit    stack     addr refs prio     state  name\r\n\r\n");
  tp = chRegFirstThread();
  do {
    #if (CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE)
    uint32_t stklimit = (uint32_t)tp->wabase;
    #else
    uint32_t stklimit = 0U;
    #endif
    chprintf(chp, "%08lx %08lx %08lx %4lu %4lu %9s  %s\r\n",
             stklimit, (uint32_t)tp->ctx.sp, (uint32_t)tp, 
             (uint32_t)tp->refs - 1, (uint32_t)tp->prio, states[tp->state],
             tp->name == NULL ? "" : tp->name);
    tp = chRegNextThread(tp);
  } while (tp != NULL);
}

static msg_t rci_thread(void *p){
	chRegSetThreadName("RCI");

	struct RCICommand * commands = (struct RCICommand *)p;
	struct sockaddr from;
	socklen_t fromlen;

	struct sockaddr own;
	set_sockaddr(&own, "0.0.0.0", 23);

	char rx_buf[ETH_MTU];
	char tx_buf[ETH_MTU];

	int sock = socket(AF_INET, SOCK_STREAM, 0);
	chDbgAssert(sock >= 0, "Could not get RCI socket", NULL);

	int optval = 1;
	socklen_t optlen = sizeof(optval);
	setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, optlen);

	if(bind(sock, &own, sizeof(struct sockaddr_in)) < 0){
		perror("bind");
		chDbgPanic("Could not bind RCI socket");
	}
	if(listen(sock, 1) < 0){
		chDbgPanic("Could not listen on RCI socket");
	}

	while(TRUE) {
		struct RCICmdData cmd = {
			.name = NULL,
			.data = rx_buf,
			.len = 0,
		};
		struct RCIRetData ret = {
			.data = tx_buf,
			.len = 0,
		};

		fromlen = sizeof(from);
		int s = ACCEPT(sock, &from, &fromlen);
		if(s < 0){
			continue;
		}

		cmd.len = get_command(s, rx_buf, sizeof(rx_buf));
		if(cmd.len < 0){
			close(s);
			continue;
		}

		handle_command(&cmd, &ret, commands);

		//if there's data to return, return it to the address it came from
		send_response(s, ret.data, ret.len, sizeof(tx_buf));
		close(s);
	}
	return -1;
}

void RCICreate(struct RCICommand * cmd){
	chDbgAssert(cmd, "RCICreate needs a config", NULL);

#if 0 //FIXME: because of threads this doesn't actually work all the time.
	  //       What's a better way of finding lwip has started.
	/* Check if LWIP has been started */
	Thread * thd = chRegFirstThread();
	while(thd){
		if(!strcmp("lwipthread", thd->p_name)){
			break;
		}
		thd = chRegNextThread(thd);
	}
	chDbgAssert(thd, "RCICreate needs lwip started beforehand", NULL);
#endif
	chThdCreateStatic(wa_rci, sizeof(wa_rci), NORMALPRIO, rci_thread, (void *)cmd);
}

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
//.........这里部分代码省略.........

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", &current);
			sscanf(argv[2], "%f", &min_rpm);
			sscanf(argv[3], "%f", &low_duty);
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........