add distance processing
This commit is contained in:
parent
09416c4113
commit
67b9187312
GPG Key ID:
FF51F4E4BCE459EE
94
src/main.c
94
src/main.c
|
|
@ -10,6 +10,9 @@
|
|||
// UART buffer size
|
||||
#define BUFFER_LEN 48
|
||||
|
||||
#define AMOUNT_DISTANCE_MEASURE 8
|
||||
#define ACTIVATION_DISTANCE 40
|
||||
|
||||
// LED pin info
|
||||
#define LED_PIN_DIR PORT_C.DDR.Pin3
|
||||
#define LED_PIN_CR1 PORT_C.CR1.Pin3
|
||||
|
|
@ -28,6 +31,11 @@
|
|||
#define TX_PIN_CR2 PORT_D.CR2.Pin5
|
||||
#define TX_PIN_OUT PORT_D.ODR.Pin5
|
||||
|
||||
#define PRESS_ACTIVATION_TIME 30 * 4
|
||||
#define SENSE_ACTIVATION_TIME PRESS_ACTIVATION_TIME
|
||||
|
||||
const char *RANGE = "Range ";
|
||||
|
||||
// Time defines
|
||||
#define SHORT_PRESS_MS 25
|
||||
#define LONG_PRESS_MS 1000
|
||||
|
|
@ -48,8 +56,10 @@ volatile u08 tx_buffer[BUFFER_LEN];
|
|||
volatile u08 tx_pos = 0;
|
||||
volatile u08 tx_len = 0;
|
||||
|
||||
volatile u08 state = 0;
|
||||
u08 state = 0;
|
||||
u08 led_state = 0;
|
||||
u16 last_distance[AMOUNT_DISTANCE_MEASURE] = {0};
|
||||
u08 distance_count = 0;
|
||||
|
||||
/**
|
||||
* Hang processor with big delay and hope for watchdog to kick-in or directly
|
||||
|
|
@ -119,6 +129,9 @@ void send_some_command(void) {
|
|||
start_cmd(3);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialization state process
|
||||
*/
|
||||
void initialize_state(void) {
|
||||
switch (state) {
|
||||
case 0:
|
||||
|
|
@ -169,6 +182,25 @@ void turn_off(void) {
|
|||
LED_PIN_OUT = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate average distance from last N measurements
|
||||
* Note: returns 0 if no measurements done
|
||||
*/
|
||||
u16 avg_distance(void) {
|
||||
u16 sum = 0;
|
||||
u08 cnt = 0;
|
||||
for (u08 i = 0; i < AMOUNT_DISTANCE_MEASURE; i++) {
|
||||
sum += last_distance[i];
|
||||
if (last_distance[i] > 0) {
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
if (cnt > 0) {
|
||||
return sum / cnt;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Tick event handler
|
||||
*/
|
||||
|
|
@ -183,6 +215,12 @@ void tick_250ms(void) {
|
|||
if (timer > 0) {
|
||||
timer--;
|
||||
}
|
||||
if (timer == 0) {
|
||||
u16 distance = avg_distance();
|
||||
if (distance != 0 && distance < ACTIVATION_DISTANCE) {
|
||||
timer = SENSE_ACTIVATION_TIME;
|
||||
}
|
||||
}
|
||||
// If we still have time
|
||||
// note: timer can be below zero to work without timer
|
||||
if (timer != 0) {
|
||||
|
|
@ -199,11 +237,20 @@ void tick_250ms(void) {
|
|||
*/
|
||||
int is_rx_finished(void) {
|
||||
// head + len + cmd + ver + tail
|
||||
if (rx_pos < 4 + 2 + 2 + 2 + 4) {
|
||||
if (rx_pos < 6 || (rx_buffer[0] != RANGE[0] && rx_pos < 4 + 2 + 2 + 2 + 4)) {
|
||||
return false;
|
||||
}
|
||||
// Check head, tail and just in case - length
|
||||
return rx_buffer[0] == 0xFD &&
|
||||
return
|
||||
(rx_buffer[0] == RANGE[0] &&
|
||||
rx_buffer[1] == RANGE[1] &&
|
||||
rx_buffer[2] == RANGE[2] &&
|
||||
rx_buffer[3] == RANGE[3] &&
|
||||
rx_buffer[4] == RANGE[4] &&
|
||||
rx_buffer[5] == RANGE[5] &&
|
||||
(rx_buffer[rx_pos - 1] == '\r' ||
|
||||
rx_buffer[rx_pos - 1] == '\n')) ||
|
||||
(rx_buffer[0] == 0xFD &&
|
||||
rx_buffer[1] == 0xFC &&
|
||||
rx_buffer[2] == 0xFB &&
|
||||
rx_buffer[3] == 0xFA &&
|
||||
|
|
@ -211,7 +258,39 @@ int is_rx_finished(void) {
|
|||
rx_buffer[rx_pos - 2] == 0x02 &&
|
||||
rx_buffer[rx_pos - 3] == 0x03 &&
|
||||
rx_buffer[rx_pos - 4] == 0x04 &&
|
||||
(rx_buffer[4] | (rx_buffer[5] << 8)) == rx_pos - 12;
|
||||
(rx_buffer[4] | (rx_buffer[5] << 8)) == rx_pos - 12);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add distance measurement to list
|
||||
*/
|
||||
void add_distance(u16 distance) {
|
||||
last_distance[distance_count] = distance;
|
||||
distance_count++;
|
||||
if (distance_count >= AMOUNT_DISTANCE_MEASURE) {
|
||||
distance_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Process input from UART
|
||||
*/
|
||||
void process_rx(void) {
|
||||
if (rx_buffer[0] == RANGE[0]) {
|
||||
u16 distance = 0;
|
||||
for (u08 i = 6; i <= rx_pos; i++) {
|
||||
if (rx_buffer[i] >= '0' && rx_buffer[i] <= '9') {
|
||||
distance = distance * 10 + (rx_buffer[i] - '0');
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (distance != 0) {
|
||||
add_distance(distance);
|
||||
}
|
||||
} else {
|
||||
// TODO: Process command packet
|
||||
}
|
||||
}
|
||||
|
||||
interrupt(IRQ_UART1_RX_F, uart_recv) {
|
||||
|
|
@ -222,7 +301,7 @@ interrupt(IRQ_UART1_RX_F, uart_recv) {
|
|||
}
|
||||
|
||||
if (rx_pos < 4) {
|
||||
if (rx_pos == 0xFD - c) {
|
||||
if (rx_pos == 0xFD - c || RANGE[rx_pos] == c) {
|
||||
rx_buffer[rx_pos] = c;
|
||||
} else {
|
||||
// Something wrong - reset
|
||||
|
|
@ -232,7 +311,8 @@ interrupt(IRQ_UART1_RX_F, uart_recv) {
|
|||
} else {
|
||||
rx_buffer[rx_pos] = c;
|
||||
if (is_rx_finished()) {
|
||||
// TODO: Process
|
||||
process_rx();
|
||||
// Reset buffer
|
||||
rx_pos = 0;
|
||||
memset(rx_buffer, 0, BUFFER_LEN);
|
||||
return;
|
||||
|
|
@ -267,7 +347,7 @@ interrupt(IRQ_EXTI_C, touch) {
|
|||
timer = -1;
|
||||
} else if (time > SHORT_PRESS_MS) {
|
||||
// If it was short touch set timer for enough time
|
||||
timer = 30 * 4; // 30 seconds * 4 (250ms tick)
|
||||
timer = PRESS_ACTIVATION_TIME; // 30 seconds * 4 (250ms tick)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue