コード例 #1
0
ファイル: gyro.c プロジェクト: johncobb/spike_328p_imu
// 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;
	}
}
コード例 #2
0
ファイル: gyro.c プロジェクト: johncobb/spike_328p_imu
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;

}
コード例 #3
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;
        }
    }
}