static void
sem_test_1_task1_handler(void *arg)
{
os_error_t err;
struct os_task *t;
int i;;
for (i = 0; i < 3; i++) {
t = os_sched_get_current_task();
TEST_ASSERT(t->t_func == sem_test_1_task1_handler);
err = os_sem_pend(&g_sem1, 0);
TEST_ASSERT(err == OS_OK);
/* Sleep to let other tasks run */
os_time_delay(100);
/* Release the semaphore */
err = os_sem_release(&g_sem1);
TEST_ASSERT(err == OS_OK);
/* Sleep to let other tasks run */
os_time_delay(100);
}
os_test_restart();
}
static void
ble_os_disc_test_task_handler(void *arg)
{
struct ble_gap_disc_params disc_params;
int cb_called;
int rc;
/* Receive acknowledgements for the startup sequence. We sent the
* corresponding requests when the host task was started.
*/
ble_hs_test_util_set_startup_acks();
/* Set the connect callback so we can verify that it gets called with the
* proper arguments.
*/
cb_called = 0;
os_time_delay(10);
/* Make sure there are no created connections and no connections in
* progress.
*/
TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
TEST_ASSERT(!ble_gap_master_in_progress());
/* Initiate the general discovery procedure with a 300 ms timeout. */
memset(&disc_params, 0, sizeof disc_params);
rc = ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC, 300, &disc_params,
ble_os_disc_test_cb,
&cb_called, 0, 0);
TEST_ASSERT(rc == 0);
TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
TEST_ASSERT(ble_gap_master_in_progress());
TEST_ASSERT(!cb_called);
/* Receive acks from the controller. */
TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
TEST_ASSERT(ble_gap_master_in_progress());
TEST_ASSERT(!cb_called);
/* Wait 100 ms; verify scan still in progress. */
os_time_delay(100 * OS_TICKS_PER_SEC / 1000);
TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
TEST_ASSERT(ble_gap_master_in_progress());
TEST_ASSERT(!cb_called);
ble_hs_test_util_set_ack(
ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
0);
/* Wait 250 more ms; verify scan completed. */
os_time_delay(250 * OS_TICKS_PER_SEC / 1000);
TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
TEST_ASSERT(!ble_gap_master_in_progress());
TEST_ASSERT(cb_called);
tu_restart();
}
/**
* Run Self test on sensor
*
* @param the sensor interface
* @param pointer to return test result in (0 on pass, non-zero on failure)
*
* @return 0 on sucess, non-zero on failure
*/
int lis2dw12_run_self_test(struct sensor_itf *itf, int *result)
{
int rc;
int16_t base[3], pos[3];
int i;
int16_t change;
/* ensure self test mode is disabled */
rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_DISABLE);
if (rc) {
return rc;
}
os_time_delay(OS_TICKS_PER_SEC / 10);
/* take base reading */
rc = lis2dw12_get_data(itf, &(base[0]), &(base[1]), &(base[2]));
if (rc) {
return rc;
}
/* set self test mode to positive self test */
rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_MODE1);
if (rc) {
return rc;
}
os_time_delay(OS_TICKS_PER_SEC / 10);
/* take self test reading */
rc = lis2dw12_get_data(itf, &(pos[0]), &(pos[1]), &(pos[2]));
if (rc) {
return rc;
}
/* disable self test mod */
rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_DISABLE);
if (rc) {
return rc;
}
/* calculate accel data difference */
change = 0;
for(i = 0; i < 3; i++) {
change += pos[i] - base[i];
}
if ((change > 70) && (change < 1500)) {
*result = 0;
} else {
*result = -1;
}
return 0;
}
void
task1_handler(void *arg)
{
struct os_task *t;
/* Set the led pin for the E407 devboard */
g_led_pin = LED_BLINK_PIN;
hal_gpio_init_out(g_led_pin, 1);
while (1) {
t = os_sched_get_current_task();
assert(t->t_func == task1_handler);
++g_task1_loops;
/* Wait one second */
os_time_delay(1000);
/* Toggle the LED */
hal_gpio_toggle(g_led_pin);
/* Release semaphore to task 2 */
os_sem_release(&g_test_sem);
}
}
static void
task1_handler(void *arg)
{
struct os_task *t;
/* Set the led pin for the E407 devboard */
g_led_pin = LED_BLINK_PIN;
hal_gpio_init_out(g_led_pin, 1);
console_printf("\nSensors Test App\n");
while (1) {
t = os_sched_get_current_task();
assert(t->t_func == task1_handler);
++g_task1_loops;
/* Wait one second */
os_time_delay(OS_TICKS_PER_SEC * MYNEWT_VAL(SENSOR_OIC_OBS_RATE));
/* Toggle the LED */
(void)hal_gpio_toggle(g_led_pin);
/* Release semaphore to task 2 */
os_sem_release(&g_test_sem);
}
}
void task_1()
{
while(1)
{
PB8 = !PB8;
os_time_delay(1000);
}
}
/**
* Use external crystal 32.768KHz
*
* @param operational mode of the sensor
* @return 0 on success, non-zero on failure
*/
static int
bno055_set_ext_xtal_use(uint8_t use_xtal, uint8_t mode)
{
int rc;
if (mode != BNO055_OPR_MODE_CONFIG) {
/* Switch to config mode */
rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
}
os_time_delay((OS_TICKS_PER_SEC * 25)/1000 + 1);
rc = bno055_write8(BNO055_PAGE_ID_ADDR, 0);
if (rc) {
goto err;
}
if (use_xtal) {
/* Use External Clock */
rc = bno055_write8(BNO055_SYS_TRIGGER_ADDR, BNO055_SYS_TRIGGER_CLK_SEL);
if (rc) {
goto err;
}
} else {
/* Use Internal clock */
rc = bno055_write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
if (rc) {
goto err;
}
}
os_time_delay((OS_TICKS_PER_SEC * 10)/1000 + 1);
/* Reset to previous operating mode */
rc = bno055_set_opr_mode(mode);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
static void
sem_test_pend_release_loop(int delay, int timeout, int itvl)
{
os_error_t err;
os_time_delay(delay);
while (1) {
err = os_sem_pend(&g_sem1, timeout);
TEST_ASSERT(err == OS_OK);
err = os_sem_release(&g_sem1);
TEST_ASSERT(err == OS_OK);
os_time_delay(itvl);
}
}
void task_2()
{
while(1)
{
task2count++;
delay_ms(100);
os_time_delay(1000);
}
}
/**
*
* Writes to the sensor's offset registers from an offset struct
*
* @param pointer to the offset structure
* @return 0 on success, non-zero on failure
*/
int
bno055_set_sensor_offsets(struct bno055_sensor_offsets *offsets)
{
uint8_t prev_mode;
int rc;
rc = bno055_get_opr_mode(&prev_mode);
if (rc) {
goto err;
}
rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
os_time_delay((25 * OS_TICKS_PER_SEC)/1000 + 1);
rc |= bno055_write8(BNO055_ACCEL_OFFSET_X_LSB_ADDR, (offsets->bso_acc_off_x) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_OFFSET_X_MSB_ADDR, (offsets->bso_acc_off_x >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_OFFSET_Y_LSB_ADDR, (offsets->bso_acc_off_y) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_OFFSET_Y_MSB_ADDR, (offsets->bso_acc_off_y >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_OFFSET_Z_LSB_ADDR, (offsets->bso_acc_off_z) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_OFFSET_Z_MSB_ADDR, (offsets->bso_acc_off_z >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_GYRO_OFFSET_X_LSB_ADDR, (offsets->bso_gyro_off_x) & 0x0FF);
rc |= bno055_write8(BNO055_GYRO_OFFSET_X_MSB_ADDR, (offsets->bso_gyro_off_x >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_GYRO_OFFSET_Y_LSB_ADDR, (offsets->bso_gyro_off_y) & 0x0FF);
rc |= bno055_write8(BNO055_GYRO_OFFSET_Y_MSB_ADDR, (offsets->bso_gyro_off_y >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_GYRO_OFFSET_Z_LSB_ADDR, (offsets->bso_gyro_off_z) & 0x0FF);
rc |= bno055_write8(BNO055_GYRO_OFFSET_Z_MSB_ADDR, (offsets->bso_gyro_off_z >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_OFFSET_X_LSB_ADDR, (offsets->bso_mag_off_x) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_OFFSET_X_MSB_ADDR, (offsets->bso_mag_off_x >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_OFFSET_Y_LSB_ADDR, (offsets->bso_mag_off_y) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_OFFSET_Y_MSB_ADDR, (offsets->bso_mag_off_y >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_OFFSET_Z_LSB_ADDR, (offsets->bso_mag_off_z) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_OFFSET_Z_MSB_ADDR, (offsets->bso_mag_off_z >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_RADIUS_LSB_ADDR, (offsets->bso_acc_radius) & 0x0FF);
rc |= bno055_write8(BNO055_ACCEL_RADIUS_MSB_ADDR, (offsets->bso_acc_radius >> 8) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_RADIUS_LSB_ADDR, (offsets->bso_mag_radius) & 0x0FF);
rc |= bno055_write8(BNO055_MAG_RADIUS_MSB_ADDR, (offsets->bso_mag_radius >> 8) & 0x0FF);
rc |= bno055_set_opr_mode(prev_mode);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
static void
sem_test_sleep_task_handler(void *arg)
{
struct os_task *t;
t = os_sched_get_current_task();
TEST_ASSERT(t->t_func == sem_test_sleep_task_handler);
os_time_delay(2000);
os_test_restart();
}
void task_3()
{
while(1)
{
uart1.printf("Task 3 Running!!!\r\n");
cpu = os_get_cpu();
mem = os_get_stack_max_usage(TASK_1_STK,TASK_1_STK_SIZE);
uart1.printf("cpu = %0.2f%%\r\n",cpu);
uart1.printf("mem = %02d%%\r\n",mem);
os_time_delay(1000);
}
}
void
sem_test_sleep_task_handler(void *arg)
{
struct os_task *t;
t = os_sched_get_current_task();
TEST_ASSERT(t->t_func == sem_test_sleep_task_handler);
os_time_delay(2 * OS_TICKS_PER_SEC);
#if MYNEWT_VAL(SELFTEST)
os_test_restart();
#endif
}
int
tsl2561_get_data(uint16_t *broadband, uint16_t *ir, struct tsl2561 *tsl2561)
{
int rc;
int delay_ticks;
/* Wait integration time ms before getting a data sample */
switch (tsl2561->cfg.integration_time) {
case TSL2561_LIGHT_ITIME_13MS:
delay_ticks = 14 * OS_TICKS_PER_SEC / 1000;
break;
case TSL2561_LIGHT_ITIME_101MS:
delay_ticks = 102 * OS_TICKS_PER_SEC / 1000;
break;
case TSL2561_LIGHT_ITIME_402MS:
default:
delay_ticks = 403 * OS_TICKS_PER_SEC / 1000;
break;
}
os_time_delay(delay_ticks);
*broadband = *ir = 0;
rc = tsl2561_read16(TSL2561_COMMAND_BIT | TSL2561_WORD_BIT | TSL2561_REGISTER_CHAN0_LOW,
broadband);
if (rc) {
goto err;
}
rc = tsl2561_read16(TSL2561_COMMAND_BIT | TSL2561_WORD_BIT | TSL2561_REGISTER_CHAN1_LOW,
ir);
if (rc) {
goto err;
}
#if MYNEWT_VAL(TSL2561_STATS)
switch (tsl2561->cfg.integration_time) {
case TSL2561_LIGHT_ITIME_13MS:
STATS_INC(g_tsl2561stats, samples_13ms);
break;
case TSL2561_LIGHT_ITIME_101MS:
STATS_INC(g_tsl2561stats, samples_101ms);
break;
case TSL2561_LIGHT_ITIME_402MS:
STATS_INC(g_tsl2561stats, samples_402ms);
default:
break;
}
#endif
err:
return rc;
}
/**
* timer_task_handler
*
* The task function for Timer Task. Waits 20 seconds then
* prints simulation info. Swaps Task A and B's priorities
* and runs another 20 second simulation, printing final
* stats after the second run.
*/
void
timer_task_handler(void *arg)
{
while (1) {
console_printf(" Starting First Simulation...\n");
/* Wait for tasks to run */
os_time_delay(OS_TICKS_PER_SEC * TIMER_LENGTH_SEC);
/* Print data and save total loops */
int total1 = print_task_data();
/* Save initial runtime */
a_time_offset = a_task.t_run_time;
b_time_offset = b_task.t_run_time;
/* Change Priorities */
int tmp = a_task.t_prio;
a_task.t_prio = b_task.t_prio;
os_sched_resort(&a_task);
b_task.t_prio = tmp;
os_sched_resort(&b_task);
/* Reset Tasks */
console_printf(" Switching priorities and restarting...\n");
a_loops = 0;
b_loops = 0;
restart_a = 1;
restart_b= 1;
/* Wait for tasks to run again*/
os_time_delay(OS_TICKS_PER_SEC * TIMER_LENGTH_SEC);
int total2 = print_task_data();
/* Print final speedup */
double speedup = ((double)total2/total1);
print_double("\n\n Final Speedup (Sim2 / Sim1): ", speedup, "");
while(1) {}
}
}
/**
* Setting power mode for the bno055 sensor
*
* @param power mode for the sensor
* @return 0 on success, non-zero on failure
*/
int
bno055_set_pwr_mode(uint8_t mode)
{
int rc;
rc = bno055_write8(BNO055_PWR_MODE_ADDR, mode);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 1)/1000 + 1);
return 0;
err:
return rc;
}
static void
console_queue_char(char ch)
{
struct console_tty *ct = &console_tty;
int sr;
OS_ENTER_CRITICAL(sr);
while (CONSOLE_HEAD_INC(&ct->ct_tx) == ct->ct_tx.cr_tail) {
/* TX needs to drain */
hal_uart_start_tx(CONSOLE_UART);
OS_EXIT_CRITICAL(sr);
os_time_delay(1);
OS_ENTER_CRITICAL(sr);
}
console_add_char(&ct->ct_tx, ch);
OS_EXIT_CRITICAL(sr);
}
/**
* a_task_handler
*
* The task function for Task A. Loops through A_LOOP_SIZE
* with a delay of time A_DELAY between each full loop.
*/
void
a_task_handler(void *arg)
{
while (1) {
int i;
for(i = 0; i < A_LOOP_SIZE; ++i) {
if(restart_a) break;
++a_loops;
/* Simulate doing a noticeable amount of work */
fake_work_function(i);
}
if(restart_a) {
restart_a = 0;
continue;
}
if(VERBOSE) {
console_printf(" %s looped\n", a_task.t_name);
}
os_time_delay(A_DELAY);
}
}
void
task1_handler(void *arg)
{
struct os_task *t;
int prev_pin_state, curr_pin_state;
struct image_version ver;
/* Set the led pin for the E407 devboard */
g_led_pin = LED_BLINK_PIN;
hal_gpio_init_out(g_led_pin, 1);
if (imgr_my_version(&ver) == 0) {
console_printf("\nSlinky_OIC %u.%u.%u.%u\n",
ver.iv_major, ver.iv_minor, ver.iv_revision,
(unsigned int)ver.iv_build_num);
} else {
console_printf("\nSlinky\n");
}
while (1) {
t = os_sched_get_current_task();
assert(t->t_func == task1_handler);
++g_task1_loops;
/* Wait one second */
os_time_delay(OS_TICKS_PER_SEC);
/* Toggle the LED */
prev_pin_state = hal_gpio_read(g_led_pin);
curr_pin_state = hal_gpio_toggle(g_led_pin);
LOG_INFO(&my_log, LOG_MODULE_DEFAULT, "GPIO toggle from %u to %u",
prev_pin_state, curr_pin_state);
STATS_INC(g_stats_gpio_toggle, toggles);
/* Release semaphore to task 2 */
os_sem_release(&g_test_sem);
}
}
/**
* b_task_handler
*
* The task function for Task B. Loops through B_LOOP_SIZE
* with a delay of time B_DELAY between each full loop.
*/
void
b_task_handler(void *arg)
{
while (1) {
int i;
for(i = 0; i < B_LOOP_SIZE; ++i) {
if(restart_b) break;
++b_loops;
/* Simulate doing a noticeable amount of work if verbose is off */
if(!VERBOSE) fake_work_function(i);
/* Print updates if verbose is set. Replaces fake_work_function */
if (VERBOSE && i % 10000 == 0) {
console_printf(" %s: %d%% \n", b_task.t_name, i/10000);
}
}
/* If reset_b is 1, skip the delay */
if(restart_b) {
restart_b = 0;
continue;
}
os_time_delay(B_DELAY);
}
}
/**
* Reset lis2dw12
*
* @param The sensor interface
* @return 0 on success, non-zero on failure
*/
int
lis2dw12_reset(struct sensor_itf *itf)
{
int rc;
uint8_t reg;
rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG2, ®);
if (rc) {
goto err;
}
reg |= LIS2DW12_CTRL_REG2_SOFT_RESET | LIS2DW12_CTRL_REG2_BOOT;
rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG2, reg);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 6/1000) + 1);
err:
return rc;
}
/**
*
* Writes calibration data to the sensor's offset registers
*
* @param calibration data
* @param calibration data length
* @return 0 on success, non-zero on success
*/
int
bno055_set_sensor_raw_offsets(uint8_t* calibdata, uint8_t len)
{
uint8_t prev_mode;
int rc;
if (len != 22) {
rc = SYS_EINVAL;
goto err;
}
rc = bno055_get_opr_mode(&prev_mode);
if (rc) {
goto err;
}
rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
os_time_delay((25 * OS_TICKS_PER_SEC)/1000 + 1);
rc = bno055_writelen(BNO055_ACCEL_OFFSET_X_LSB_ADDR, calibdata, len);
if (rc) {
goto err;
}
rc = bno055_set_opr_mode(prev_mode);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
int
bno055_config(struct bno055 *bno055, struct bno055_cfg *cfg)
{
int rc;
uint8_t id;
uint8_t mode;
/* Check if we can read the chip address */
rc = bno055_get_chip_id(&id);
if (rc) {
goto err;
}
if (id != BNO055_ID) {
os_time_delay((OS_TICKS_PER_SEC * 100)/1000 + 1);
rc = bno055_get_chip_id(&id);
if (rc) {
goto err;
}
if(id != BNO055_ID) {
rc = SYS_EINVAL;
goto err;
}
}
/* Reset sensor */
rc = bno055_write8(BNO055_SYS_TRIGGER_ADDR, BNO055_SYS_TRIGGER_RST_SYS);
if (rc) {
goto err;
}
os_time_delay(OS_TICKS_PER_SEC);
rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
/* Set to normal power mode */
rc = bno055_set_pwr_mode(cfg->bc_pwr_mode);
if (rc) {
goto err;
}
bno055->cfg.bc_pwr_mode = cfg->bc_pwr_mode;
/**
* As per Section 5.5 in the BNO055 Datasheet,
* external crystal should be used for accurate
* results
*/
rc = bno055_set_ext_xtal_use(cfg->bc_use_ext_xtal, BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
bno055->cfg.bc_use_ext_xtal = cfg->bc_use_ext_xtal;
/* Setting units and data output format */
rc = bno055_set_units(cfg->bc_units);
if (rc) {
goto err;
}
bno055->cfg.bc_units = cfg->bc_units;
/* Change mode to requested mode */
rc = bno055_set_opr_mode(cfg->bc_opr_mode);
if (rc) {
goto err;
}
os_time_delay(OS_TICKS_PER_SEC/2);
rc = bno055_get_opr_mode(&mode);
if (rc) {
goto err;
}
if (cfg->bc_opr_mode != mode) {
/* Trying to set operation mode again */
rc = bno055_set_opr_mode(cfg->bc_opr_mode);
if (rc) {
goto err;
}
rc = bno055_get_opr_mode(&mode);
if (rc) {
goto err;
}
if (cfg->bc_opr_mode != mode) {
BNO055_ERR("Config mode and read mode do not match.\n");
rc = SYS_EINVAL;
goto err;
}
}
bno055->cfg.bc_opr_mode = cfg->bc_opr_mode;
rc = bno055_acc_cfg(cfg);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Writes a single byte to the specified register
*
* @param The register address to write to
* @param The value to write
*
* @return 0 on success, non-zero error on failure.
*/
int
bno055_write8(uint8_t reg, uint8_t value)
{
int rc;
uint8_t payload[2] = { reg, value};
struct hal_i2c_master_data data_struct = {
.address = MYNEWT_VAL(BNO055_I2CADDR),
.len = 2,
.buffer = payload
};
rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC, 1);
if (rc) {
BNO055_ERR("Failed to write to 0x%02X:0x%02X with value 0x%02X\n",
data_struct.address, reg, value);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
}
return rc;
}
/**
* Writes a multiple bytes to the specified register
*
* @param The register address to write to
* @param The data buffer to write from
*
* @return 0 on success, non-zero error on failure.
*/
int
bno055_writelen(uint8_t reg, uint8_t *buffer, uint8_t len)
{
int rc;
uint8_t payload[23] = { reg, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = MYNEWT_VAL(BNO055_I2CADDR),
.len = 1,
.buffer = payload
};
memcpy(&payload[1], buffer, len);
/* Register write */
rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
BNO055_ERR("I2C access failed at address 0x%02X\n", addr);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
goto err;
}
memset(payload, 0, sizeof(payload));
data_struct.len = len;
rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC / 10, len);
if (rc) {
BNO055_ERR("Failed to read from 0x%02X:0x%02X\n", addr, reg);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
goto err;
}
return 0;
err:
return rc;
}
/**
* Reads a single byte from the specified register
*
* @param The register address to read from
* @param Pointer to where the register value should be written
*
* @return 0 on success, non-zero error on failure.
*/
int
bno055_read8(uint8_t reg, uint8_t *value)
{
int rc;
uint8_t payload;
struct hal_i2c_master_data data_struct = {
.address = MYNEWT_VAL(BNO055_I2CADDR),
.len = 1,
.buffer = &payload
};
/* Register write */
payload = reg;
rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC / 10, 0);
if (rc) {
BNO055_ERR("I2C register write failed at address 0x%02X:0x%02X\n",
data_struct.address, reg);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
goto err;
}
/* Read one byte back */
payload = 0;
rc = hal_i2c_master_read(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC / 10, 1);
*value = payload;
if (rc) {
BNO055_ERR("Failed to read from 0x%02X:0x%02X\n", addr, reg);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
}
err:
return rc;
}
/**
* Read data from the sensor of variable length (MAX: 8 bytes)
*
*
* @param Register to read from
* @param Bufer to read into
* @param Length of the buffer
*
* @return 0 on success and non-zero on failure
*/
static int
bno055_readlen(uint8_t reg, uint8_t *buffer, uint8_t len)
{
int rc;
uint8_t payload[23] = { reg, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = MYNEWT_VAL(BNO055_I2CADDR),
.len = 1,
.buffer = payload
};
/* Clear the supplied buffer */
memset(buffer, 0, len);
/* Register write */
rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
BNO055_ERR("I2C access failed at address 0x%02X\n", addr);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
goto err;
}
/* Read len bytes back */
memset(payload, 0, sizeof(payload));
data_struct.len = len;
rc = hal_i2c_master_read(MYNEWT_VAL(BNO055_I2CBUS), &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
BNO055_ERR("Failed to read from 0x%02X:0x%02X\n", addr, reg);
#if MYNEWT_VAL(BNO055_STATS)
STATS_INC(g_bno055stats, errors);
#endif
goto err;
}
/* Copy the I2C results into the supplied buffer */
memcpy(buffer, payload, len);
return 0;
err:
return rc;
}
/**
* Setting operation mode for the bno055 sensor
*
* @param Operation mode for the sensor
* @return 0 on success, non-zero on failure
*/
int
bno055_set_opr_mode(uint8_t mode)
{
int rc;
rc = bno055_write8(BNO055_OPR_MODE_ADDR, BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 19)/1000 + 1);
rc = bno055_write8(BNO055_OPR_MODE_ADDR, mode);
if (rc) {
goto err;
}
/* Refer table 3-6 in the datasheet for the delay values */
os_time_delay((OS_TICKS_PER_SEC * 7)/1000 + 1);
return 0;
err:
return rc;
}
/**
* BLE test task
*
* @param arg
*/
void
bletest_task_handler(void *arg)
{
int rc;
uint64_t event_mask;
struct os_event *ev;
struct os_callout_func *cf;
/* Set LED blink rate */
g_bletest_led_rate = OS_TICKS_PER_SEC / 20;
/* Wait one second before starting test task */
os_time_delay(OS_TICKS_PER_SEC);
/* Initialize eventq */
os_eventq_init(&g_bletest_evq);
/* Initialize the host timer */
os_callout_func_init(&g_bletest_timer, &g_bletest_evq, bletest_timer_cb,
NULL);
/* Send the reset command first */
rc = host_hci_cmd_send(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET,
0, NULL);
assert(rc == 0);
host_hci_outstanding_opcode = 0;
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_ADVERTISER)
/* Initialize the advertiser */
console_printf("Starting BLE test task as advertiser\n");
bletest_init_advertising();
#endif
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_SCANNER)
/* Initialize the scanner */
console_printf("Starting BLE test task as scanner\n");
bletest_init_scanner();
#endif
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_INITIATOR)
/* Initialize the scanner */
console_printf("Starting BLE test task as initiator\n");
bletest_init_initiator();
#endif
/* Set the event mask we want to display */
event_mask = 0x7FF;
rc = host_hci_cmd_le_set_event_mask(event_mask);
assert(rc == 0);
host_hci_outstanding_opcode = 0;
/* Turn on all events */
event_mask = 0xffffffffffffffff;
rc = host_hci_cmd_set_event_mask(event_mask);
assert(rc == 0);
host_hci_outstanding_opcode = 0;
/* Turn on all events */
rc = host_hci_cmd_rd_local_version();
assert(rc == 0);
host_hci_outstanding_opcode = 0;
/* Wait some time before starting */
os_time_delay(OS_TICKS_PER_SEC);
/* Init bletest variables */
g_bletest_state = 0;
g_next_os_time = os_time_get();
/* Begin advertising if we are an advertiser */
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_ADVERTISER)
rc = host_hci_cmd_le_set_adv_enable(1);
assert(rc == 0);
host_hci_outstanding_opcode = 0;
#endif
bletest_timer_cb(NULL);
while (1) {
ev = os_eventq_get(&g_bletest_evq);
switch (ev->ev_type) {
case OS_EVENT_T_TIMER:
cf = (struct os_callout_func *)ev;
assert(cf->cf_func);
cf->cf_func(cf->cf_arg);
break;
default:
assert(0);
break;
}
}
}
/**
* Gets system status, test results and errors if any from the sensor
*
* @param ptr to system status
* @param ptr to self test result
* @param ptr to system error
*/
int
bno055_get_sys_status(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
{
int rc;
rc = bno055_write8(BNO055_PAGE_ID_ADDR, 0);
if (rc) {
goto err;
}
/**
* System Status (see section 4.3.58)
* ---------------------------------
* bit 0: Idle
* bit 1: System Error
* bit 2: Initializing Peripherals
* bit 3: System Iniitalization
* bit 4: Executing Self-Test
* bit 5: Sensor fusion algorithm running
* bit 6: System running without fusion algorithms
*/
if (system_status != 0) {
rc = bno055_read8(BNO055_SYS_STAT_ADDR, system_status);
if (rc) {
goto err;
}
}
/**
* Self Test Results (see section )
* --------------------------------
* 1: test passed, 0: test failed
* bit 0: Accelerometer self test
* bit 1: Magnetometer self test
* bit 2: Gyroscope self test
* bit 3: MCU self test
*
* 0x0F : All Good
*/
if (self_test_result != 0) {
rc = bno055_read8(BNO055_SELFTEST_RESULT_ADDR, self_test_result);
if (rc) {
goto err;
}
}
/**
* System Error (see section 4.3.59)
* ---------------------------------
* bit 0 : No error
* bit 1 : Peripheral initialization error
* bit 2 : System initialization error
* bit 3 : Self test result failed
* bit 4 : Register map value out of range
* bit 5 : Register map address out of range
* bit 6 : Register map write error
* bit 7 : BNO low power mode not available for selected operat ion mode
* bit 8 : Accelerometer power mode not available
* bit 9 : Fusion algorithm configuration error
* bit 10 : Sensor configuration error
*/
if (system_error != 0) {
rc = bno055_read8(BNO055_SYS_ERR_ADDR, system_error);
if (rc) {
goto err;
}
}
os_time_delay((OS_TICKS_PER_SEC * 200)/1000 + 1);
return 0;
err:
return rc;
}
/**
* Writes a single byte to the specified register
*
* @param The sensor interface
* @param The register address to write to
* @param The value to write
*
* @return 0 on success, non-zero error on failure.
*/
int
tcs34725_write8(struct sensor_itf *itf, uint8_t reg, uint32_t value)
{
int rc;
uint8_t payload[2] = { reg | TCS34725_COMMAND_BIT, value & 0xFF };
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = 2,
.buffer = payload
};
rc = sensor_itf_lock(itf, MYNEWT_VAL(TCS34725_ITF_LOCK_TMO));
if (rc) {
return rc;
}
rc = hal_i2c_master_write(itf->si_num, &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
TCS34725_LOG(ERROR,
"Failed to write to 0x%02X:0x%02X with value 0x%02lX\n",
data_struct.address, reg, value);
STATS_INC(g_tcs34725stats, errors);
}
sensor_itf_unlock(itf);
return rc;
}
/**
* Reads a single byte from the specified register
*
* @param The sensor interface
* @param The register address to read from
* @param Pointer to where the register value should be written
*
* @return 0 on success, non-zero error on failure.
*/
int
tcs34725_read8(struct sensor_itf *itf, uint8_t reg, uint8_t *value)
{
int rc;
uint8_t payload;
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = 1,
.buffer = &payload
};
rc = sensor_itf_lock(itf, MYNEWT_VAL(TCS34725_ITF_LOCK_TMO));
if (rc) {
return rc;
}
/* Register write */
payload = reg | TCS34725_COMMAND_BIT;
rc = hal_i2c_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1);
if (rc) {
TCS34725_LOG(ERROR, "I2C access failed at address 0x%02X\n",
data_struct.address);
STATS_INC(g_tcs34725stats, errors);
goto err;
}
/* Read one byte back */
payload = 0;
rc = hal_i2c_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1);
*value = payload;
if (rc) {
TCS34725_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
data_struct.address, reg);
STATS_INC(g_tcs34725stats, errors);
}
err:
sensor_itf_unlock(itf);
return rc;
}
/**
* Read data from the sensor of variable length (MAX: 8 bytes)
*
* @param Register to read from
* @param Bufer to read into
* @param Length of the buffer
*
* @return 0 on success and non-zero on failure
*/
int
tcs34725_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer, uint8_t len)
{
int rc;
uint8_t payload[9] = { reg | TCS34725_COMMAND_BIT, 0, 0, 0, 0, 0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = 1,
.buffer = payload
};
/* Clear the supplied buffer */
memset(buffer, 0, len);
rc = sensor_itf_lock(itf, MYNEWT_VAL(TCS34725_ITF_LOCK_TMO));
if (rc) {
return rc;
}
/* Register write */
rc = hal_i2c_master_write(itf->si_num, &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
TCS34725_LOG(ERROR, "I2C access failed at address 0x%02X\n",
data_struct.address);
STATS_INC(g_tcs34725stats, errors);
goto err;
}
/* Read len bytes back */
memset(payload, 0, sizeof(payload));
data_struct.len = len;
rc = hal_i2c_master_read(itf->si_num, &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
TCS34725_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
data_struct.address, reg);
STATS_INC(g_tcs34725stats, errors);
goto err;
}
/* Copy the I2C results into the supplied buffer */
memcpy(buffer, payload, len);
err:
sensor_itf_unlock(itf);
return rc;
}
/**
* Writes a multiple bytes to the specified register (MAX: 8 bytes)
*
* @param The sensor interface
* @param The register address to write to
* @param The data buffer to write from
*
* @return 0 on success, non-zero error on failure.
*/
int
tcs34725_writelen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer, uint8_t len)
{
int rc;
uint8_t payload[9] = { reg, 0, 0, 0, 0, 0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = 1,
.buffer = payload
};
if (len > (sizeof(payload) - 1)) {
rc = OS_EINVAL;
goto err;
}
memcpy(&payload[1], buffer, len);
rc = sensor_itf_lock(itf, MYNEWT_VAL(TCS34725_ITF_LOCK_TMO));
if (rc) {
return rc;
}
/* Register write */
rc = hal_i2c_master_write(itf->si_num, &data_struct,
OS_TICKS_PER_SEC / 10, 1);
if (rc) {
TCS34725_LOG(ERROR, "I2C access failed at address 0x%02X\n",
data_struct.address);
STATS_INC(g_tcs34725stats, errors);
goto err;
}
memset(payload, 0, sizeof(payload));
data_struct.len = len;
rc = hal_i2c_master_write(itf->si_num, &data_struct,
OS_TICKS_PER_SEC / 10, len);
if (rc) {
TCS34725_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
data_struct.address, reg);
STATS_INC(g_tcs34725stats, errors);
goto err;
}
err:
sensor_itf_unlock(itf);
return rc;
}
#if MYNEWT_VAL(MATHLIB_SUPPORT)
/**
* Float power function
*
* @param float base
* @param float exponent
*/
static float
powf(float base, float exp)
{
return (float)(pow((double)base, (double)exp));
}
#endif
/**
*
* Enables the device
*
* @param The sensor interface
* @param enable/disable
* @return 0 on success, non-zero on error
*/
int
tcs34725_enable(struct sensor_itf *itf, uint8_t enable)
{
int rc;
uint8_t reg;
rc = tcs34725_read8(itf, TCS34725_REG_ENABLE, ®);
if (rc) {
goto err;
}
os_time_delay((3 * OS_TICKS_PER_SEC)/1000 + 1);
if (enable) {
rc = tcs34725_write8(itf, TCS34725_REG_ENABLE,
reg | TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN);
if (rc) {
goto err;
}
} else {
rc = tcs34725_write8(itf, TCS34725_REG_ENABLE, reg &
~(TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN));
if (rc) {
goto err;
}
}
return 0;
err:
return rc;
}
void
task1_handler(void *arg)
{
int i;
int rc;
uint16_t rxval;
uint8_t last_val;
uint8_t spi_nb_cntr;
uint8_t spi_b_cntr;
/* Set the led pin for the E407 devboard */
g_led_pin = LED_BLINK_PIN;
hal_gpio_init_out(g_led_pin, 1);
/* Use SS pin for testing */
hal_gpio_init_out(SPI_SS_PIN, 1);
sblinky_spi_cfg(0);
hal_spi_set_txrx_cb(0, NULL, NULL);
hal_spi_enable(0);
/*
* Send some bytes in a non-blocking manner to SPI using tx val. The
* slave should send back 0x77.
*/
g_spi_tx_buf[0] = 0xde;
g_spi_tx_buf[1] = 0xad;
g_spi_tx_buf[2] = 0xbe;
g_spi_tx_buf[3] = 0xef;
hal_gpio_write(SPI_SS_PIN, 0);
for (i = 0; i < 4; ++i) {
rxval = hal_spi_tx_val(0, g_spi_tx_buf[i]);
assert(rxval == 0x77);
g_spi_rx_buf[i] = (uint8_t)rxval;
}
hal_gpio_write(SPI_SS_PIN, 1);
++g_spi_xfr_num;
/* Set up the callback to use when non-blocking API used */
hal_spi_disable(0);
spi_cb_arg = &spi_cb_obj;
spi_cb_obj.txlen = 32;
hal_spi_set_txrx_cb(0, sblinky_spi_irqm_handler, spi_cb_arg);
hal_spi_enable(0);
spi_nb_cntr = 0;
spi_b_cntr = 0;
while (1) {
/* Wait one second */
os_time_delay(OS_TICKS_PER_SEC);
/* Toggle the LED */
hal_gpio_toggle(g_led_pin);
/* Get random length to send */
g_last_tx_len = spi_cb_obj.txlen;
spi_cb_obj.txlen = (rand() & 0x1F) + 1;
memcpy(g_spi_last_tx_buf, g_spi_tx_buf, g_last_tx_len);
last_val = g_spi_last_tx_buf[g_last_tx_len - 1];
for (i= 0; i < spi_cb_obj.txlen; ++i) {
g_spi_tx_buf[i] = (uint8_t)(last_val + i);
}
if (g_spi_xfr_num & 1) {
/* Send non-blocking */
++spi_nb_cntr;
assert(hal_gpio_read(SPI_SS_PIN) == 1);
hal_gpio_write(SPI_SS_PIN, 0);
#if 0
if (spi_nb_cntr == 7) {
g_spi_null_rx = 1;
rc = hal_spi_txrx_noblock(0, g_spi_tx_buf, NULL, 32);
} else {
g_spi_null_rx = 0;
rc = hal_spi_txrx_noblock(0, g_spi_tx_buf, g_spi_rx_buf, 32);
}
assert(!rc);
#else
g_spi_null_rx = 0;
rc = hal_spi_txrx_noblock(0, g_spi_tx_buf, g_spi_rx_buf,
spi_cb_obj.txlen);
assert(!rc);
console_printf("a transmitted: ");
for (i = 0; i < spi_cb_obj.txlen; i++) {
console_printf("%2x ", g_spi_tx_buf[i]);
}
console_printf("\n");
console_printf("received: ");
for (i = 0; i < spi_cb_obj.txlen; i++) {
console_printf("%2x ", g_spi_rx_buf[i]);
}
console_printf("\n");
#endif
} else {
/* Send blocking */
++spi_b_cntr;
assert(hal_gpio_read(SPI_SS_PIN) == 1);
hal_gpio_write(SPI_SS_PIN, 0);
#if 0
if (spi_b_cntr == 7) {
g_spi_null_rx = 1;
rc = hal_spi_txrx(0, g_spi_tx_buf, NULL, 32);
spi_b_cntr = 0;
} else {
g_spi_null_rx = 0;
rc = hal_spi_txrx(0, g_spi_tx_buf, g_spi_rx_buf, 32);
}
assert(!rc);
hal_gpio_write(SPI_SS_PIN, 1);
spitest_validate_last(spi_cb_obj.txlen);
#else
rc = hal_spi_txrx(0, g_spi_tx_buf, g_spi_rx_buf, spi_cb_obj.txlen);
assert(!rc);
hal_gpio_write(SPI_SS_PIN, 1);
console_printf("b transmitted: ");
for (i = 0; i < spi_cb_obj.txlen; i++) {
console_printf("%2x ", g_spi_tx_buf[i]);
}
console_printf("\n");
console_printf("received: ");
for (i = 0; i < spi_cb_obj.txlen; i++) {
console_printf("%2x ", g_spi_rx_buf[i]);
}
console_printf("\n");
spitest_validate_last(spi_cb_obj.txlen);
++g_spi_xfr_num;
#endif
}
}
}
/**
*
* Converts raw RGB values to color temp in deg K and lux
*
* @param The sensor interface
* @param ptr to sensor color data ptr
* @param ptr to sensor
*/
static int
tcs34725_calc_colortemp_lux(struct sensor_itf *itf,
struct sensor_color_data *scd,
struct tcs34725 *tcs34725)
{
int rc;
rc = 0;
#if MYNEWT_VAL(USE_TCS34725_TAOS_DN25)
float n;
/**
* From the designer's notebook by TAOS:
* Mapping sensor response RGB values to CIE tristimulus values(XYZ)
* based on broad enough transformation, the light sources chosen were a
* high color temperature fluorescent (6500K), a low color temperature
* fluorescent (3000K), and an incandescent (60W)
* Note: y = Illuminance or lux
*
* For applications requiring more precision,
* narrower range of light sources should be used and a new correlation
* matrix could be formulated and CIE tristimulus values should be
* calculated. Please refer the manual for calculating tristumulus values.
*
* x = (-0.14282F * r) + (1.54924F * g) + (-0.95641F * b);
* y = (-0.32466F * r) + (1.57837F * g) + (-0.73191F * b);
* z = (-0.68202F * r) + (0.77073F * g) + ( 0.56332F * b);
*
*
* Calculating chromaticity co-ordinates, the light can be plotted on a two
* dimensional chromaticity diagram
*
* xc = x / (x + y + z);
* yc = y / (x + y + z);
*
* Use McCamy's formula to determine the CCT
* n = (xc - 0.3320F) / (0.1858F - yc);
*/
/*
* n can be calculated directly using the following formula for
* above considerations
*/
n = ((0.23881)*scd->scd_r + (0.25499)*scd->scd_g + (-0.58291)*scd->scd_b) /
((0.11109)*scd->scd_r + (-0.85406)*scd->scd_g + (0.52289)*scd->scd_b);
/*
* Calculate the final CCT
* CCT is only meant to characterize near white lights.
*/
#if MYNEWT_VAL(USE_MATH)
scd->scd_colortemp = (449.0F * powf(n, 3)) + (3525.0F * powf(n, 2)) + (6823.3F * n) + 5520.33F;
#else
scd->scd_colortemp = (449.0F * n * n * n) + (3525.0F * n * n) + (6823.3F * n) + 5520.33F;
#endif
scd->scd_lux = (-0.32466F * scd->scd_r) + (1.57837F * scd->scd_g) + (-0.73191F * scd->scd_b);
scd->scd_colortemp_is_valid = 1;
scd->scd_lux_is_valid = 1;
goto err;
#else
uint8_t againx;
uint8_t atime;
uint16_t atime_ms;
uint16_t r_comp;
uint16_t g_comp;
uint16_t b_comp;
float cpl;
uint8_t agc_cur;
const struct tcs_agc agc_list[4] = {
{ TCS34725_GAIN_60X, TCS34725_INTEGRATIONTIME_700MS, 0, 47566 },
{ TCS34725_GAIN_16X, TCS34725_INTEGRATIONTIME_154MS, 3171, 63422 },
{ TCS34725_GAIN_4X, TCS34725_INTEGRATIONTIME_154MS, 15855, 63422 },
{ TCS34725_GAIN_1X, TCS34725_INTEGRATIONTIME_2_4MS, 248, 0 }
};
agc_cur = 0;
while(1) {
if (agc_list[agc_cur].max_cnt && scd->scd_c > agc_list[agc_cur].max_cnt) {
agc_cur++;
} else if (agc_list[agc_cur].min_cnt &&
scd->scd_c < agc_list[agc_cur].min_cnt) {
agc_cur--;
break;
} else {
break;
}
rc = tcs34725_set_gain(itf, agc_list[agc_cur].ta_gain);
if (rc) {
goto err;
}
rc = tcs34725_set_integration_time(itf, agc_list[agc_cur].ta_time);
if (rc) {
goto err;
}
/* Shock absorber */
os_time_delay((256 - ((uint16_t)agc_list[agc_cur].ta_time) * 2.4 * 2 * OS_TICKS_PER_SEC)/1000 + 1);
rc = tcs34725_get_rawdata(itf, &scd->scd_r, &scd->scd_g, &scd->scd_b, &scd->scd_c, tcs34725);
if (rc) {
goto err;
}
break;
}
atime = (uint16_t)agc_list[agc_cur].ta_time;
/* Formula from the datasheet */
atime_ms = ((256 - atime) * 2.4);
switch(agc_list[agc_cur].ta_time) {
case TCS34725_GAIN_1X:
againx = 1;
break;
case TCS34725_GAIN_4X:
againx = 4;
break;
case TCS34725_GAIN_16X:
againx = 16;
break;
case TCS34725_GAIN_60X:
againx = 60;
break;
default:
rc = SYS_EINVAL;
goto err;
}
scd->scd_ir = (scd->scd_r + scd->scd_g + scd->scd_b > scd->scd_c) ?
(scd->scd_r + scd->scd_g + scd->scd_b - scd->scd_c) / 2 : 0;
r_comp = scd->scd_r - scd->scd_ir;
g_comp = scd->scd_g - scd->scd_ir;
b_comp = scd->scd_b - scd->scd_ir;
scd->scd_cratio = (float)scd->scd_ir / (float)scd->scd_c;
scd->scd_saturation = ((256 - atime) > 63) ? 65535 : 1024 * (256 - atime);
scd->scd_saturation75 = (atime_ms < 150) ? (scd->scd_saturation - scd->scd_saturation / 4) : scd->scd_saturation;
scd->scd_is_sat = (atime_ms < 150 && scd->scd_c > scd->scd_saturation75) ? 1 : 0;
cpl = (atime_ms * againx) / (TCS34725_GA * TCS34725_DF);
scd->scd_maxlux = 65535 / (cpl * 3);
scd->scd_lux = (TCS34725_R_COEF * (float)r_comp + TCS34725_G_COEF * (float)g_comp +
TCS34725_B_COEF * (float)b_comp) / cpl;
scd->scd_colortemp = TCS34725_CT_COEF * (float)b_comp / (float)r_comp + TCS34725_CT_OFFSET;
scd->scd_lux_is_valid = 1;
scd->scd_colortemp_is_valid = 1;
scd->scd_saturation_is_valid = 1;
scd->scd_saturation75_is_valid = 1;
scd->scd_is_sat_is_valid = 1;
scd->scd_cratio_is_valid = 1;
scd->scd_maxlux_is_valid = 1;
scd->scd_ir_is_valid = 1;
#endif
err:
return rc;
}
/**
* BLE test task
*
* @param arg
*/
void
bletest_task_handler(void *arg)
{
int rc;
uint64_t rand64;
uint64_t event_mask;
/* Set LED blink rate */
g_bletest_led_rate = OS_TICKS_PER_SEC / 20;
/* Wait one second before starting test task */
os_time_delay(OS_TICKS_PER_SEC);
/* Initialize the host timer */
os_callout_init(&g_bletest_timer, &g_bletest_evq, bletest_timer_cb,
NULL);
ble_hs_dbg_set_sync_state(BLE_HS_SYNC_STATE_GOOD);
/* Send the reset command first */
rc = bletest_hci_reset_ctlr();
assert(rc == 0);
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_ADVERTISER)
/* Initialize the advertiser */
console_printf("Starting BLE test task as advertiser\n");
#if MYNEWT_VAL(BLE_ANDROID_MULTI_ADV_SUPPORT)
/* Start up all advertising instances except default one */
bletest_init_adv_instances();
/* Start advertising on instance 0 at 0 dbm */
bletest_init_advertising(0, 0);
#else
bletest_init_advertising();
#endif
#endif
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_SCANNER)
/* Initialize the scanner */
console_printf("Starting BLE test task as scanner\n");
bletest_init_scanner();
#endif
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_INITIATOR)
/* Initialize the scanner */
console_printf("Starting BLE test task as initiator\n");
bletest_init_initiator();
#endif
/* Read unique HW id */
rc = hal_bsp_hw_id((void *)&g_bletest_hw_id[0], sizeof(g_bletest_hw_id));
assert(rc == 16);
console_printf("HW id=%04x%04x%04x%04x\n",
(unsigned int)g_bletest_hw_id[0],
(unsigned int)g_bletest_hw_id[1],
(unsigned int)g_bletest_hw_id[2],
(unsigned int)g_bletest_hw_id[3]);
/* Set the event mask we want to display */
event_mask = 0x7FF;
rc = bletest_hci_le_set_event_mask(event_mask);
assert(rc == 0);
/* Turn on all events */
event_mask = 0xffffffffffffffff;
rc = bletest_hci_set_event_mask(event_mask);
assert(rc == 0);
/* Read device address */
rc = bletest_hci_rd_bd_addr();
assert(rc == 0);
/* Read local features */
rc = bletest_hci_rd_local_feat();
assert(rc == 0);
/* Read local commands */
rc = bletest_hci_rd_local_supp_cmd();
assert(rc == 0);
/* Read version */
rc = bletest_hci_rd_local_version();
assert(rc == 0);
/* Read supported states */
rc = bletest_hci_le_read_supp_states();
assert(rc == 0);
/* Read maximum data length */
rc = bletest_hci_le_rd_max_datalen();
assert(rc == 0);
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) == 1)
/* Read suggested data length */
rc = bletest_hci_le_rd_sugg_datalen();
assert(rc == 0);
/* write suggested default data length */
rc = bletest_hci_le_write_sugg_datalen(BLETEST_CFG_SUGG_DEF_TXOCTETS,
BLETEST_CFG_SUGG_DEF_TXTIME);
assert(rc == 0);
/* Read suggested data length */
rc = bletest_hci_le_rd_sugg_datalen();
assert(rc == 0);
/* Set data length (note: we know there is no connection; just a test) */
rc = bletest_hci_le_set_datalen(0x1234, BLETEST_CFG_SUGG_DEF_TXOCTETS,
BLETEST_CFG_SUGG_DEF_TXTIME);
assert(rc != 0);
#endif
/* Encrypt a block */
#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
rc = bletest_hci_le_encrypt((uint8_t *)g_ble_ll_encrypt_test_key,
(uint8_t *)g_ble_ll_encrypt_test_plain_text);
assert(rc == 0);
#endif
/* Get a random number */
rc = ble_hs_hci_util_rand(&rand64, 8);
assert(rc == 0);
/* Wait some time before starting */
os_time_delay(OS_TICKS_PER_SEC);
/* Init state */
g_bletest_state = 0;
/* Begin advertising if we are an advertiser */
#if (BLETEST_CFG_ROLE == BLETEST_ROLE_ADVERTISER)
#if MYNEWT_VAL(BLE_ANDROID_MULTI_ADV_SUPPORT)
rc = bletest_hci_le_set_multi_adv_enable(1, 0);
assert(rc == 0);
#else
rc = bletest_hci_le_set_adv_enable(1);
assert(rc == 0);
#endif
#endif
bletest_timer_cb(NULL);
while (1) {
os_eventq_run(&g_bletest_evq);
}
}
