int8_t module(void *state, Message *msg)
#endif
{
app_state *s = (app_state*) state;
switch (msg->type){
case MSG_INIT:
{
s->pid = msg->did;
if (ker_hipri_int_register(s->pid)==-EINVAL)
{
post_short(RAGOBOT_CB2BUS_PID, s->pid, MSG_DECKLIGHT6, ON, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, s->pid, MSG_LOAD, 0, 0, 0);
}
break;
}
case MSG_HIPRI_INT:
{
post_short(RAGOBOT_CB2BUS_PID, s->pid, MSG_DECKLIGHT1, TOGGLE, 0, 0);
post_short(RAGOBOT_CB2BUS_PID, s->pid, MSG_LOAD, 0, 0, 0);
break;
}
case MSG_FINAL:
{
ker_timer_stop(s->pid, 0);
ker_hipri_int_deregister(s->pid);
break;
}
}
return SOS_OK;
}
static void load_script( uint8_t *script, uint8_t size )
{
codemem_t cm = ker_codemem_alloc(size, CODEMEM_TYPE_EXECUTABLE);
ker_codemem_write( cm, THIS_MODULE_ID, script, size, 0);
ker_codemem_flush( cm, THIS_MODULE_ID);
post_short(script[0], THIS_MODULE_ID, MSG_LOADER_DATA_AVAILABLE, script[1], cm, 0);
return;
}
void pushbutton_broadcast()
{
uint8_t i;
for (i = 0; i < numRegMod; i++)
{
post_short(registered_modules[i], registered_modules[i], MSG_PUSHBUTTON_PRESSED, 0, 0, SOS_MSG_HIGH_PRIORITY);
}
}
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;
}
/**
* Send MSG_I2C_SEND_DONE
*/
void i2c_send_done(uint8_t status) {
// figure out if we have someone to send this message to
if ((i2c_sys.calling_mod_id != NULL_PID) &&
(((i2c_sys.state == I2C_SYS_MASTER_TX) && (status & I2C_SYS_MASTER_FLAG)) || ((i2c_sys.state == I2C_SYS_SLAVE_TX)))) {
/* bus was reserved by someone AND
* bus reserved as master and a send was requested, packet transmited as a master
* OR bus reserved as slave and a send was requested */
if (status & I2C_SYS_ERROR_FLAG) {
post_short(i2c_sys.calling_mod_id, I2C_PID, MSG_I2C_SEND_DONE, 0, 0, SOS_MSG_SEND_FAIL|SOS_MSG_HIGH_PRIORITY);
} else if(status & I2C_SYS_RX_PEND_FLAG) {
// This is the result of a read request done sending the destination
// address and now waiting for the read done
i2c_sys.state = (i2c_sys.flags & I2C_SYS_MASTER_FLAG)?I2C_SYS_MASTER_RX:I2C_SYS_SLAVE_RX;
return;
} else {
post_short(i2c_sys.calling_mod_id, I2C_PID, MSG_I2C_SEND_DONE, 0, 0, SOS_MSG_HIGH_PRIORITY);
}
i2c_sys.state = (i2c_sys.flags & I2C_SYS_MASTER_FLAG)?I2C_SYS_MASTER_WAIT:I2C_SYS_SLAVE_WAIT;
}
}
SOS_MODULE
int8_t module(void *state, Message *msg)
#endif
{
app_state *s = (app_state*) state;
MsgParam *p = (MsgParam*)(msg->data);
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_start(s->pid, MCONTROL_TEST_TIMER, TIMER_REPEAT, TIMER_INTERVAL);
break;
}
case MSG_TIMER_TIMEOUT:
{
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;
}
/**
* Send MSG_SPI_SEND_DONE
*/
void spi_send_done(uint8_t length, uint8_t status) {
// always un chip select
if (s.flags & SPI_SYS_CS_HIGH_FLAG) {
spi_cs_release_high(s.addr);
} else {
spi_cs_release_low(s.addr);
}
// read will be initiated async. by driver or in callback
if (s.flags & SPI_SYS_READ_PENDING_FLAG) {
s.state = SPI_SYS_RX_WAIT;
if (s.flags & SPI_SYS_SEND_DONE_CB_FLAG) {
s.send_done_cb(length, s.cnt, status);
}
} else {
if (s.flags & SPI_SYS_SEND_DONE_CB_FLAG) {
s.send_done_cb(length, s.cnt, status);
} else {
post_short(s.calling_mod_id, SPI_PID, MSG_SPI_SEND_DONE, 0, 0, SOS_MSG_HIGH_PRIORITY);
}
ker_spi_release_bus(SPI_PID);
}
}
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;
}
}
}
/**
* Send MSG_SPI_READ_DONE
*/
void spi_read_done(uint8_t *buff, uint8_t length, uint8_t status) {
if (s.flags & SPI_SYS_CS_HIGH_FLAG) {
spi_cs_release_high(s.addr);
} else {
spi_cs_release_low(s.addr);
}
if((NULL == buff) && (NULL_PID != s.calling_mod_id)) {
post_short(
s.calling_mod_id,
SPI_PID,
MSG_ERROR,
SPI_READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
return;
}
if (NULL == buff) {
return;
}
if (s.len == length) {
s.cnt--;
if (s.cnt > 0) {
s.bufPtr += s.len;
s.state = SPI_SYS_DMA_WAIT;
} else {
// read done send response
if (s.flags & SPI_SYS_READ_DONE_CB_FLAG) {
s.read_done_cb(s.usrBuf, length, s.cnt, status);
} else {
if (s.flags & SPI_SYS_SHARED_MEM_FLAG) {
post_long(
s.calling_mod_id,
SPI_PID,
MSG_SPI_READ_DONE,
length, // read length
s.usrBuf, // return the buffer pointer
SOS_MSG_HIGH_PRIORITY);
} else {
// dont trust the user to free the memory to message only if not shared
post_long(
s.calling_mod_id,
SPI_PID,
MSG_SPI_READ_DONE,
length, // byte length
s.usrBuf,
SOS_MSG_RELEASE|SOS_MSG_HIGH_PRIORITY);
}
}
/*if (s.flags & SPI_SYS_SHARED_MEM_FLAG) {
ker_free(s.usrBuf);
}*/
s.usrBuf = NULL;
s.bufPtr = NULL;
}
s.state = SPI_SYS_WAIT;
} else { // short message
post_short(
s.calling_mod_id,
SPI_PID,
MSG_ERROR,
SPI_READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
if (!(s.flags & SPI_SYS_SHARED_MEM_FLAG)) {
ker_free(s.usrBuf);
}
spi_system_init();
}
if (!(s.flags & SPI_SYS_LOCK_BUS_FLAG)) {
ker_spi_release_bus(s.calling_mod_id);
}
}
int8_t module(void *state, Message *msg)
#endif
{
compass_state *s = (compass_state*)state;
switch (msg->type)
{
case MSG_INIT:
{
s->pid = msg->did;
return SOS_OK;
}
case MSG_GET_HEADING:
{
post_short(RAGOBOT_MAG_SENSOR_PID, RAGOBOT_COMPASS_PID, TSK_READ_COMPASS, 0, 0, 0);
return SOS_OK;
}
case MSG_HEADING_READY:
{
uint16_t heading = 0;
// float xsf = 1.0, ysf = 1.1202873, xoff=14.0, yoff=16.8;
float xsf=1.0, ysf=1.274, xoff=34.0, yoff=-50.96;
float xh = 0, yh = 0;
readings * dir = (readings *) msg->data;
s->x = dir->x;
s->y = dir->y;
s->z = dir->z;
//Do the computation for the direction
xh = ((float)dir->x-650)*xsf + xoff;
yh = ((float)dir->y-650)*ysf + yoff;
if(xh >0 )
{
if(yh < 0)
{
heading = (uint16_t) ( (int16_t) -((atan(yh / xh) * 180.0) / PI));
}
else
{
heading = (uint16_t)(360 - (int16_t) ( (atan(yh / xh) * 180.0) / PI));
}
}
else if(xh == 0)
{
if(yh < 0)
heading = 90;
else
heading = 270;
}
else //xh < 0
{
heading = (uint16_t)(180 - (int16_t) ( (atan(yh / xh) * 180.0) / PI));
}
DEBUG("CALCULATED: x=%d y=%d z=%d => heading = %d\n", s->x, s->y, s->z, heading);
return SOS_OK;
}
default: 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;
}
/**
* Send MSG_I2C_READ_DONE
*/
void i2c_read_done(uint8_t *buff, uint8_t len, uint8_t status) {
uint8_t *bufPtr = NULL;
Message *msgPtr = NULL;
// this is a problem that should be handled
if (buff == NULL) {
return;
}
if ((i2c_sys.calling_mod_id != NULL_PID) && (status & I2C_SYS_ERROR_FLAG)) {
goto post_error_msg;
}
// the bus was reserved the read was of the length we requested
if ((i2c_sys.calling_mod_id != NULL_PID) && (len == i2c_sys.rxLen) &&
// the data was recieved in the correct mode
(((i2c_sys.state == I2C_SYS_MASTER_RX) && (I2C_SYS_MASTER_FLAG & status)) ||
((i2c_sys.state == I2C_SYS_SLAVE_RX) && !(I2C_SYS_MASTER_FLAG & status)))) {
// reserved read done will only be raw reads, wrap in message and send
post_long(
i2c_sys.calling_mod_id,
I2C_PID,
MSG_I2C_READ_DONE,
len,
buff,
SOS_MSG_RELEASE|SOS_MSG_HIGH_PRIORITY);
i2c_sys.state = (i2c_sys.flags & I2C_SYS_MASTER_FLAG)?I2C_SYS_MASTER_WAIT:I2C_SYS_SLAVE_WAIT;
return;
}
// there appers to be a bug in avr-gcc this a work around to make sure
// the cast to message works correctly
// the following code seems to cat the value 0x800008 independent of what
// buff actually ii2c_sys. this has no correlation to the data in buff or the
// value of the pointer the cast (along with many others that should) works
// in gdb but fail to execute correctly when compilied
/* bufPtr = &buff[HDLC_PROTOCOL_SIZE]; */
// DO NOT CHANGE THIS SECTION OF CODE
// start of section
bufPtr = buff+1;
// end of DO NOT CHANGE SECTION
// if it passes sanity checks give it to the scheduler
if (!(I2C_SYS_MASTER_FLAG & status) && (len >= SOS_MSG_HEADER_SIZE) && (buff[0] == HDLC_SOS_MSG)) {
if ((len >= (HDLC_PROTOCOL_SIZE + SOS_MSG_HEADER_SIZE + ((Message*)bufPtr)->len + SOS_MSG_CRC_SIZE)) &&
(len <= I2C_MAX_MSG_LEN)) {
// please do not edit the next line
bufPtr = buff;
// we have enough bytes for it to be a message, lets start the copy out
// XXX msgPtr = (Message*)ker_malloc(sizeof(Message), I2C_PID);
msgPtr = msg_create();
if (msgPtr !=NULL) {
uint8_t i=0;
uint16_t runningCRC=0, crc_in=0;
// extract the protocol field
for (i=0; i<HDLC_PROTOCOL_SIZE; i++) {
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
// extract the header
bufPtr = &buff[HDLC_PROTOCOL_SIZE];
for (i=0; i<SOS_MSG_HEADER_SIZE; i++) {
((uint8_t*)msgPtr)[i] = bufPtr[i];
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
// extract the data if it exists
if (msgPtr->len != 0) {
uint8_t *dataPtr;
dataPtr = ker_malloc(((Message*)msgPtr)->len, I2C_PID);
if (dataPtr != NULL) {
msgPtr->data = dataPtr;
bufPtr = &buff[HDLC_PROTOCOL_SIZE+SOS_MSG_HEADER_SIZE];
for (i=0; i<msgPtr->len; i++) {
msgPtr->data[i] = bufPtr[i];
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
} else { // -ENOMEM
ker_free(msgPtr);
goto post_error_msg;
}
} else {
msgPtr->data = NULL;
}
// get the CRC and check it
bufPtr = &buff[HDLC_PROTOCOL_SIZE+SOS_MSG_HEADER_SIZE+msgPtr->len];
crc_in = bufPtr[0] | (bufPtr[1]<<8);
if (crc_in == runningCRC) {
// message passed all sanity checks including crc
LED_DBG(LED_YELLOW_TOGGLE);
if(msgPtr->data != NULL ) {
msgPtr->flag = SOS_MSG_RELEASE;
}
handle_incoming_msg(msgPtr, SOS_MSG_I2C_IO);
return;
} else { // clean up
ker_free(msgPtr->data);
ker_free(msgPtr);
}
}
}
}
// if we make it to here return error message
post_error_msg:
post_short(
i2c_sys.calling_mod_id,
I2C_PID,
MSG_ERROR,
READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
}
static int8_t handle_fetcher_done( Message *msg )
{
fetcher_state_t *f = (fetcher_state_t*) msg->data;
if( is_fetcher_succeed( f ) == true ) {
//mod_header_ptr p;
loader_cam_t *cam;
fetcher_commit(f, true);
st.blocked = 0;
restartInterval( 0 );
cam = ker_cam_lookup( f->map.key );
if( cam->fetcher.fetchtype == FETCHTYPE_DATA) {
uint8_t buf[2];
ker_codemem_read( cam->fetcher.cm, KER_DFT_LOADER_PID, buf, 2, 0);
post_short(buf[0], KER_DFT_LOADER_PID, MSG_LOADER_DATA_AVAILABLE,
buf[1], cam->fetcher.cm, 0);
DEBUG_PID(KER_DFT_LOADER_PID, "Data Ready\n" );
#ifdef LOADER_NET_EXPERIMENT
ker_led(LED_GREEN_TOGGLE);
#endif
} else {
uint8_t mcu_type;
#ifndef SOS_SIM
uint8_t plat_type;
#endif
mod_header_ptr p;
#ifdef MINIELF_LOADER
// Link and load the module here
melf_load_module(cam->fetcher.cm);
#endif//MINIELF_LOADER
// Get the address of the module header
p = ker_codemem_get_header_address( cam->fetcher.cm );
// get processor type and platform type
mcu_type = sos_read_header_byte(p,
offsetof( mod_header_t, processor_type ));
#ifdef SOS_SIM
if( (mcu_type == MCU_TYPE) )
// In simulation, we don't check for platform
#else
plat_type = sos_read_header_byte(p,
offsetof( mod_header_t, platform_type ));
if( (mcu_type == MCU_TYPE) &&
( plat_type == HW_TYPE || plat_type == PLATFORM_ANY) )
#endif
{
/*
* If MCU is matched, this means we are using the same
* instruction set.
* And if this module is for this *specific* platform or
* simply for all platform with the same MCU
*/
// mark module executable
ker_codemem_mark_executable( cam->fetcher.cm );
if (cam->version & 0x80) {
#ifdef SOS_SFI
#ifdef MINIELF_LOADER
sfi_modtable_register(cam->fetcher.cm);
if (SOS_OK == ker_verify_module(cam->fetcher.cm)){
sfi_modtable_flash(p);
ker_register_module(p);
}
else
sfi_exception(KER_VERIFY_FAIL_EXCEPTION);
#else
uint16_t init_offset, code_size, handler_addr;
uint32_t handler_byte_addr, module_start_byte_addr;
uint16_t handler_sfi_addr;
handler_sfi_addr = sos_read_header_ptr(p, offsetof(mod_header_t, module_handler));
handler_addr = sfi_modtable_get_real_addr(handler_sfi_addr);
handler_byte_addr = (handler_addr * 2);
module_start_byte_addr = (p * 2);
init_offset = (uint16_t)(handler_byte_addr - module_start_byte_addr);
code_size = cam->code_size * LOADER_SIZE_MULTIPLIER;
if (SOS_OK == ker_verify_module(cam->fetcher.cm, init_offset, code_size)){
sfi_modtable_flash(p);
ker_register_module(p);
}
else
sfi_exception(KER_VERIFY_FAIL_EXCEPTION);
#endif //MINIELF_LOADER
#else
ker_register_module(p);
#endif //SOS_SFI
}
}
}
process_version_data( st.version_data, st.recent_neighbor);
} else {
DEBUG_PID( KER_DFT_LOADER_PID, "Fetch failed!, request %d\n", st.recent_neighbor);
f = (fetcher_state_t*) ker_msg_take_data(KER_DFT_LOADER_PID, msg);
fetcher_restart( f, st.recent_neighbor );
}
return SOS_OK;
}
