Add simple loop for updating the serial link
This commit is contained in:
parent
17a1102a2a
commit
9cb139adc4
|
@ -22,19 +22,83 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||||
SOFTWARE.
|
SOFTWARE.
|
||||||
*/
|
*/
|
||||||
#include "serial_link/system/system.h"
|
#include "serial_link/system/system.h"
|
||||||
|
#include "hal.h"
|
||||||
|
#include "serial_link/protocol/byte_stuffer.h"
|
||||||
|
#include "serial_link/protocol/transport.h"
|
||||||
|
#include "serial_link/protocol/frame_router.h"
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
static event_source_t new_data_event;
|
||||||
|
|
||||||
|
|
||||||
|
// Slow speed for testing
|
||||||
|
static SerialConfig config = {
|
||||||
|
.sc_speed = 38400
|
||||||
|
};
|
||||||
|
|
||||||
|
static uint32_t read_from_serial(SerialDriver* driver) {
|
||||||
|
const uint32_t buffer_size = 16;
|
||||||
|
uint8_t buffer[buffer_size];
|
||||||
|
uint32_t bytes_read = sdAsynchronousRead(driver, buffer, buffer_size);
|
||||||
|
uint8_t* current = buffer;
|
||||||
|
uint8_t* end = current + bytes_read;
|
||||||
|
while(current < end) {
|
||||||
|
byte_stuffer_recv_byte(0, *current);
|
||||||
|
current++;
|
||||||
|
}
|
||||||
|
return bytes_read;
|
||||||
|
}
|
||||||
|
|
||||||
// TODO: Optimize the stack size, this is probably way too big
|
// TODO: Optimize the stack size, this is probably way too big
|
||||||
static THD_WORKING_AREA(serialThreadStack, 1024);
|
static THD_WORKING_AREA(serialThreadStack, 1024);
|
||||||
static THD_FUNCTION(serialThread, arg) {
|
static THD_FUNCTION(serialThread, arg) {
|
||||||
(void)arg;
|
(void)arg;
|
||||||
|
event_listener_t new_data_listener;
|
||||||
|
event_listener_t sd1_listener;
|
||||||
|
event_listener_t sd2_listener;
|
||||||
|
chEvtRegister(&new_data_event, &new_data_listener, 0);
|
||||||
|
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD1),
|
||||||
|
&sd1_listener,
|
||||||
|
EVENT_MASK(1),
|
||||||
|
CHN_INPUT_AVAILABLE);
|
||||||
|
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD2),
|
||||||
|
&sd2_listener,
|
||||||
|
EVENT_MASK(2),
|
||||||
|
CHN_INPUT_AVAILABLE);
|
||||||
|
bool need_wait = false;
|
||||||
|
while(true) {
|
||||||
|
if (need_wait) {
|
||||||
|
chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
|
||||||
|
}
|
||||||
|
bool is_master = usbGetDriverStateI(&USBD1) == USB_ACTIVE;
|
||||||
|
router_set_master(is_master);
|
||||||
|
|
||||||
|
need_wait = true;
|
||||||
|
need_wait &= read_from_serial(&SD1) == 0;
|
||||||
|
need_wait &= read_from_serial(&SD2) == 0;
|
||||||
|
update_transport();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
|
||||||
|
if (link == 0) {
|
||||||
|
sdWrite(&SD1, data, size);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
sdWrite(&SD2, data, size);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void init_serial_link(void) {
|
void init_serial_link(void) {
|
||||||
|
init_byte_stuffer();
|
||||||
|
sdStart(&SD1, &config);
|
||||||
|
sdStart(&SD2, &config);
|
||||||
|
chEvtObjectInit(&new_data_event);
|
||||||
(void)chThdCreateStatic(serialThreadStack, sizeof(serialThreadStack),
|
(void)chThdCreateStatic(serialThreadStack, sizeof(serialThreadStack),
|
||||||
LOWPRIO, serialThread, NULL);
|
LOWPRIO, serialThread, NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
void signal_data_written(void) {
|
void signal_data_written(void) {
|
||||||
|
chEvtBroadcast(&new_data_event);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue