static int read_acc(const void *dev, phydat_t *res)
{
adxl345_read((const adxl345_t *)dev, (adxl345_data_t *)res->val);
res->unit = UNIT_G;
res->scale = -3;
return 3;
}
void adxl345_getxyz(int16_t *x, int16_t *y, int16_t *z)
{
uint16_t high, low;
//wait for a sample to be available
while(!(adxl345_read(INT_SOURCE) & DATA_READY));
high = adxl345_read(DATAX1);
low = adxl345_read(DATAX0);
*x = (high << 8) | low;
high = adxl345_read(DATAY1);
low = adxl345_read(DATAY0);
*y = (high << 8) | low;
high = adxl345_read(DATAZ1);
low = adxl345_read(DATAZ0);
*z = (high << 8) | low;
}
int main(void)
{
SetupHardware();
data_init(&data, &calib_params); //put some values into structure for testing
ahrs_init(&data, &u8g, &calib_params);
GlobalInterruptEnable();
for (;;)
{
//time between updates
timer_old = timer;
timer = get_timer_ms();
if (timer > timer_old) {
t_period = (timer - timer_old) / 1000.0; //in seconds
}
else
t_period = 0;
data.time_period = t_period;
//read data - sensors
adxl345_read(&data); //accelerometer read
l3g4200d_read_seq(&data); //gyroscope read
hmc5883l_read(&data); //magnetometer read
//buttons
buttons_read(&data);
//compute
//ahrs_orientation_from_gyro(&data);
ahrs_orientation(&data);
//ahrs_orientation_from_accel_mag(&data);
HID_Task();
USB_USBTask();
}
}
void shutdown(int forever)
{
// Turn off the LEDs
send_rgb(0,0,0);
// Reset the ADXL threshold level
adxl345_write(THRESH_ACT, 50); // Threshold to detect activity
// If we don't want to turn back on turn off the accelerometer
if(forever)
adxl345_write(POWER_CTL, 0);
// Loop until we die
while(1)
{
// Read the ADXL interrupts to clear out PWR_EN_ADXL
adxl345_read(INT_SOURCE);
// Lower the power latch (PWR_EN_CPU)
PORTC &= ~(_BV(PORTC4));
_delay_ms(5000);
}
}
uint16 SensorTag_ProcessEvent(uint8 task_id, uint16 events)
{
VOID task_id; // OSAL required parameter that isn't used in this function
///////////////////////////////////////////////////////////////////////
// system event handle //
///////////////////////////////////////////////////////////////////////
if (events & SYS_EVENT_MSG) {
uint8 *msg;
if ((msg = osal_msg_receive(sensorTag_TaskID)) != NULL) {
sensorTag_ProcessOSALMsg((osal_event_hdr_t *) msg);
// release the OSAL message
osal_msg_deallocate(msg);
}
// return unprocessed events
return (events ^ SYS_EVENT_MSG);
}
///////////////////////////////////////////////////////////////////////
// start device event //
///////////////////////////////////////////////////////////////////////
if (events & EVT_START_DEVICE) {
// start the device
GAPRole_StartDevice(&sensorTag_PeripheralCBs);
// start bond manager
#if !defined(GAPBONDMGR_NO_SUPPORT)
GAPBondMgr_Register(&sensorTag_BondMgrCBs);
#endif
// start peripheral device
oled_init();
adxl345_softrst();
// adxl345_self_calibration();
steps = 0;
BATCD_PXIFG = ~(BATCD_BV);
BATCD_IEN |= BATCD_IENBIT;
osal_start_reload_timer(sensorTag_TaskID, EVT_RTC, PERIOD_RTC);
pwmgr_state_change(PWMGR_S0, 0);
fmsg(("\033[40;32m\n[power on]\033[0m\n"));
return (events ^ EVT_START_DEVICE);
}
///////////////////////////////////////////////////////////////////////
// key long press handle //
///////////////////////////////////////////////////////////////////////
if (events & EVT_MODE) {
if (key1_press) {
oled_clr_screen();
if ((opmode & 0xF0) == MODE_NORMAL) {
opmode = MODE_WORKOUT | MODE_TIME;
workout.steps = normal.steps;
workout.time = osal_getClock();
fmsg(("\033[40;32m[workout mode]\033[0m\n"));
} else {
opmode = MODE_NORMAL | MODE_TIME;
fmsg(("\033[40;32m[normal mode]\033[0m\n"));
}
pwmgr_state_change(pwmgr, TIME_OLED_OFF);
}
return (events ^ EVT_MODE);
}
if (events & EVT_SLEEP) {
if (key1_press) {
oled_clr_screen();
opmode = MODE_SLEEP;
fmsg(("\033[40;32m[sleep mode]\033[0m\n"));
pwmgr_state_change(pwmgr, TIME_OLED_OFF);
}
return (events ^ EVT_SLEEP);
}
if (events & EVT_SYSRST) {
if (key1_press) {
fmsg(("\033[40;32m[system reset]\033[0m\n"));
HAL_SYSTEM_RESET();
// adxl345_self_calibration();
}
return (events ^ EVT_SYSRST);
}
///////////////////////////////////////////////////////////////////////
// display handle //
///////////////////////////////////////////////////////////////////////
if (events & EVT_DISP) {
if (pwmgr == PWMGR_S1) {
sensorTag_HandleDisp(opmode, acc);
} else {
// display battery only
sensorTag_BattDisp(batt_get_level());
}
if (pwmgr != PWMGR_S6) {
osal_start_timerEx(sensorTag_TaskID, EVT_DISP, PERIOD_DISP);
}
return (events ^ EVT_DISP);
}
///////////////////////////////////////////////////////////////////////
// g-sensor handle //
///////////////////////////////////////////////////////////////////////
if (events & EVT_GSNINT1) {
adxl345_exit_sleep();
pwmgr_state_change(PWMGR_S3, TIME_GSEN_OFF);
return (events ^ EVT_GSNINT1);
}
if (events & EVT_GSNINT2) {
unsigned char sampling;
unsigned char i;
sampling = adxl345_chk_fifo();
for (i=0; i<sampling; i++) {
adxl345_read(acc);
#if (DEBUG_MESSAGE & MSG_STEPS)
{
unsigned long tmp = algo_step(acc);
if (normal.steps != tmp) {
stepmsg(("\033[1;33mstep=%0lu\n\033[0m", tmp));
}
normal.steps = tmp;
}
#else
normal.steps = algo_step(acc);
#endif
}
normal.distance = calc_distance(normal.steps, pi.stride);
workout.distance = calc_distance((normal.steps - workout.steps), pi.stride);
normal.calorie = calc_calorie(normal.distance, pi.weight);
workout.calorie = calc_calorie(workout.distance, pi.weight);
return (events ^ EVT_GSNINT2);
}
if (events & EVT_GSENSOR) {
adxl345_exit_sleep();
return (events ^ EVT_GSENSOR);
}
///////////////////////////////////////////////////////////////////////
// RTC handle //
///////////////////////////////////////////////////////////////////////
if (events & EVT_RTC) {
// performed once per second
// record data
if ((pwmgr != PWMGR_S5) && (pwmgr != PWMGR_S6)) {
#if defined(HAL_IMAGE_A) || defined(HAL_IMAGE_B)
if ((osal_getClock() - mark.time) >= (12UL*60UL)) {
#else
if ((osal_getClock() - mark.time) >= (12UL)) {
#endif
if (!hash_is_full()) {
unsigned short tmp = normal.steps - mark.steps;
switch (opmode & 0xF0) {
case MODE_WORKOUT: tmp |= 0x8000; break;
case MODE_SLEEP: tmp |= 0x4000; break;
}
hash_put(&tmp);
}
mark.time = osal_getClock();
#if defined(HAL_IMAGE_A) || defined(HAL_IMAGE_B)
if ((mark.time % (24UL*60UL*60UL)) <= (13UL*60UL)) {
#else
if ((mark.time % (24UL*60UL*60UL)) <= (13UL)) {
#endif
dmsg(("reset steps...\n"));
normal.steps = 0;
workout.steps = 0;
STEPS = 0;
}
mark.steps = normal.steps;
}
}
// power management
switch (pwmgr) {
case PWMGR_S0:
pmsg(("\033[40;35mS0 (power on)\033[0m\n"));
if (pwmgr_saving_timer()) {
adxl345_enter_sleep();
osal_pwrmgr_device(PWRMGR_BATTERY);
pwmgr_state_change(PWMGR_S4, 0);
}
break;
case PWMGR_S1:
pmsg(("\033[40;35mS1 (rtc+gsen+ble+oled)\033[0m\n"));
if (pwmgr_saving_timer()) {
oled_enter_sleep();
osal_stop_timerEx(sensorTag_TaskID, EVT_MODE);
osal_stop_timerEx(sensorTag_TaskID, EVT_SLEEP);
osal_stop_timerEx(sensorTag_TaskID, EVT_SYSRST);
pwmgr_state_change(PWMGR_S3, TIME_GSEN_OFF);
}
break;
case PWMGR_S2:
pmsg(("\033[40;35mS2 (rtc+gsen+ble)\033[0m\n"));
if (gapProfileState == GAPROLE_WAITING) {
// enable key interrupt mode
InitBoard(OB_READY);
pwmgr_state_change(PWMGR_S3, TIME_GSEN_OFF);
}
break;
case PWMGR_S3:
pmsg(("\033[40;35mS3 (rtc+gsen)\033[0m\n"));
if (steps == normal.steps) {
if (pwmgr_saving_timer()) {
adxl345_enter_sleep();
pwmgr_state_change(PWMGR_S4, 0);
}
} else {
steps = normal.steps;
pwmgr_state_change(pwmgr, TIME_GSEN_OFF);
}
break;
case PWMGR_S4:
pmsg(("\033[40;35mS4 (rtc)\033[0m\n"));
dmsg(("$"));
break;
default:
case PWMGR_S5:
pmsg(("\033[40;35mS5 (shutdown)\033[0m\n"));
adxl345_shutdown();
osal_stop_timerEx(sensorTag_TaskID, EVT_RTC);
break;
case PWMGR_S6:
pmsg(("\033[40;35mS6 (rtc+oled)\033[0m\n"));
if (pwmgr_saving_timer()) {
oled_enter_sleep();
// enable key interrupt mode
InitBoard(OB_READY);
pwmgr_state_change(PWMGR_S5, 0);
}
break;
}
// battery measure
if ((!batt_measure()) && (pwmgr != PWMGR_S6)) {
pwmgr_state_change(PWMGR_S5, 0);
}
return (events ^ EVT_RTC);
}
///////////////////////////////////////////////////////////////////////
// battery charge detect handle //
///////////////////////////////////////////////////////////////////////
if (events & EVT_CHARGING) {
if (pwmgr != PWMGR_S1) {
if (!BATCD_SBIT) {
dmsg(("[charging]\n"));
oled_exit_sleep();
if ((pwmgr == PWMGR_S5) || (pwmgr == PWMGR_S6)) {
osal_start_reload_timer(sensorTag_TaskID, EVT_RTC, PERIOD_RTC);
pwmgr_state_change(PWMGR_S4, 0);
}
} else {
dmsg(("[no charge]\n"));
oled_enter_sleep();
}
}
return (events ^ EVT_CHARGING);
}
///////////////////////////////////////////////////////////////////////
// discard unknown events //
///////////////////////////////////////////////////////////////////////
return 0;
}
void ahrs_init(dataexchange_t *data, u8g_t *disp, params_t *calib_params) {
//vectors for absolute rotation matrix
vector3d acc_v, mag_v;
matrix3x3d initial_rmat;
quaternion initial_q;
calib_ptr = calib_params;
int16_t amount = 100;
//show init start message
_delay_ms(2000);
display_draw_line2x("Initialization,", "please stand still");
_delay_ms(2000);
acc_v.x = 0.0;
acc_v.y = 0.0;
acc_v.z = 0.0;
mag_v.x = 0.0;
mag_v.y = 0.0;
mag_v.z = 0.0;
for (int i = 0; i < amount; i++) {
adxl345_read(data); //accelerometer read
hmc5883l_read(data); //magnetometer read
acc_v.x += (*data).acc_x;
acc_v.y += (*data).acc_y;
acc_v.z += (*data).acc_z;
mag_v.x += (*data).mag_x;
mag_v.y += (*data).mag_y;
mag_v.z += (*data).mag_z;
// if ((i % 10) == 0) {
// char str[3];
// sprintf(str, "ready: %d%%", i);
// display_draw_line2x("Initialization:", str);
// }
}
display_draw_line2x("Initialization:", "...done!");
_delay_ms(2000);
display_draw_logo();
acc_v.x /= amount;
acc_v.y /= amount;
acc_v.z /= amount;
mag_v.x /= amount;
mag_v.y /= amount;
mag_v.z /= amount;
hmc5883l_applyCalibration(&mag_v, calib_ptr);
vector3d down = vector_inv(acc_v);
vector3d east = vector_cross(down, mag_v);
vector3d north = vector_cross(east, down);
//normalize vectors
vector_norm(&down);
vector_norm(&east);
vector_norm(&north);
//vectors to matrix
matrix_vector_to_row(&initial_rmat, north, 1);
matrix_vector_to_row(&initial_rmat, east, 2);
matrix_vector_to_row(&initial_rmat, down, 3);
//matrix to quaternion
initial_q = quaternion_from_matrix(&initial_rmat);
//normalize
quaternion_norm(&initial_q);
//initialize
(*data).qr.w = initial_q.w;
(*data).qr.x = initial_q.x;
(*data).qr.y = initial_q.y;
(*data).qr.z = initial_q.z;
}
