static THD_FUNCTION(thread2, p) {
(void)p;
chEvtBroadcast(&es1);
chThdSleepMilliseconds(50);
chEvtBroadcast(&es2);
}
static msg_t thread2(void *p) {
(void)p;
chEvtBroadcast(&es1);
chThdSleepMilliseconds(50);
chEvtBroadcast(&es2);
return 0;
}
static void mpu9150_int_event_handler(eventid_t id) {
(void) id;
mpu9150_a_read_x_y_z(mpu9150_driver.i2c_instance, &mpu9150_current_read.accel_xyz);
mpu9150_g_read_x_y_z(mpu9150_driver.i2c_instance, &mpu9150_current_read.gyro_xyz);
mpu9150_current_read.celsius = mpu9150_a_g_read_temperature(mpu9150_driver.i2c_instance) ;
// broadcast event to data_udp thread for enet transmit to FC
chEvtBroadcast(&mpu9150_data_event);
// clear the interrupt status bits on mpu9150
mpu9150_a_g_read_int_status(mpu9150_driver.i2c_instance);
#ifdef DEBUG_SENSOR_MPU
BaseSequentialStream *chp = (BaseSequentialStream *)&SDU_PSAS;
static uint16_t count = 0;
++count;
if (count > 2000) {
chprintf(chp, "\r\n*** MPU9150 ***\r\n");
chprintf(chp, "raw_temp: %d C\r\n", mpu9150_temp_to_dC(mpu9150_current_read.celsius));
chprintf(chp, "ACCL: x: %d\ty: %d\tz: %d\r\n", mpu9150_current_read.accel_xyz.x, mpu9150_current_read.accel_xyz.y, mpu9150_current_read.accel_xyz.z);
chprintf(chp, "GRYO: x: 0x%x\ty: 0x%x\tz: 0x%x\r\n", mpu9150_current_read.gyro_xyz.x, mpu9150_current_read.gyro_xyz.y, mpu9150_current_read.gyro_xyz.z);
count = 0;
}
#endif
}
/*!
* \brief Card insertion event.
*
* Also use at power up to see if card was or remained
* inserted while power unavailable.
*/
void sdc_insert_handler(eventid_t id) {
FRESULT err;
(void)id;
/*! \todo generate a mailbox event here */
/*! \todo test event message system */
/*!
* On insertion SDC initialization and FS mount.
*/
if (sdcConnect(&SDCD1)) {
if(sdcConnect(&SDCD1)) { // why does it often fail the first time but not the second?
return;
}
}
err = f_mount(0, &SDC_FS);
if (err != FR_OK) {
err = f_mount(0, &SDC_FS);
if (err != FR_OK) {
sdcDisconnect(&SDCD1);
return;
}
}
sdc_reset_fp_index();
fs_ready = TRUE;
chEvtBroadcast(&sdc_start_event);
}
static void mpu9150_int_event_handler(eventid_t id UNUSED) {
mpu9150_a_read_x_y_z(mpu9150_driver.i2c_instance, &mpu9150_current_read.accel_xyz);
mpu9150_g_read_x_y_z(mpu9150_driver.i2c_instance, &mpu9150_current_read.gyro_xyz);
mpu9150_current_read.celsius = mpu9150_a_g_read_temperature(mpu9150_driver.i2c_instance) ;
/* broadcast event to data_udp thread for enet transmit to FC */
chEvtBroadcast(&mpu9150_data_event);
/* clear the interrupt status bits on mpu9150 */
mpu9150_a_g_read_int_status(mpu9150_driver.i2c_instance);
#if DEBUG_MPU9150
static uint16_t count = 0;
BaseSequentialStream *chp = getUsbStream();
++count;
if (count > 5000) {
chprintf(chp, "\r\nraw_temp: %d C\r\n", mpu9150_temp_to_dC(mpu9150_current_read.celsius));
chprintf(chp, "ACCL: x: %d\ty: %d\tz: %d\r\n", mpu9150_current_read.accel_xyz.x, mpu9150_current_read.accel_xyz.y, mpu9150_current_read.accel_xyz.z);
chprintf(chp, "GRYO: x: 0x%x\ty: 0x%x\tz: 0x%x\r\n", mpu9150_current_read.gyro_xyz.x, mpu9150_current_read.gyro_xyz.y, mpu9150_current_read.gyro_xyz.z);
count = 0;
}
#endif
}
void Infrared_t::IRxTask() {
msg_t Msg;
while(1) {
// Fetch byte from queue
if(chMBFetch(&imailbox, &Msg, IBitDelay) == RDY_OK) {
//Uart.Printf("%u\r", Msg);
PieceType_t Piece = ProcessInterval(Msg);
switch(Piece) {
case ptHeader: IStartPkt(); break;
case ptOne:
if(IReceivingData) IAppend(1);
else ICancelPkt();
break;
case ptZero:
if(IReceivingData) IAppend(0);
else ICancelPkt();
break;
default: ICancelPkt(); break;
} // switch
// Check if Rx completed
if(IBitCnt == 14) {
ICancelPkt();
RxWord = IRxW << 2;
chEvtBroadcast(&IEvtSrcIrRx);
} // if completed
} // if msg
} // while 1
}
static void data_udp_process_rx(char* rxbuf, uint16_t rxbuflen) {
int retval = -1;
const char reset_string_cmd[] = "USER_RESET";
retval = strncmp( rxbuf, reset_string_cmd, rxbuflen-1);
if(retval == 0) {
chEvtBroadcast(&fc_req_reset_event);
}
}
void sdc_haltnow(void) {
bool b_ret;
chEvtBroadcast(&sdc_halt_event);
chThdSleepMilliseconds(20);
b_ret = sdcDisconnect(&SDCD1);
if(b_ret) {
SDCDEBUG("sdcDiscon fail\r\n"); // this happens a lot!
b_ret = sdcDisconnect(&SDCD1);
if(b_ret) {
SDCDEBUG("sdcDiscon fail2\r\n");
}
}
sdc_reset_fp_index();
fs_ready = FALSE;
SDCDEBUG("SDC card halted.\r\n");
}
/*!
* \brief Card removal event.
*/
void sdc_remove_handler(eventid_t id) {
(void)id;
bool_t ret;
chEvtBroadcast(&sdc_halt_event);
chThdSleepMilliseconds(5);
/*! \todo generate a mailbox event here */
/*! \todo test event message system */
ret = sdcDisconnect(&SDCD1);
if(ret) {
SDCDEBUG("sdcDiscon fail\r\n"); // this happens a lot!
ret = sdcDisconnect(&SDCD1);
if(ret) {
SDCDEBUG("sdcDiscon fail2\r\n");
}
}
sdc_reset_fp_index();
fs_ready = FALSE;
}
static msg_t PillThread(void *arg) {
(void)arg;
chRegSetThreadName("Pill");
bool OldState, HasChanged = false;
while(1) {
chThdSleepMilliseconds(PILL_SEARCH_INTERVAL_MS);
// Check if i2c error
if(i2c.Error) {
Uart.Printf("Err\r");
PillReset();
}
// Discover if pill status changed
HasChanged = false;
for(uint8_t i=0; i<PILL_CNT; i++) {
OldState = Pill[i].Connected; // Save current state
Pill[i].CheckIfConnected();
if(Pill[i].Connected != OldState) HasChanged = true;
} // for
if(HasChanged) chEvtBroadcast(&IEvtPillChange);
} // while 1
return 0;
}
void Event::Broadcast(void) {
chEvtBroadcast(&event);
}
void signal_data_written(void) {
chEvtBroadcast(&new_data_event);
}