/**@brief Location and navigation time-out handler.
*
* @details This function will be called each time the location and navigation measurement timer expires.
*
* @param[in] p_context Pointer used for passing some arbitrary information (context) from the
* app_start_timer() call to the time-out handler.
*/
static void loc_and_nav_timeout_handler(void * p_context)
{
uint32_t err_code;
UNUSED_PARAMETER(p_context);
loc_speed_simulation_update();
position_quality_simulation_update();
navigation_simulation_update();
err_code = ble_lns_loc_speed_send(&m_lns);
if (
(err_code != NRF_ERROR_INVALID_STATE)
&&
(err_code != BLE_ERROR_NO_TX_BUFFERS)
&&
(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
)
{
APP_ERROR_CHECK(err_code);
}
err_code = ble_lns_navigation_send(&m_lns);
if (
(err_code != NRF_ERROR_INVALID_STATE)
&&
(err_code != BLE_ERROR_NO_TX_BUFFERS)
&&
(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
)
{
APP_ERROR_CHECK(err_code);
}
}
/**@brief Location and navigation time-out handler.
*
* @details This function will be called each time the location and navigation measurement timer expires.
*
* @param[in] p_context Pointer used for passing some arbitrary information (context) from the
* app_start_timer() call to the time-out handler.
*/
static void loc_and_nav_timeout_handler(void * p_context)
{
uint32_t err_code;
UNUSED_PARAMETER(p_context);
loc_speed_simulation_update();
position_quality_simulation_update();
navigation_simulation_update();
err_code = ble_lns_loc_speed_send(&m_lns);
if (err_code != NRF_ERROR_INVALID_STATE)
{
APP_ERROR_CHECK(err_code);
}
err_code = ble_lns_navigation_send(&m_lns);
if (err_code != NRF_ERROR_INVALID_STATE)
{
APP_ERROR_CHECK(err_code);
}
}
/**@brief Application main function.
*/
int main(void)
{
uint32_t err_code;
bool erase_bonds = false;
sI2Cchannel i2cChannel;
int16_t acc[3];
int16_t mag[3];
// bool erase_bonds;
uint16_t heading;
// SEGGER_RTT_WriteString(0, "Hello World 3!\n");
// Initialize
app_trace_init();
app_trace_log( "test trace log\r\n" );
printf( "Hej, printf!\n" );
timers_init();
// buttons_leds_init(&erase_bonds);
erl_ble_init( erase_bonds, lns_error_handler, sleep_mode_enter );
// sim_init();
// Start out slow, at 100 kbps. Keep it simple - blocking - to start
nrf51822_app_i2c_init( &i2cChannel, NRF_TWI_FREQ_400K, APP_IRQ_PRIORITY_LOW, 1, 0 );
// SEGGER_RTT_WriteString(0, "i2c initialized\n");
lsm303dlhc_init( &i2cChannel );
// Test tilt compensation
acc[0] = 20000;
acc[1] = 2;
acc[2] = 999;
mag[0] = -408;
mag[1] = -297;
mag[2] = 237;
heading = lsm303dlhc_tilt_compensate( acc, mag );
heading = ((uint32_t) heading) * 360 / UINT16_MAX;
printf( "Test heading: %d\n", heading );
lsm303dlhc_acc_setDataRate( ACC_RATE_1_HZ );
lsm303dlhc_mag_setMode( LSM303DLHC_MAG_MODE_CONTINUOUS );
lsm303dlhc_acc_read( acc );
lsm303dlhc_mag_read( mag );
printf( "Acc: %d,%d,%d\n", acc[0], acc[1], acc[2] );
printf( "Mag: %d,%d,%d\n", mag[0], mag[1], mag[2] );
// Start execution
// application_timers_start();
// Move this function call to erl_bleng
// why is fast/slow specified here, when it is also enabled/disabled in advertising options?
err_code = ble_advertising_start(BLE_ADV_MODE_SLOW); // changed from fast to slow
APP_ERROR_CHECK(err_code);
#if USE_BSP
err_code = bsp_indication_set(BSP_INDICATE_ADVERTISING);
APP_ERROR_CHECK(err_code);
#endif
APPL_LOG( "main(): before loop\r\n" );
// Enter main loop
for (;;)
{
lsm303dlhc_acc_read( acc );
lsm303dlhc_mag_read( mag );
heading = lsm303dlhc_tilt_compensate( acc, mag );
heading = ((uint32_t) heading) * 36000 / UINT16_MAX;
printf( "Acc: %d,%d,%d\n", acc[0], acc[1], acc[2] );
printf( "Mag: %d,%d,%d\n", mag[0], mag[1], mag[2] );
printf( "Heading=%d\n", heading );
m_sim_location_speed.heading = heading; // ++;
err_code = ble_lns_loc_speed_send(&m_lns);
if (
(err_code != NRF_ERROR_INVALID_STATE)
&&
(err_code != BLE_ERROR_NO_TX_BUFFERS)
&&
(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
)
{
APP_ERROR_CHECK(err_code);
}
nrf_delay_ms( 500 );
// fflush( stdout );
// power_manage();
}
}
