void esc32_periodic(void) {
if (esc32_priv.initialized == false) {
uart_put_byte(esc32_priv.dev, 0, '\r');
// enter binary mode
uint8_t data[] = "binary\r";
int i = 0;
while (i < (int) sizeof(data)) {
uart_put_byte(esc32_priv.dev, 0, data[i]);
i++;
}
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_RATE, 0.0, 0.0, 1);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 0.0, BINARY_VALUE_AMPS, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 1.0, BINARY_VALUE_VOLTS_BAT, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 2.0, BINARY_VALUE_RPM, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 3.0, BINARY_VALUE_VOLTS_MOTOR, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_RATE, ESC32_TELEMETRY_RATE, 0.0, 1);
esc32_priv.initialized = true;
}
else {
#if ESC32_DEBUG
if (!debug_start) {
// motor spining for debug
esc32_send_command(&esc32_priv, BINARY_COMMAND_ARM, 0.0, 0.0, 0);
esc32_send_command(&esc32_priv, BINARY_COMMAND_STOP, 0.0, 0.0, 0);
esc32_send_command(&esc32_priv, BINARY_COMMAND_START, 0.0, 0.0, 0);
esc32_send_command(&esc32_priv, BINARY_COMMAND_DUTY, 8.0, 0.0, 1);
debug_start = 1;
}
#endif
}
}
void drop_ball(uint8_t number) {
for(uint8_t i = 0; i < DROP_STRINGLEN; i++)
uart_put_byte(&UART_DROP_PORT, 0, drop_string[i]);
uint8_t last = '>';
if(number == 1) {
last = '1';
} else if(number == 2) {
last = '2';
} else if(number == 3) {
last = '3';
} else if(number == 4) {
last = '4';
}
uart_put_byte(&UART_DROP_PORT, 0, last);
}
void
uart_put_str(U32 u, const U8 *str)
{
while (*str) {
while (!uart_put_byte(u, *str)) {
}
str++;
}
}
static void esc32_send(struct esc32_private *esc) {
int i;
for (i = 0; i < esc->out_idx; i++) {
uart_put_byte(esc->dev, 0, esc->buf_out[i]);
}
}
/**
* @brief sdlogger_spi_direct_periodic
* Periodic function called at module frequency
*/
void sdlogger_spi_direct_periodic(void)
{
sdcard_spi_periodic(&sdcard1);
#if SDLOGGER_ON_ARM
if(autopilot_motors_on) {
sdlogger_spi.do_log = 1;
} else {
sdlogger_spi.do_log = 0;
}
#endif
switch (sdlogger_spi.status) {
case SDLogger_Initializing:
if (sdcard1.status == SDCard_Idle) {
sdcard_spi_read_block(&sdcard1, 0x00002000, &sdlogger_spi_direct_index_received);
sdlogger_spi.status = SDLogger_RetreivingIndex;
}
break;
case SDLogger_Ready:
if ((sdlogger_spi.do_log == 1) &&
sdcard1.status == SDCard_Idle) {
LOGGER_LED_ON;
sdcard_spi_multiwrite_start(&sdcard1, sdlogger_spi.next_available_address);
sdlogger_spi.status = SDLogger_Logging;
}
break;
case SDLogger_Logging:
/* This line is NOT unit-tested because it is an inline function */
#if PERIODIC_TELEMETRY
periodic_telemetry_send_Logger(DefaultPeriodic,
&pprzlog_tp.trans_tx,
&sdlogger_spi.device);
#endif
/* Check if SD Card buffer is full and SD Card is ready for new data */
if (sdlogger_spi.sdcard_buf_idx > 512 &&
sdcard1.status == SDCard_MultiWriteIdle) {
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
}
/* Check if switch is flipped to stop logging */
if (sdlogger_spi.do_log == 0) {
sdlogger_spi.status = SDLogger_LoggingFinalBlock;
}
break;
case SDLogger_LoggingFinalBlock:
if (sdcard1.status == SDCard_MultiWriteIdle) {
if (sdlogger_spi.sdcard_buf_idx > 512) {
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
}
else if (sdlogger_spi.sdcard_buf_idx > 1) {
/* Fill with trailing zero's */
for (uint16_t i = sdlogger_spi.sdcard_buf_idx; i < (SD_BLOCK_SIZE+1); i++) {
sdcard1.output_buf[i] = 0x00;
}
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
}
else if (sdlogger_spi.sdcard_buf_idx == 1) {
sdcard_spi_multiwrite_stop(&sdcard1);
sdlogger_spi.status = SDLogger_StoppedLogging;
}
}
break;
case SDLogger_StoppedLogging:
if (sdcard1.status == SDCard_Idle) {
sdcard_spi_read_block(&sdcard1, 0x00002000, &sdlogger_spi_direct_index_received);
sdlogger_spi.status = SDLogger_GettingIndexForUpdate;
}
break;
case SDLogger_UpdatingIndex:
if (sdcard1.status == SDCard_Idle) {
LOGGER_LED_OFF;
sdlogger_spi.status = SDLogger_Ready;
}
break;
case SDLogger_Downloading:
if (sdcard1.status == SDCard_Idle) {
/* Put bytes to the buffer until all is written or buffer is full */
for (uint16_t i = sdlogger_spi.sdcard_buf_idx; i < SD_BLOCK_SIZE; i++) {
long fd = 0;
if (uart_check_free_space(&(DOWNLINK_DEVICE), &fd, 1)) {
uart_put_byte(&(DOWNLINK_DEVICE), fd, sdcard1.input_buf[i]);
}
else {
/* No free space left, abort for-loop */
break;
}
sdlogger_spi.sdcard_buf_idx++;
}
/* Request next block if entire buffer was written to uart */
if (sdlogger_spi.sdcard_buf_idx >= SD_BLOCK_SIZE) {
if (sdlogger_spi.download_length > 0) {
sdcard_spi_read_block(&sdcard1, sdlogger_spi.download_address, NULL);
sdlogger_spi.download_address++;
sdlogger_spi.download_length--;
}
else {
LOGGER_LED_OFF;
sdlogger_spi.status = SDLogger_Ready;
}
sdlogger_spi.sdcard_buf_idx = 0;
}
}
break;
default:
break;
}
}
inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length)
{
for (int i = 0; i < packet_length; i++) {
uart_put_byte(&(UM6_LINK), 0, packet_buffer[i]);
}
}
