void cliFuncConfig(void *cmd, char *cmdLine) {
char param[8];
if (state > ESC_STATE_STOPPED) {
serialPrint(stopError);
}
else if (sscanf(cmdLine, "%8s", param) != 1) {
cliUsage((cliCommand_t *)cmd);
}
else if (!strcasecmp(param, "default")) {
configLoadDefault();
serialPrint("CONFIG: defaults loaded\r\n");
}
else if (!strcasecmp(param, "read")) {
configReadFlash();
serialPrint("CONFIG: read flash\r\n");
}
else if (!strcasecmp(param, "write")) {
if (configWriteFlash()) {
serialPrint("CONFIG: wrote flash\r\n");
}
else {
serialPrint("CONFIG: write flash failed!\r\n");
}
}
else {
cliUsage((cliCommand_t *)cmd);
}
}
void IMUTester::readSerialCommand() {
int serialData = Serial.read();
if (serialData != -1)
// clear some space
serialPrint("\n\n\n");
switch (serialData)
{
case -1:
break;
case 'i':
//initialize(Serial.read());
break;
case 'a':
reportAccel = toggle(reportAccel);
break;
case 'y':
reportGyro = toggle(reportGyro);
break;
case 'Y':
reportGyroAll = toggle(reportGyroAll);
break;
case 'g':
reportGPS = toggle(reportGPS);
break;
case 'c':
reportComp = toggle(reportComp);
break;
default:
serialPrint("unknown command: ");
serialPrintln(serialData);
}
}
int cliLs(const char ** argv)
{
FILINFO fno;
DIR dir;
char *fn; /* This function is assuming non-Unicode cfg. */
#if _USE_LFN
TCHAR lfn[_MAX_LFN + 1];
fno.lfname = lfn;
fno.lfsize = sizeof(lfn);
#endif
FRESULT res = f_opendir(&dir, argv[1]); /* Open the directory */
if (res == FR_OK) {
for (;;) {
res = f_readdir(&dir, &fno); /* Read a directory item */
if (res != FR_OK || fno.fname[0] == 0) break; /* Break on error or end of dir */
#if _USE_LFN
fn = *fno.lfname ? fno.lfname : fno.fname;
#else
fn = fno.fname;
#endif
serialPrint(fn);
}
}
else {
serialPrint("%s: Invalid directory \"%s\"", argv[0], argv[1]);
}
return 0;
}
void cliUsage(cliCommand_t *cmd) {
serialPrint("usage: ");
serialPrint(cmd->name);
serialWrite(' ');
serialPrint(cmd->params);
serialPrint("\r\n");
}
int cliDisplay(const char ** argv)
{
int address = 0;
for (const MemArea * area = memAreas; area->name != NULL; area++) {
if (!strcmp(area->name, argv[1])) {
dump((uint8_t *)area->start, area->size);
return 0;
}
}
if (!strcmp(argv[1], "adc")) {
extern uint16_t Analog_values[NUMBER_ANALOG];
for (int i=0; i<NUMBER_ANALOG; i++) {
serialPrint("adc[%d] = %04X", i, Analog_values[i]);
}
}
else if (!strcmp(argv[1], "outputs")) {
for (int i=0; i<NUM_CHNOUT; i++) {
serialPrint("outputs[%d] = %04X", i, channelOutputs[i]);
}
}
else if (!strcmp(argv[1], "time")) {
struct gtm utm;
gettime(&utm);
serialPrint("time = %4d-%02d-%02d %02d:%02d:%02d.%02d0", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
}
else if (toInt(argv, 1, &address) > 0) {
int size = 256;
if (toInt(argv, 2, &size) >= 0) {
dump((uint8_t *)address, size);
}
}
return 0;
}
int cliDisplay(const char ** argv)
{
int address = 0;
for (const MemArea * area = memAreas; area->name != NULL; area++) {
if (!strcmp(area->name, argv[1])) {
dump((uint8_t *)area->start, area->size);
return 0;
}
}
if (!strcmp(argv[1], "adc")) {
for (int i=0; i<NUMBER_ANALOG; i++) {
serialPrint("adc[%d] = %04X", i, Analog_values[i]);
}
}
else if (!strcmp(argv[1], "outputs")) {
for (int i=0; i<NUM_CHNOUT; i++) {
serialPrint("outputs[%d] = %04X", i, channelOutputs[i]);
}
}
else if (toInt(argv, 1, &address) > 0) {
int size = 256;
if (toInt(argv, 2, &size) >= 0) {
dump((uint8_t *)address, size);
}
}
return 0;
}
void cliFuncRpm(void *cmd, char *cmdLine) {
float target;
if (state < ESC_STATE_RUNNING) {
serialPrint(runError);
}
else {
if (sscanf(cmdLine, "%f", &target) != 1) {
cliUsage((cliCommand_t *)cmd);
}
else if (p[FF1TERM] == 0.0f) {
serialPrint("Calibration parameters required\r\n");
}
else if (target < 100.0f || target > 10000.0f) {
serialPrint("RPM out of range: 100 => 10000\r\n");
}
else {
if (runMode != CLOSED_LOOP_RPM) {
runRpmPIDReset();
runMode = CLOSED_LOOP_RPM;
}
targetRpm = target;
sprintf(tempBuf, "RPM set to %6.0f\r\n", target);
serialPrint(tempBuf);
}
}
}
int IMUTester::timeComp()
{
finish = millis();
serialPrint(comp.getRawX()); serialPrint(",");
serialPrint(comp.getRawY()); serialPrint(",");
return millis() - finish;
}
void cliPrintParam(int i) {
const char *format = "%-20s = ";
sprintf(tempBuf, format, configParameterStrings[i]);
serialPrint(tempBuf);
sprintf(tempBuf, configFormatStrings[i], p[i]);
serialPrint(tempBuf);
serialPrint("\r\n");
}
void cliFuncBinary(void *cmd, char *cmdLine) {
if (state > ESC_STATE_STOPPED) {
serialPrint(stopError);
}
else {
serialPrint("Entering binary command mode...\r\n");
cliTelemetry = 0;
commandMode = BINARY_MODE;
}
}
void cliInit(void) {
serialPrint(cliHome);
serialPrint(cliClear);
sprintf(version, "%s.%d", VERSION, getBuildNumber());
cliFuncVer(0, 0);
serialPrint("\r\nCLI ready.\r\n");
cliPrompt();
}
void cliFuncStop(void *cmd, char *cmdLine) {
if (state < ESC_STATE_NOCOMM) {
serialPrint(runError);
}
else {
runStop();
cliFuncChangeInput(ESC_INPUT_UART);
serialPrint("ESC stopping\r\n");
}
}
void cliFuncBoot(void *cmd, char *cmdLine) {
if (state != ESC_STATE_DISARMED) {
serialPrint("ESC armed, disarm first\r\n");
}
else {
serialPrint("Rebooting in boot loader mode...\r\n");
timerDelay(0xffff);
rccReset();
}
}
void cliFuncArm(void *cmd, char *cmdLine) {
if (state > ESC_STATE_DISARMED) {
serialPrint("ESC already armed\r\n");
}
else {
if (runMode != SERVO_MODE)
cliFuncChangeInput(ESC_INPUT_UART);
runArm();
serialPrint("ESC armed\r\n");
}
}
void cliFuncStart(void *cmd, char *cmdLine) {
if (state == ESC_STATE_DISARMED) {
serialPrint("ESC disarmed, arm first\r\n");
}
else if (state > ESC_STATE_STOPPED) {
serialPrint("ESC already running\r\n");
}
else {
runStart();
serialPrint("ESC started\r\n");
}
}
void cliFuncHelp(void *cmd, char *cmdLine) {
int i;
serialPrint("Available commands:\r\n\n");
for (i = 0; i < CLI_N_CMDS; i++) {
serialPrint(cliCommandTable[i].name);
serialWrite(' ');
serialPrint(cliCommandTable[i].params);
serialPrint("\r\n");
}
}
int cliHelp(const char ** argv)
{
for (const CliCommand * command = cliCommands; command->name != NULL; command++) {
if (argv[1][0] == '\0' || !strcmp(command->name, argv[0])) {
serialPrint("%s %s", command->name, command->args);
if (argv[1][0] != '\0') {
return 0;
}
}
}
if (argv[1][0] != '\0') {
serialPrint("Invalid command \"%s\"", argv[0]);
}
return -1;
}
void gpsInitNmea(void)
{
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
uint32_t now;
#endif
switch(gpsData.state) {
case GPS_INITIALIZING:
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
now = millis();
if (now - gpsData.state_ts < 1000)
return;
gpsData.state_ts = now;
if (gpsData.state_position < 1) {
serialSetBaudRate(gpsPort, 4800);
gpsData.state_position++;
} else if (gpsData.state_position < 2) {
// print our FIXED init string for the baudrate we want to be at
serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
gpsData.state_position++;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_CHANGE_BAUD);
}
break;
#endif
case GPS_CHANGE_BAUD:
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
now = millis();
if (now - gpsData.state_ts < 1000)
return;
gpsData.state_ts = now;
if (gpsData.state_position < 1) {
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
gpsData.state_position++;
} else if (gpsData.state_position < 2) {
serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
gpsData.state_position++;
} else {
#else
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
#endif
gpsSetState(GPS_RECEIVING_DATA);
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
}
#endif
break;
}
}
/*---------------------------------------------------------------------------
TITLE : cmd_bluetooth_check
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
void cmd_bluetooth_check( void )
{
uint32_t time_out;
uint8_t ch;
uint8_t ch_array[2];
uint8_t ch_i;
osStatus ret;
ret = osMutexWait( Mutex_Loop, 1000 );
if( ret != osOK )
{
_menu_printf("Fail to osMutexWait\r\n");
return;
}
core.blueport = uartOpen(USART2, NULL, 115200, MODE_RXTX);
_menu_printf("\r\n");
_menu_printf("AT -> ");
serialPrint(core.blueport, "AT");
ch_array[0] = 0;
ch_array[1] = 0;
ch_i = 0;
//-- 응답이 올때까지 기다림
time_out = 1000;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
_menu_putch(ch);
ch_array[ch_i++] = ch;
if( ch_i >= 2 ) break;
}
osDelay(1);
}
if( ch_array[0] == 'O' && ch_array[1] == 'K' )
{
_menu_printf("\r\nBluetooth OK");
}
else
{
_menu_printf("\r\nBluetooth Fail");
}
_menu_printf("\r\n");
serialInit(mcfg.serial_baudrate);
osMutexRelease( Mutex_Loop );
}
/**
* Writes an ADB message with payload to the ADB device.
*
* @param device USB device handle.
* @param command ADB command.
* @param arg0 first ADB argument (command dependent).
* @param arg0 second ADB argument (command dependent).
* @param length payload length.
* @param data command payload.
* @return error code or 0 for success.
*/
int ADB::writeMessage(usb_device * device, uint32_t command, uint32_t arg0, uint32_t arg1, uint32_t length, uint8_t * data)
{
adb_message message;
uint32_t count, sum = 0;
uint8_t * x;
uint8_t rcode;
// Calculate data checksum
count = length;
x = data;
while(count-- > 0) sum += *x++;
// Fill out the message record.
message.command = command;
message.arg0 = arg0;
message.arg1 = arg1;
message.data_length = length;
message.data_check = sum;
message.magic = command ^ 0xffffffff;
#ifdef DEBUG
serialPrint("OUT << "); adb_printMessage(&message);
#endif
rcode = USB::bulkWrite(device, sizeof(adb_message), (uint8_t*)&message);
if (rcode) return rcode;
rcode = USB::bulkWrite(device, length, data);
return rcode;
}
void cliFuncChangeInput(uint8_t input) {
if (inputMode != input) {
inputMode = input;
sprintf(tempBuf, "Input mode set to %s\r\n", cliInputModes[input]);
serialPrint(tempBuf);
}
}
void pinMode(uint8_t pin, uint8_t mode)
{
serialPrint("Pin Number - %u is set to ", unsigned(pin));
if (mode == INPUT)
serialPrintln("INPUT");
else
serialPrintln("OUTPUT");
}
void digitalWrite(uint8_t pin, uint8_t val)
{
serialPrint("Pin Number - %u is ", unsigned(pin));
if (val == HIGH)
serialPrintln("HIGH");
else
serialPrintln("LOW");
}
static void gpsInitHardware(void)
{
switch (mcfg.gps_type) {
case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsSetState(GPS_RECEIVINGDATA);
break;
case GPS_UBLOX:
// UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate
// Wait until GPS transmit buffer is empty
if (!isSerialTransmitBufferEmpty(core.gpsport))
break;
if (gpsData.state == GPS_INITIALIZING) {
uint32_t m = millis();
if (m - gpsData.state_ts < GPS_BAUD_DELAY)
return;
if (gpsData.state_position < GPS_INIT_ENTRIES) {
// try different speed to INIT
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.state_position].baudrate);
// but print our FIXED init string for the baudrate we want to be at
serialPrint(core.gpsport, gpsInitData[gpsData.baudrateIndex].ubx);
gpsData.state_position++;
gpsData.state_ts = m;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsSetState(GPS_INITDONE);
}
} else {
// GPS_INITDONE, set our real baud rate and push some ublox config strings
if (gpsData.state_position == 0)
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(core.gpsport, ubloxInit[gpsData.state_position]); // send ubx init binary
gpsData.state_position++;
} else {
// ublox should be init'd, time to try receiving some junk
gpsSetState(GPS_RECEIVINGDATA);
}
}
break;
case GPS_MTK_NMEA:
case GPS_MTK_BINARY:
// TODO. need to find my old piece of shit MTK GPS.
break;
}
// clear error counter
gpsData.errors = 0;
}
void productionDebug(void)
{
gpio_config_t gpio;
// remap PB6 to USART1_TX
gpio.pin = Pin_6;
gpio.mode = Mode_AF_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true);
serialInit(mcfg.serial_baudrate);
delay(25);
serialPrint(core.mainport, "DBG ");
printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2);
serialPrint(core.mainport, "EOF");
delay(25);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false);
}
void ssMainUpdate() {
if (activeSendFlag) {
uint8_t pacLen = 0;
uint8_t inband = 0;
ssActiveSend(decodedBuffer, &pacLen, &inband);
// Send reply active packet
if (pacLen > 0 && pacLen <= ACTIVE_PACKET_MAX_LEN) {
encodedBuffer[0] = 0x00;
encodedBuffer[1] = packetType;
encodedBuffer[2] = ((!!inband) << 7) | (pacLen+4);
cobs_encode(encodedBuffer+3, decodedBuffer, pacLen);
serialPrint(encodedBuffer, pacLen+4);
}
activeSendFlag = 0;
}
if (rxBufferFlag == RX_FLAG_DATA_AVAILABLE) { // if done receiving
rxBufferFlag = RX_FLAG_DATA_IN_USE;
if (dataLen >= 1) {
cobs_decode(decodedBuffer, rxBuffer, dataLen);
}
if (packetType < 0x80) {
// Read game mode from 0th channel
if (decodedBuffer[0] <= MAX_MODE) gameMode = decodedBuffer[0];
// Read other channels
ssActiveInRec(decodedBuffer+1, dataLen-2, in_band_sigFlag);
} else if (packetType <= 0xFD && packetType >= 0xF0) {
enumerationPacket(packetType, decodedBuffer, dataLen-1);
} else {
uint8_t pacLen = 0;
maintenancePacket(packetType, decodedBuffer, dataLen-1,
decodedBuffer, &pacLen);
// Send reply maintenance packet
if (pacLen > 0 && pacLen <= 0xFF-4) {
encodedBuffer[0] = 0x00;
encodedBuffer[1] = packetType; // Respond with same type as sent.
encodedBuffer[2] = pacLen+4;
cobs_encode(encodedBuffer+3, decodedBuffer, pacLen);
serialPrint(encodedBuffer, pacLen+4);
}
}
rxBufferFlag = RX_FLAG_READY; // should be below if statement
}
}
void cliFuncDuty(void *cmd, char *cmdLine) {
float duty;
if (state < ESC_STATE_RUNNING) {
serialPrint(runError);
}
else {
if (sscanf(cmdLine, "%f", &duty) != 1) {
cliUsage((cliCommand_t *)cmd);
}
else if (!runDuty(duty)) {
serialPrint("duty out of range: 0 => 100\r\n");
}
else {
sprintf(tempBuf, "Fet duty set to %.2f%%\r\n", (float)fetDutyCycle/fetPeriod*100.0f);
serialPrint(tempBuf);
}
}
}
int main(void) {
// Stop whatchdog
WDTCTL = WDTPW + WDTHOLD;
// Set clock to 1 MHz
BCSCTL1 = CALBC1_1MHZ;
DCOCTL = CALDCO_1MHZ;
if (DEBUG) SerialBegin(SERIALSPEED, CLK_SPEED);
// Init the Ports and the timers
initPorts();
if (DEBUG) {
serialPrintln("Init Done!");
serialPrint("CLK at: ");
serialPrintInt(CLK_SPEED);
serialPrintln(" MHz");
serialPrint("PWM at: ");
serialPrintInt(PWM_FREQUZENCY);
serialPrintln(" kHz");
serialPrint("Periode: ");
serialPrintInt(PWM_PERIODE);
serialPrintln(" clkCycles");
}
// Do forever and for always
while(1) {
// If Push Button pressed
if (!(P1IN & BIT3)) {
// Send specifid Code
sendCode(CODE);
// Display sending with LED on
P1OUT |= BIT0;
// Wait so that no Burst can be sended
_delay_cycles(DELAY_BETWEEN_TWO_SENDS);
} else {
// Turn LED off
P1OUT &= ~BIT0;
}
}
}
int cliVolume(const char ** argv)
{
int level = 0;
if (toInt(argv, 1, &level) > 0) {
}
else {
serialPrint("%s: Invalid argument \"%s\"", argv[0], argv[1]);
}
return 0;
}
void cliFuncTelemetry(void *cmd, char *cmdLine) {
uint16_t freq;
if (sscanf(cmdLine, "%hu", &freq) != 1) {
cliUsage((cliCommand_t *)cmd);
}
else if (freq > 100) {
serialPrint("Frequency out of range: 0 => 100\r\n");
}
else {
if (freq > 0) {
cliTelemetry = RUN_FREQ/freq;
serialPrint(cliHome);
serialPrint(cliClear);
serialWrite('\n');
}
else
cliTelemetry = 0;
}
}
