static THD_FUNCTION(uart_thread, arg)
{
(void)arg;
chRegSetThreadName("UART thread");
// KPA.
event_listener_t kpa_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD1), &kpa_event_listener, EVENT_MASK(1), CHN_INPUT_AVAILABLE);
struct writer kpa_writer;
writer_init(&kpa_writer, &SD1, uart_write);
// FBV.
event_listener_t fbv_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD6), &fbv_event_listener, EVENT_MASK(2), CHN_INPUT_AVAILABLE);
struct writer fbv_writer;
writer_init(&fbv_writer, &SD6, uart_write);
// Bridge-
struct bridge bridge;
bridge_init(&bridge, &kpa_writer, &fbv_writer);
uint8_t byte = 0;
while (true)
{
eventflags_t evt = chEvtWaitAnyTimeout(EVENT_MASK(1) | EVENT_MASK(2), MS2ST(1));
if (evt & EVENT_MASK(1))
{
chEvtGetAndClearFlags(&kpa_event_listener);
while (readByte(&SD1, &byte))
bridge_update_kpa(&bridge, byte);
}
if (evt & EVENT_MASK(2))
{
chEvtGetAndClearFlags(&fbv_event_listener);
while (readByte(&SD6, &byte))
bridge_update_fbv(&bridge, byte);
}
bridge_update_time(&bridge, clock_get_ms());
}
}
static THD_FUNCTION(serialThread, 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);
eventflags_t events = CHN_INPUT_AVAILABLE
| SD_PARITY_ERROR | SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR | SD_BREAK_DETECTED;
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD1),
&sd1_listener,
EVENT_MASK(1),
events);
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD2),
&sd2_listener,
EVENT_MASK(2),
events);
bool need_wait = false;
while(true) {
eventflags_t flags1 = 0;
eventflags_t flags2 = 0;
if (need_wait) {
eventmask_t mask = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
if (mask & EVENT_MASK(1)) {
flags1 = chEvtGetAndClearFlags(&sd1_listener);
print_error("DOWNLINK", flags1, &SD1);
}
if (mask & EVENT_MASK(2)) {
flags2 = chEvtGetAndClearFlags(&sd2_listener);
print_error("UPLINK", flags2, &SD2);
}
}
// Always stay as master, even if the USB goes into sleep mode
is_master |= usbGetDriverStateI(&USBD1) == USB_ACTIVE;
router_set_master(is_master);
need_wait = true;
need_wait &= read_from_serial(&SD2, UP_LINK) == 0;
need_wait &= read_from_serial(&SD1, DOWN_LINK) == 0;
update_transport();
}
}
static THD_FUNCTION( esp8266, arg )
{
(void)arg;
chRegSetThreadName( "esp8266" );
event_listener_t serialListener;
/* Registering on the serial driver 6 as event 1, interested in
* error flags and data-available only, other flags will not wakeup
* the thread.
*/
chEvtRegisterMaskWithFlags(
(struct event_source_t *)chnGetEventSource( &SD6 ),
&serialListener,
EVENT_MASK( 1 ),
SD_FRAMING_ERROR | SD_PARITY_ERROR |
CHN_INPUT_AVAILABLE );
while( true )
{
// Waiting for any of the events we're registered on.
eventmask_t evt = chEvtWaitAny( ALL_EVENTS );
// Serving events.
if( evt & EVENT_MASK(1) )
{
/* Event from the serial interface, getting serial
* flags as first thing.
*/
eventflags_t flags = chEvtGetAndClearFlags( &serialListener );
//Handling errors first.
if( flags & (SD_FRAMING_ERROR | SD_PARITY_ERROR) )
{
DPRINT( 4, KRED "FRAMING/PARITY ERROR" );
}
if( flags & CHN_INPUT_AVAILABLE )
{
char c;
c = sdGet( &SD6 );
sdPut( &SD3, c );
}
}
}
}
static THD_FUNCTION(stream, arg)
{
(void)arg;
chRegSetThreadName("stream");
static event_listener_t sensor_event_listener;
chEvtRegisterMaskWithFlags(&sensor_events, &sensor_event_listener,
(eventmask_t)ONBOARDSENSOR_EVENT,
(eventflags_t)SENSOR_EVENT_MPU6000 | SENSOR_EVENT_H3LIS331DL | SENSOR_EVENT_HMC5883L | SENSOR_EVENT_MS5611);
static char dtgrm[100];
static cmp_mem_access_t mem;
static cmp_ctx_t cmp;
while (1) {
eventmask_t events = chEvtWaitAny(ONBOARDSENSOR_EVENT);
if (events & ONBOARDSENSOR_EVENT) {
eventflags_t event_flags = chEvtGetAndClearFlags(&sensor_event_listener);
if (event_flags & SENSOR_EVENT_MPU6000) {
rate_gyro_sample_t gyro;
accelerometer_sample_t acc;
onboard_sensor_get_mpu6000_gyro_sample(&gyro);
onboard_sensor_get_mpu6000_acc_sample(&acc);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "imu");
err = err || !cmp_write_map(&cmp, 3);
err = err || !CMP_WRITE_C_STRING(cmp, "gyro");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, gyro.rate[0]);
err = err || !cmp_write_float(&cmp, gyro.rate[1]);
err = err || !cmp_write_float(&cmp, gyro.rate[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "acc");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, acc.acceleration[0]);
err = err || !cmp_write_float(&cmp, acc.acceleration[1]);
err = err || !cmp_write_float(&cmp, acc.acceleration[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, gyro.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
err = false;
float att[4];
attitude_determination_get_attitude(att);
err = err || !msg_header_write(&cmp, "att");
err = err || !cmp_write_array(&cmp, 4);
err = err || !cmp_write_float(&cmp, att[0]);
err = err || !cmp_write_float(&cmp, att[1]);
err = err || !cmp_write_float(&cmp, att[2]);
err = err || !cmp_write_float(&cmp, att[3]);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_HMC5883L) {
magnetometer_sample_t magnetometer;
onboard_sensor_get_hmc5883l_mag_sample(&magnetometer);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "mag");
err = err || !cmp_write_map(&cmp, 2);
err = err || !CMP_WRITE_C_STRING(cmp, "field");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[0]);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[1]);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, magnetometer.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_H3LIS331DL) {
accelerometer_sample_t acc;
onboard_sensor_get_h3lis331dl_acc_sample(&acc);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "hi_acc");
err = err || !cmp_write_map(&cmp, 2);
err = err || !CMP_WRITE_C_STRING(cmp, "acc");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, acc.acceleration[0]);
err = err || !cmp_write_float(&cmp, acc.acceleration[1]);
err = err || !cmp_write_float(&cmp, acc.acceleration[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, acc.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_MS5611) {
barometer_sample_t baro;
onboard_sensor_get_ms5511_baro_sample(&baro);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "baro");
err = err || !cmp_write_map(&cmp, 3);
err = err || !CMP_WRITE_C_STRING(cmp, "static_press");
err = err || !cmp_write_float(&cmp, baro.pressure);
err = err || !CMP_WRITE_C_STRING(cmp, "air_temp");
err = err || !cmp_write_float(&cmp, baro.temperature);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, baro.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
} // ONBOARDSENSOR_EVENT
}
}
/**
* @brief Main estimation thread.
*
* @param[in/out] arg Unused.
*/
static THD_FUNCTION(ThreadEstimation, arg)
{
(void)arg;
chRegSetThreadName("Estimation");
tp = chThdGetSelfX();
/* Initialization data */
//uint32_t i;
//quaternion_t q_init = {1.0f, 0.0f, 0.0f, 0.0f};
//vector3f_t am, wb_init = {0.0f, 0.0f, 0.0f}; //, acc_init, mag_init;
/* Event registration for new sensor data */
event_listener_t el;
/* Register to new data from accelerometer and gyroscope */
chEvtRegisterMaskWithFlags(ptrGetNewDataEventSource(),
&el,
EVENT_MASK(0),
ACCGYRO_DATA_AVAILABLE_EVENTMASK |
MAG_DATA_AVAILABLE_EVENTMASK |
BARO_DATA_AVAILABLE_EVENTMASK);
/* Force an initialization of the estimation */
//chEvtAddEvents(ESTIMATION_RESET_EVENTMASK);
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
vInitializeMotionCaptureEstimator(&states);
while(1)
{
//if (isnan(states.q.q0) || isnan(states.q.q1) || isnan(states.q.q2) || isnan(states.q.q3) ||
// isnan(states.w.x) || isnan(states.w.y) || isnan(states.w.z) ||
// isnan(states.wb.x) || isnan(states.wb.y) || isnan(states.wb.z))
// AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
/* Check if there has been a request to reset the filter */
//if (chEvtWaitOneTimeout(ESTIMATION_RESET_EVENTMASK, TIME_IMMEDIATE))
// {
/* Initialize the estimation */
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
//}
/* Wait for new measurement data */
chEvtWaitAny(ALL_EVENTS);
eventflags_t flags = chEvtGetAndClearFlags(&el);
if (flags & ACCGYRO_DATA_AVAILABLE_EVENTMASK)
{
/* Get sensor data */
GetIMUData(&imu_data);
/* Run estimation */
vInnovateMotionCaptureEstimator(&states,
&imu_data,
SENSOR_ACCGYRO_DT,
0.0007f);
/*InnovateAttitudeEKF(&states,
&data,
imu_data.gyroscope,
imu_data.accelerometer,
imu_data.magnetometer,
0.0f,
0.0f,
ESTIMATION_DT);*/
//states.w.x = -imu_data.gyroscope[0];
//states.w.y = -imu_data.gyroscope[1];
//states.w.z = imu_data.gyroscope[2];
//am.x = -imu_data.accelerometer[0];
//am.y = -imu_data.accelerometer[1];
//am.z = imu_data.accelerometer[2];
//states.q = MadgwickAHRSupdateIMU(states.w,
// am,
// states.q,
// 0.15f,
// dt);
/* Broadcast new estimation available */
chEvtBroadcastFlags(&estimation_events_es,
ESTIMATION_NEW_ESTIMATION_EVENTMASK);
}
}
}
static THD_FUNCTION(sdlog, arg)
{
(void)arg;
chRegSetThreadName("sdlog");
static event_listener_t sensor_listener;
chEvtRegisterMaskWithFlags(&sensor_events, &sensor_listener,
(eventmask_t)ONBOARDSENSOR_EVENT,
(eventflags_t)SENSOR_EVENT_MPU6000);
static uint8_t writebuf[200];
static MemoryStream writebuf_stream;
bool error = false;
UINT _bytes_written;
FRESULT res;
static FIL version_fd;
res = f_open(&version_fd, "/log/version", FA_WRITE | FA_CREATE_ALWAYS);
if (res) {
chprintf(stdout, "error %d opening %s\n", res, "/log/version");
return;
}
msObjectInit(&writebuf_stream, writebuf, sizeof(writebuf), 0);
chprintf((BaseSequentialStream*)&writebuf_stream,
"git version: %s (%s)\n"
"compiler: %s\n"
"built: %s\n",
build_git_version, build_git_branch,
PORT_COMPILER_NAME,
build_date);
error = error || f_write(&version_fd, writebuf, writebuf_stream.eos, &_bytes_written);
f_close(&version_fd);
static FIL mpu6000_fd;
res = f_open(&mpu6000_fd, "/log/mpu6000.csv", FA_WRITE | FA_CREATE_ALWAYS);
if (res) {
chprintf(stdout, "error %d opening %s\n", res, "/log/mpu6000.csv");
return;
}
const char *mpu_descr = "time,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z,temp\n";
error |= error || f_write(&mpu6000_fd, mpu_descr, strlen(mpu_descr), &_bytes_written);
static FIL rc_fd;
res = f_open(&rc_fd, "/log/rc.csv", FA_WRITE | FA_CREATE_ALWAYS);
if (res) {
chprintf(stdout, "error %d opening %s\n", res, "/log/rc.csv");
return;
}
const char *rc_descr = "time,signal,ch1,ch2,ch3,ch4,ch5\n";
error |= error || f_write(&rc_fd, rc_descr, strlen(rc_descr), &_bytes_written);
while (!error) {
eventmask_t events = chEvtWaitAny(ONBOARDSENSOR_EVENT);
float t = (float)chVTGetSystemTimeX() / CH_CFG_ST_FREQUENCY;
if (events & ONBOARDSENSOR_EVENT) {
chEvtGetAndClearFlags(&sensor_listener);
rate_gyro_sample_t gyro;
accelerometer_sample_t acc;
onboard_sensor_get_mpu6000_gyro_sample(&gyro);
onboard_sensor_get_mpu6000_acc_sample(&acc);
float temp = onboard_sensor_get_mpu6000_temp();
msObjectInit(&writebuf_stream, writebuf, sizeof(writebuf), 0);
chprintf((BaseSequentialStream*)&writebuf_stream,
"%f,%f,%f,%f,%f,%f,%f,%f\n", gyro.timestamp, gyro.rate[0], gyro.rate[1], gyro.rate[2], acc.acceleration[0], acc.acceleration[1], acc.acceleration[2], temp);
UINT _bytes_written;
int ret = f_write(&mpu6000_fd, writebuf, writebuf_stream.eos, &_bytes_written);
if (ret != 0) {
chprintf(stdout, "write failed %d\n", ret);
}
if (ret == 9) {
f_open(&mpu6000_fd, "/log/mpu6000.csv", FA_WRITE);
f_lseek(&mpu6000_fd, f_size(&mpu6000_fd));
}
static int sync_needed = 0;
sync_needed++;
if (sync_needed == 100) {
f_sync(&mpu6000_fd);
sync_needed = 0;
}
}
if (false) {
static struct rc_input_s rc_in;
sumd_input_get(&rc_in);
msObjectInit(&writebuf_stream, writebuf, sizeof(writebuf), 0);
chprintf((BaseSequentialStream*)&writebuf_stream,
"%f,%d,%f,%f,%f,%f,%f\n", t, !rc_in.no_signal, rc_in.channel[0], rc_in.channel[1], rc_in.channel[2], rc_in.channel[3], rc_in.channel[4]);
UINT _bytes_written;
int ret = f_write(&rc_fd, writebuf, writebuf_stream.eos, &_bytes_written);
if (ret != 0) {
chprintf(stdout, "write failed %d\n", ret);
}
static int sync_needed = 0;
sync_needed++;
if (sync_needed == 100) {
f_sync(&rc_fd);
sync_needed = 0;
}
}
}
}
