int8_t ker_sys_timer_start(uint8_t tid, int32_t interval, uint8_t type)
{
sos_pid_t my_id = ker_get_current_pid();
if( (ker_timer_init(my_id, tid, type) != SOS_OK) ||
(ker_timer_start(my_id, tid, interval) != SOS_OK)) {
return ker_mod_panic(my_id);
}
return SOS_OK;
}
static int8_t module(void *state, Message *msg)
{
app_state_t *s = (app_state_t*) state;
switch (msg->type)
{
case MSG_INIT:
{
s->pid = msg->did;
s->count = 0;
s->state[0] = MOTOR_FULL_SPEED;
s->state[1] = RGT_REV_NORMAL;
s->state[2] = LFT_REV_NORMAL;
s->state[3] = LFT_STOP;
s->state[4] = RGT_STOP;
ker_timer_init(s->pid, MCONTROL_TEST_TIMER, TIMER_REPEAT);
ker_timer_start(s->pid, MCONTROL_TEST_TIMER, TIMER_INTERVAL);
break;
}
case MSG_TIMER_TIMEOUT:
{
post_short(RAGOBOT_CB2BUS_PID, s->pid, MSG_BARBINARY, 0, s->state[s->count], 0);
post_short(RAGOBOT_CB2BUS_PID, s->pid, MSG_LOAD, 0, 0, 0);
post_short(RAGOBOT_MCONTROL_PID, s->pid, MSG_CHANGE_SPEED, s->state[s->count], 0, 0);
s->count = (s->count+1) % 5;
break;
}
case MSG_FINAL:
{
ker_timer_stop(s->pid, 0);
break;
}
}
return SOS_OK;
}
static int8_t module(void *state, Message *msg)
{
inertialsensor_state_t *s = (inertialsensor_state_t*)state;
uint8_t *data;
switch (msg->type){
case MSG_INIT:
{
s->state = SEND_SETUP;
ACCSENSOR_ENABLE();
MAGSENSOR_ENABLE();
s->write_data[0] = INTREF_AIN_AUTOSHTDWN;
ker_timer_init(RAGOBOT_INERTIALSENSOR_PID, 0, TIMER_ONE_SHOT);
MAGSENSOR_RESET();
if (ker_i2c_send_data(I2C_EXT_ADC_ADDR, s->write_data, 1, RAGOBOT_INERTIALSENSOR_PID) != SOS_OK)
{
//if i2c bus busy, retry later.
s->state = RETRY_SEND_SETUP;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
return SOS_OK;
}
//-------------------------------------------------------------------------
// Restart timer timeout called from timer to rehandle exception
// Arguments: none
//-------------------------------------------------------------------------
case MSG_TIMER_TIMEOUT:
{
if (s->state == RETRY_SEND_SETUP)
{
s->state = SEND_SETUP;
if (ker_i2c_send_data(I2C_EXT_ADC_ADDR, s->write_data, 1, RAGOBOT_INERTIALSENSOR_PID) != SOS_OK)
{
//if i2c bus busy, retry later.
s->state = RETRY_SEND_SETUP;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
}
else if (s->state == RETRY_SCAN)
{
s->state = SCANNING;
if (ker_i2c_send_data(I2C_EXT_ADC_ADDR, s->write_data, 1, RAGOBOT_INERTIALSENSOR_PID) != SOS_OK)
{
//if i2c bus busy, retry later.
s->state = RETRY_SCAN;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
}
else if (s->state == RETRY_READ)
{
s->state = READING;
if ((ker_i2c_read_data(I2C_EXT_ADC_ADDR, 6, RAGOBOT_INERTIALSENSOR_PID)) != SOS_OK) // read 6 bytes
{
//if i2c bus is busy, then wait until it is free.
s->state = RETRY_READ;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
}
return SOS_OK;
}
case MSG_GET_ACC_READINGS:
{
if (s->state != IDLE)
{
return -EBUSY;
}
s->state = SCANNING;
s->mode = MSG_GET_ACC_READINGS;
s->callingpid = msg->sid;
s->write_data[0] = SCAN_UP_ZERO_TO_SGL | CHANNEL5;
if (ker_i2c_send_data(I2C_EXT_ADC_ADDR, s->write_data, 1, RAGOBOT_INERTIALSENSOR_PID) != SOS_OK)
{
//if i2c bus busy, retry later.
s->state = RETRY_SCAN;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
return SOS_OK;
}
case MSG_GET_MAG_READINGS:
{
if (s->state != IDLE)
{
return -EBUSY;
}
s->state = SCANNING;
s->mode = MSG_GET_MAG_READINGS;
s->callingpid = msg->sid;
s->write_data[0] = SCAN_UP_ZERO_TO_SGL | CHANNEL5;
if (ker_i2c_send_data(I2C_EXT_ADC_ADDR, s->write_data, 1, RAGOBOT_INERTIALSENSOR_PID) != SOS_OK)
{
//if i2c bus busy, retry later.
s->state = RETRY_SCAN;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
return SOS_OK;
}
case MSG_GET_INERTIAL_READINGS:
{
if (s->state != IDLE)
{
return -EBUSY;
}
s->state = SCANNING;
s->mode = MSG_GET_INERTIAL_READINGS;
s->callingpid = msg->sid;
s->write_data[0] = SCAN_UP_ZERO_TO_SGL | CHANNEL5;
if (ker_i2c_send_data(I2C_EXT_ADC_ADDR, s->write_data, 1, RAGOBOT_INERTIALSENSOR_PID) != SOS_OK)
{
//if i2c bus busy, retry later.
s->state = RETRY_SCAN;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
return SOS_OK;
}
//-------------------------------------------------------------------------
// called from lower level after sending is done
// we send command to read data back
// Arguments: none
//-------------------------------------------------------------------------
case MSG_I2C_SEND_DONE:
{
if (s->state == SEND_SETUP)
{
s->state = IDLE;
}
else if (s->state == SCANNING)
{
s->state = READING;
if ((ker_i2c_read_data(I2C_EXT_ADC_ADDR, 12, RAGOBOT_INERTIALSENSOR_PID)) != SOS_OK) // read 12 bytes
{
//if i2c bus is busy, then wait until it is free.
s->state = RETRY_READ;
ker_timer_start(RAGOBOT_INERTIALSENSOR_PID, 0, RETRY_TIMEOUT);
}
}
return SOS_OK;
}
//-------------------------------------------------------------------------
// post_long function, data is back from lower level
// we store the data and signal up
// Arguments: data, the second and third byte is the first adc value
//-------------------------------------------------------------------------
case MSG_I2C_READ_DONE:
{
s->state = IDLE;
data = (uint8_t*) msg->data; //(((uint8_t*)(msg->data)) + 1); // the first byte is something else
if (msg->len != 12)
{
if (s->mode == MSG_GET_ACC_READINGS)
{
post_short(s->callingpid, RAGOBOT_INERTIALSENSOR_PID, MSG_GET_ACC_READINGS_FAIL, 0, 0, 0);
}
else if (s->mode == MSG_GET_MAG_READINGS)
{
post_short(s->callingpid, RAGOBOT_INERTIALSENSOR_PID, MSG_GET_MAG_READINGS_FAIL, 0, 0, 0);
}
else if (s->mode == MSG_GET_INERTIAL_READINGS)
{
post_long(s->callingpid, RAGOBOT_INERTIALSENSOR_PID, MSG_GET_INERTIAL_READINGS_DONE, sizeof(s->readings), s->readings, 0);
}
return SOS_OK;
}
s->readings[0] = (((uint16_t)(data[0] & 0x03)) << 8) | (uint16_t) (data[1]);
s->readings[1] = (((uint16_t)(data[2] & 0x03)) << 8) | (uint16_t) (data[3]);
s->readings[2] = (((uint16_t)(data[4] & 0x03)) << 8) | (uint16_t) (data[5]);
s->readings[3] = (((uint16_t)(data[6] & 0x03)) << 8) | (uint16_t) (data[7] & 0xFC);
s->readings[4] = (((uint16_t)(data[8] & 0x03)) << 8) | (uint16_t) (data[9] & 0xFC);
s->readings[5] = (((uint16_t)(data[10] & 0x03)) << 8) | (uint16_t) (data[11] & 0xFC);
if (s->mode == MSG_GET_ACC_READINGS)
{
post_long(s->callingpid, RAGOBOT_INERTIALSENSOR_PID, MSG_GET_ACC_READINGS_DONE, 6, (s->readings)+3, 0);
}
else if (s->mode == MSG_GET_MAG_READINGS)
{
post_long(s->callingpid, RAGOBOT_INERTIALSENSOR_PID, MSG_GET_MAG_READINGS_DONE, 6, s->readings, 0);
}
else if (s->mode == MSG_GET_INERTIAL_READINGS)
{
post_long(s->callingpid, RAGOBOT_INERTIALSENSOR_PID, MSG_GET_INERTIAL_READINGS_DONE, sizeof(s->readings), s->readings, 0);
}
return SOS_OK;
}
case MSG_FINAL:
default:
{
return SOS_OK;
}
}
}
static int8_t fetcher_handler(void *state, Message *msg)
{
switch (msg->type) {
case MSG_FETCHER_FRAGMENT:
{
fetcher_fragment_t *f;
f = (fetcher_fragment_t*)msg->data;
f->key = entohs( f->key );
f->frag_id = entohs(f->frag_id);
DEBUG_PID(KER_FETCHER_PID,"MSG_FETCHER_FRAGMENT:\n");
handle_overheard_fragment(msg);
if(fst == NULL) {
DEBUG_PID(KER_FETCHER_PID, "NO Request!!!\n");
return SOS_OK; //!< no request
}
//DEBUG_PID(KER_FETCHER_PID,"calling restart_request_timer()\n");
restart_request_timer();
fst->retx = 0;
//DEBUG_PID(KER_FETCHER_PID,"calling handle_data()\n");
return handle_data(msg);
}
case MSG_FETCHER_REQUEST:
{
fetcher_bitmap_t *bmap =
(fetcher_bitmap_t *) msg->data;
bmap->key = entohs( bmap->key );
//! received request from neighbors
DEBUG("handling request to %d from %d\n", msg->daddr, msg->saddr);
if(msg->daddr == ker_id()) {
return handle_request(msg);
}
if(fst == NULL) return SOS_OK; //!< no request
restart_request_timer();
fst->retx = 0;
return SOS_OK;
}
case MSG_TIMER_TIMEOUT:
{
MsgParam *params = (MsgParam*)(msg->data);
if(params->byte == FETCHER_REQUEST_TID) {
//DEBUG("request timeout\n");
if( no_mem_retry ) {
send_fetcher_done();
return SOS_OK;
}
handle_request_timeout();
} else if(params->byte == FETCHER_TRANSMIT_TID) {
//DEBUG("send fragment timeout\n");
if( send_state.num_msg_in_queue < FETCHER_MAX_MSG_IN_QUEUE ) {
send_fragment();
}
}
return SOS_OK;
}
case MSG_PKT_SENDDONE:
{
if( send_state.num_msg_in_queue > 0 ) {
send_state.num_msg_in_queue--;
}
return SOS_OK;
}
#ifdef SOS_HAS_EXFLASH
case MSG_EXFLASH_WRITEDONE:
{
ker_free(send_state.fragr);
send_state.fragr = NULL;
check_map_and_post();
return SOS_OK;
}
case MSG_EXFLASH_READDONE:
{
post_auto(KER_FETCHER_PID,
KER_FETCHER_PID,
MSG_FETCHER_FRAGMENT,
sizeof(fetcher_fragment_t),
send_state.frag,
SOS_MSG_RELEASE,
send_state.dest);
send_state.frag = NULL;
return SOS_OK;
}
#endif
case MSG_INIT:
{
send_state.map = NULL;
send_state.frag = NULL;
send_state.fragr = NULL;
send_state.num_msg_in_queue = 0;
ker_msg_change_rules(KER_FETCHER_PID, SOS_MSG_RULES_PROMISCUOUS);
ker_permanent_timer_init(&(send_state.timer), KER_FETCHER_PID, FETCHER_TRANSMIT_TID, TIMER_REPEAT);
ker_timer_init(KER_FETCHER_PID, FETCHER_REQUEST_TID, TIMER_ONE_SHOT);
return SOS_OK;
}
}
return -EINVAL;
}
static int8_t simple_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t*)state;
/** Switch to the correct message handler
*
* This module handles three types of messages:
*
* \li MSG_INIT to start a timer and allocated a buffer
*
* \li MSG_TIMER_TIMEOUT to periodicly write a value to the buffer. Note
* that we are expecting that the timer will have timer with ID equal to
* SIMPLE_TID
*
* \li MSG_FINAL to stop the timer and deallocate the buffer
*
*/
switch (msg->type){
case MSG_INIT:
{
// I'm alive!
LED_DBG(LED_GREEN_TOGGLE);
s->pid = msg->did;
s->state = (uint8_t *) ker_malloc(sizeof(uint8_t), s->pid);
*(s->state) = 0;
ker_timer_init(s->pid, SIMPLE_TID, TIMER_REPEAT);
ker_timer_start(s->pid, SIMPLE_TID, 1000);
break;
}
case MSG_FINAL:
{
// Bye bye!
ker_free(s->state);
ker_timer_stop(s->pid, SIMPLE_TID);
break;
}
case MSG_TIMER_TIMEOUT:
{
MsgParam* params = (MsgParam*)(msg->data);
if (params->byte == SIMPLE_TID)
{
// Blink the LED
if (*(s->state) == 1) {
LED_DBG(LED_YELLOW_OFF);
} else {
LED_DBG(LED_YELLOW_ON);
}
// Update the state
*(s->state) += 1;
if (*(s->state) > 1) {
*(s->state) = 0;
}
}
break;
}
default:
return -EINVAL;
}
return SOS_OK;
}
static int8_t uart_mod_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t*)state;
switch (msg->type){
case MSG_INIT:
{
s->pid = msg->did;
uart_mod_bufcnt = 0;
uart_mod_bufoverflow = 0;
uart_mod_buf = (uint8_t *)sys_malloc(BUFSIZE);
DEBUG("Uart Start\n");
uart_mod_init();
ker_timer_init(KER_UART_PID, UART_TID, SLOW_TIMER_ONE_SHOT);
sys_led(LED_YELLOW_ON);
break;
}
case MSG_FINAL:
{
sys_timer_stop(UART_TID);
DEBUG("Uart Stop\n");
break;
}
case MSG_TIMER_TIMEOUT:
{
/* Timer fired, should sent out the message to the uart0 now
* */
HAS_CRITICAL_SECTION;
uint8_t *tmpbuf;
uint8_t tmpbufcnt;
ENTER_CRITICAL_SECTION();
tmpbufcnt = uart_mod_bufcnt;
tmpbuf = (uint8_t *)sys_malloc(tmpbufcnt);
memcpy(tmpbuf, uart_mod_buf, tmpbufcnt);
sys_post(DFLT_APP_ID1, MSG_RFID_RESPONSE, tmpbufcnt, tmpbuf, SOS_MSG_RELEASE);
uart_mod_bufcnt = 0;
uart_mod_bufoverflow = 0;
LEAVE_CRITICAL_SECTION();
sys_led(LED_YELLOW_TOGGLE);
break;
}
case MSG_RFID_COMMAND:
{
uint8_t i, msg_len;
msg_len = msg->len;
for (i = 0; i < msg_len; i++) {
USART1_send(*((msg->data)+i));
}
break;
}
default:
return -EINVAL;
}
return SOS_OK;
}
static int8_t
adcm1700_patch_handler (void *state, Message * msg)
{
// i2c_msg_state_t *s = (i2c_msg_state_t*)state;
switch (msg->type)
{
case MSG_INIT:
{
return SOS_OK;
}
case MSG_FINAL:
{
patchNumber = 0;
patchdata_index = 0;
return SOS_OK;
}
case SET_PATCH_DONE:
{
return SOS_OK;
}
//******************* Timer (allows enough time for module to exit run mode) ******
case MSG_TIMER_TIMEOUT:
{
writeBlock (ADCM1700_PATCH_PID, ADCM_REG_PADR1,
&patch1700data[patchdata_index],
(uint8_t) patch1700len[patchNumber]);
return SOS_OK;
}
//***********************Writing and Reading individual Bytes*********************/
case READ_REGISTER_DONE:
{
register_result_t *m = (register_result_t *) (msg->data);
uint16_t addr = m->reg;
uint16_t data = m->data;
uint8_t result = m->status;
//mhr:note that it is ok if the status fails when the device is stll configuring itself since we can not communicate with it in the meantime, we retry
if ((controlState != CONTROL_STATE_PENDING_CONFIGURATION)
&& (result != SOS_OK))
{
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, -EINVAL, 0, 0);
return -EINVAL;
}
switch (addr)
{
HAS_CRITICAL_SECTION;
case ADCM_REG_STATUS: //mhr:checking if the device is configured and now ready to be set.
if (controlState == CONTROL_STATE_PENDING_CONFIGURATION)
{ // part of startup
if (result == SOS_OK)
{
if (data & ADCM_STATUS_CONFIG_MASK) // still configuring, check again. Should probably add a delay
readRegister (ADCM1700_PATCH_PID, ADCM_REG_STATUS);
else // it is now configured, we officialy go the mode to set different registers of the device.
{
ENTER_CRITICAL_SECTION ();
{
controlState = CONTROL_STATE_SETTING_DEVICE;
}
LEAVE_CRITICAL_SECTION ();
//atomic controlState = CONTROL_STATE_SETTING_DEVICE;
readRegister (ADCM1700_PATCH_PID, ADCM_REG_ID);
}
}
else
readRegister (ADCM1700_PATCH_PID, ADCM_REG_STATUS);
}
break;
case ADCM_REG_ID: //checking if we have the right imager
if (controlState == CONTROL_STATE_SETTING_DEVICE)
{
// continue with startup intialization, stop the imager to re-configure it. CONTROL register is read first and the RUN bit is turned off
if ((ADCM_ID_MASK & data) == (ADCM_ID_MASK & ADCM_ID_1700))
readRegister (ADCM1700_PATCH_PID, ADCM_REG_CONTROL);
// had a failure (incorrect module ID)
else
{
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, -EINVAL, 0, 0);
return -EINVAL;
}
}
break;
case ADCM_REG_CONTROL:
switch (controlState)
{
case CONTROL_STATE_SETTING_DEVICE: // clear the RUN bit and write it out
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_CONTROL,
(data & ~ADCM_CONTROL_RUN_MASK));
break;
case CONTROL_STATE_END: // Bring the new configuration online by setting the CONFIG bit in the CONTROL register
ENTER_CRITICAL_SECTION ();
{
controlState = CONTROL_STATE_START_CFG;
}
LEAVE_CRITICAL_SECTION ();
//atomic controlState = CONTROL_STATE_START_CFG;
// was ADCM_CONTROL_CONFIG_MASK: RLB 3/18/05
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_CONTROL,
(data | ADCM_CONTROL_RUN_MASK));
break;
case CONTROL_STATE_START_CFG: // initializing the imager, and now bringing the configuration online. check if still configuring.
if ((data & ADCM_CONTROL_CONFIG_MASK) == 0) // CONFIG bit has cleared, we are done
{
ENTER_CRITICAL_SECTION ();
{
controlState = CONTROL_STATE_READY;
}
LEAVE_CRITICAL_SECTION ();
//atomic controlState = CONTROL_STATE_READY;
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, SOS_OK, 0, 0);
}
else // re-read and check bit again
readRegister (ADCM1700_PATCH_PID, ADCM_REG_CONTROL);
default:
break;
}
break;
case ADCM_REG_OUTPUT_CTRL_V:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_OUTPUT_CTRL_V,
(data | ADCM_OUTPUT_CTRL_FRVCLK_MASK |
ADCM_OUTPUT_CTRL_FR_OUT_MASK |
ADCM_OUTPUT_CTRL_JUST_MASK |
ADCM_OUTPUT_FORMAT_Y));
break;
case ADCM_REG_PROC_CTRL_V:
if (controlState == CONTROL_STATE_SETTING_DEVICE)
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_PROC_CTRL_V,
(data & ~ADCM_PROC_CTRL_NOSIZER));
break;
default:
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, -EINVAL, 0, 0);
return -EINVAL;
}
return SOS_OK;
}
case WRITE_REGISTER_DONE:
{
register_result_t *m = (register_result_t *) (msg->data);
uint16_t addr = m->reg;
//uint16_t data = m->data;
uint8_t result = m->status;
if (result != SOS_OK)
{
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, -EINVAL, 0, 0);
return -EINVAL;
}
switch (addr)
{
HAS_CRITICAL_SECTION;
case ADCM_REG_CONTROL:
switch (controlState)
{
case CONTROL_STATE_SETTING_DEVICE: // start patching code
ker_timer_init (ADCM1700_PATCH_PID, PATCH_TID,
TIMER_ONE_SHOT);
ker_timer_start (ADCM1700_PATCH_PID, PATCH_TID, 500);
break;
case CONTROL_STATE_END: // read the register to check for the config bit
readRegister (ADCM1700_PATCH_PID, ADCM_REG_CONTROL);
break;
case CONTROL_STATE_START_CFG:
// just set CONFIG bit, read the register to see if it has been cleared, indicating it is done. callback for this read will check the CONFIG bit
readRegister (ADCM1700_PATCH_PID, ADCM_REG_CONTROL);
break;
default:
break;
}
break;
case ADCM_REG_OUTPUT_CTRL_V:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_CLK_PER, 0x0FA0); // 4 MHz (4,000 kHz)
break;
case ADCM_REG_CLK_PER:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_AE2_ETIME_MIN, 0x0001); // 10 microseconds
break;
case ADCM_REG_AE2_ETIME_MIN:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_AE2_ETIME_MAX, 0xC350); // 0.5 seconds
break;
case ADCM_REG_AE2_ETIME_MAX:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_AE_GAIN_MAX, 0x0F00); // analog gain = 15
break;
case ADCM_REG_AE_GAIN_MAX:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_AF_CTRL1, 0x0013); // auto functions activated
break;
case ADCM_REG_AF_CTRL1:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_AF_CTRL2, 0x0002); // no overexposure, 60 Hz
break;
case ADCM_REG_AF_CTRL2:
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_SZR_IN_WID_V,
ADCM_SIZE_1700DEFAULT_W);
break;
case ADCM_REG_SZR_IN_WID_V: //step sizer:1:9
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_SZR_IN_HGT_V,
ADCM_SIZE_1700DEFAULT_H);
break;
case ADCM_REG_SZR_IN_HGT_V: //step sizer:2:9
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_SZR_OUT_WID_V,
ADCM_SIZE_API_DEFAULT_W);
break;
case ADCM_REG_SZR_OUT_WID_V: //step sizer:5:9
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_SZR_OUT_HGT_V,
ADCM_SIZE_API_DEFAULT_H);
break;
case ADCM_REG_SZR_OUT_HGT_V: //step sizer:6:9
readRegister (ADCM1700_PATCH_PID, ADCM_REG_PROC_CTRL_V);
break;
case ADCM_REG_PROC_CTRL_V: // set window size: step 8 of 9
writeRegister (ADCM1700_PATCH_PID, ADCM_REG_VBLANK_V,
ADCM_VBLANK_DEFAULT);
break;
case ADCM_REG_VBLANK_V: // set vblank interval: step 9 of 9
ENTER_CRITICAL_SECTION ();
{
controlState = CONTROL_STATE_READY;
}
LEAVE_CRITICAL_SECTION ();
//atomic controlState = CONTROL_STATE_READY;
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, SOS_OK, 0, 0);
return SOS_OK;
break;
}
return SOS_OK;
}
//***********************Writing and Reading Blocks*********************/
case WRITE_BLOCK_DONE:
{
block_result_t *m = (block_result_t *) (msg->data);
uint16_t startReg = m->startReg;
//char *data = m->data;
//uint8_t length = m->length;
uint8_t result = m->result;
if (result != SOS_OK)
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, -EINVAL, 0, 0);
switch (startReg)
{
case ADCM_REG_PADR1: // write the next patch block
patchdata_index = patchdata_index + patch1700len[patchNumber];
++patchNumber;
writeBlock (ADCM1700_PATCH_PID, ADCM_REG_PADR2,
&patch1700data[patchdata_index],
(uint8_t) patch1700len[patchNumber]);
break;
case ADCM_REG_PADR2: // write the next patch block
patchdata_index = patchdata_index + patch1700len[patchNumber];
++patchNumber;
writeBlock (ADCM1700_PATCH_PID, ADCM_REG_PADR3,
&patch1700data[patchdata_index],
(uint8_t) patch1700len[patchNumber]);
break;
case ADCM_REG_PADR3: // write the next patch block
patchdata_index = patchdata_index + patch1700len[patchNumber];
++patchNumber;
writeBlock (ADCM1700_PATCH_PID, ADCM_REG_PADR4,
&patch1700data[patchdata_index],
(uint8_t) patch1700len[patchNumber]);
break;
case ADCM_REG_PADR4: // write the next patch block
patchdata_index = patchdata_index + patch1700len[patchNumber];
++patchNumber;
writeBlock (ADCM1700_PATCH_PID, ADCM_REG_PADR5,
&patch1700data[patchdata_index],
(uint8_t) patch1700len[patchNumber]);
break;
case ADCM_REG_PADR5: // write the next patch block
patchdata_index = patchdata_index + patch1700len[patchNumber];
++patchNumber;
writeBlock (ADCM1700_PATCH_PID, ADCM_REG_PADR6, &patch1700data[patchdata_index], (uint8_t) patch1700len[patchNumber]); // *** diagnostic ***
break;
case ADCM_REG_PADR6: // Patching dine, set data protocol that the CPLD expects,first read the register value and set the new data in the callback
readRegister (ADCM1700_PATCH_PID, ADCM_REG_OUTPUT_CTRL_V);
break;
default:
post_short (ADCM1700_CONTROL_PID, ADCM1700_PATCH_PID,
SET_PATCH_DONE, -EINVAL, 0, 0);
break;
}
return SOS_OK;
}
case READ_BLOCK_DONE:
{
return SOS_OK;
}
default:
return -EINVAL;
}
return SOS_OK;
}
static int8_t module(void *state, Message *msg) {
lightshow_state *s = (lightshow_state*) state;
MsgParam *p = (MsgParam*)(msg->data);
uint8_t decklights_count;
uint8_t bargraph_count;
uint8_t headlights_count;
uint16_t color = BLACK;
switch (msg->type){
case MSG_INIT:
{
s->decklights_state = DISABLE;
s->bargraph_state = DISABLE;
s->headlights_state = DISABLE;
s->light_state = DISABLE;
s->count = 0;
ker_timer_init(RAGOBOT_LIGHTSHOW_PID, 0, TIMER_REPEAT);
break;
}
case MSG_LIGHTSHOW_START:
{
if (s->light_state == DISABLE)
{
s->light_state = ENABLE;
ker_timer_start(RAGOBOT_LIGHTSHOW_PID, 0, 80);
}
break;
}
case MSG_TIMER_TIMEOUT:
{
if (s->decklights_state == ENABLE)
{
decklights_count = s->count % 10;
if (decklights_count == 0)
{
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT3, OFF, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT2, ON, 0, 0);
}
else if (decklights_count == 1 || decklights_count == 9)
{
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT2, OFF, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT3, ON, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT4, OFF, 0, 0);
}
else if (decklights_count == 2 || decklights_count == 8)
{
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT3, OFF, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT4, ON, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT5, OFF, 0, 0);
}
else if (decklights_count == 3 || decklights_count == 7)
{
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT4, OFF, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT5, ON, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT6, OFF, 0, 0);
}
else if (decklights_count == 4 || decklights_count == 6)
{
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT5, OFF, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT6, ON, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT1, OFF, 0, 0);
}
else if (decklights_count == 5)
{
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT6, OFF, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_DECKLIGHT1, ON, 0, 0);
}
}
if (s->bargraph_state == ENABLE)
{
bargraph_count = s->count % 18;
if (bargraph_count < 10)
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_BARPOSITION, bargraph_count, 0, 0);
else
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_BARPOSITION, 18-bargraph_count, 0, 0);
}
if (s->headlights_state == ENABLE)
{
headlights_count = s->count % 8;
if (headlights_count == 0)
color = BLACK;
else if (headlights_count == 2)
color = GREEN;
else if (headlights_count == 4)
color = ORANGE;
else if (headlights_count == 6)
color = RED;
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_HEADLIGHT, FRONT_LEFT, color, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_HEADLIGHT, FRONT_RIGHT, color, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_HEADLIGHT, BACK_LEFT, color, 0);
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_HEADLIGHT, BACK_RIGHT, color, 0);
}
post_short(RAGOBOT_CB2BUS_PID, RAGOBOT_LIGHTSHOW_PID, MSG_LOAD, 0, 0, 0);
s->count += 1;
break;
}
case MSG_LIGHTSHOW_DECKLIGHTS:
{
if (p->byte != ENABLE && p->byte != DISABLE)
return -EINVAL;
s->decklights_state = p->byte;
break;
}
case MSG_LIGHTSHOW_HEADLIGHTS:
{
if (p->byte != ENABLE && p->byte != DISABLE)
return -EINVAL;
s->headlights_state = p->byte;
break;
}
case MSG_LIGHTSHOW_BARGRAPH:
{
if (p->byte != ENABLE && p->byte != DISABLE)
return -EINVAL;
s->bargraph_state = p->byte;
break;
}
case MSG_LIGHTSHOW_STOP:
case MSG_FINAL:
{
if (s->light_state == ENABLE)
{
s->light_state = DISABLE;
ker_timer_stop(RAGOBOT_LIGHTSHOW_PID, 0);
}
}
default:
break;
}
return SOS_OK;
}
