// get 3-axis acceleration
void read_accs()
{
int16_t raw_val[3];
int16_t dev_val[3];
imu_get_acceleration(&raw_val[0], &raw_val[1], &raw_val[2]);
dev_val[sensor_def.acc[ROLL].idx] = raw_val[0] - config.acc_offset_x;
dev_val[sensor_def.acc[PITCH].idx] = raw_val[1] - config.acc_offset_y;
dev_val[sensor_def.acc[YAW].idx] = raw_val[2] - config.acc_offset_z;
for(int8_t axis=0; axis<3; axis++){
acc_adc[axis] = dev_val[axis] * sensor_def.acc[axis].dir;
}
}
uint8_t accl_calibration()
{
int16_t dev_val[3];
int16_t min_acc[3] = {INT_MAX, INT_MAX, INT_MAX};
int16_t max_acc[3] = {INT_MIN, INT_MIN, INT_MIN};
float acc_offset[3] = {0,};
delay_millis(500); // delay 0.5 seconds
for (uint8_t i=0; i<ACC_ITERATIONS; i++){
imu_get_acceleration(&dev_val[0], &dev_val[1], &dev_val[2]);
for (uint8_t j=0; j<3; j++){
acc_offset[j] += (float) dev_val[j]/ACC_ITERATIONS;
if(dev_val[j] > max_acc[j]){
max_acc[j] = dev_val[j];
}
if(dev_val[j] < min_acc[j]){
min_acc[j] = dev_val[j];
}
}
delay_millis(2); // 2ms
}
// Debugging
#ifdef GYRO_DEBUG
for(uint8_t j=0; j<3; j++) {
LOG("gyro: avg/max/min[");
LOG("%d", (uint8_t)j);
LOG(("] "));
LOG("%d", acc_offset[j], 3);
LOG((" / "));
LOG("%d", max_acc[j]);
LOG((" / "));
LOG("%d", min_acc[j]);
LOG("\r\n");
}
#endif
for (uint8_t i=0; i<3; i++){
if((max_acc[i] - min_acc[i]) > ACC_THRESH_FAIL){
return -1; // failed
}
}
// store calibration
if(abs(acc_offset[0]) < ACC_THRESH_GMIN){
config.acc_offset_x = acc_offset[0];
}
if(abs(acc_offset[1]) < ACC_THRESH_GMIN){
config.acc_offset_y = acc_offset[1];
}
if(abs(acc_offset[2]) < ACC_THRESH_GMIN){
config.acc_offset_z = acc_offset[2];
}
return 0;
}
void application_mainloop(application_t *app)
{
DEBUG("application_mainloop()");
assert(app != 0);
void *data;
size_t length;
float att[3], alt, spd, trk, brg, dst;
char wpt[32];
float accsum[3];
float difftime;
while(1)
{
// Process video
if(!video_read(app->video, &data, &length))
{
ERROR("Cannot read from video device");
break;
}
graphics_image_set_bitmap(app->image, data, length);
graphics_draw(app->graphics, app->image, app->window_width / 2, app->window_height / 2,
(float)app->window_width / (float)app->window_height < (float)app->video_width / (float)app->video_height ?
(float)app->window_height / (float)app->video_height : (float)app->window_width / (float)app->video_width, 0);
imu_get_attitude(app->imu, att);
gps_get_pos(app->gps, NULL, NULL, &alt);
imu_get_acceleration(app->imu, accsum, &difftime);
gps_inertial_update(app->gps, accsum[0], accsum[1], accsum[2], difftime);
// Draw landmarks
void *iterator;
float hangle, vangle, dist;
drawable_t *label = gps_get_projection_label(app->gps, &hangle, &vangle, &dist, att, &iterator);
while(iterator)
{
if((hangle > app->video_hfov / -2.0) &&
(hangle < app->video_hfov / 2.0) &&
(vangle > app->video_vfov / -2.0) &&
(vangle < app->video_vfov / 2.0) &&
(dist < app->visible_distance))
{
INFO("Projecting landmark hangle = %f, vangle = %f, distance = %f", hangle, vangle, dist / 1000.0);
uint32_t x = (float)app->window_width / 2 + (float)app->window_width * hangle / app->video_hfov;
uint32_t y = (float)app->window_height / 2 + (float)app->window_height * vangle / app->video_vfov;
graphics_draw(app->graphics, label, x, y, 1, 0);
}
label = gps_get_projection_label(app->gps, &hangle, &vangle, &dist, att, &iterator);
}
// Draw HUD overlay
gps_get_track(app->gps, &spd, &trk);
gps_get_route(app->gps, wpt, &dst, &brg);
graphics_hud_draw(app->hud, att, spd, alt, trk, brg, dst, wpt);
// Render to screen
if(!graphics_flush(app->graphics, NULL))
{
ERROR("Cannot draw");
break;
}
}
}
