예제 #1
0
/** @brief handler für die Nachricht /ardrone/navdata
 *
 * speichert die Navdata-Informationen in den Variablen von Cglobal
 */
void navdataUpdate(const ardrone_brown::Navdata::ConstPtr& navdata)
{
	Cglobal::instance().altd = navdata->altd;
	Cglobal::instance().vx = navdata->vx;
	Cglobal::instance().vy = navdata->vy;
	Cglobal::instance().vz = navdata->vz;
	float x = navdata->rotX;
	float y = navdata->rotY;
	Cglobal::instance().roty = grad2rad(y);  //grad in rad umrechnen
	Cglobal::instance().rotx = grad2rad(x);

	gettimeofday(&Cglobal::instance().sinceNoNavdataUpdate, NULL);
}
예제 #2
0
/**
 * Convert data read from gyro to rad/s
 * 
 * Gyro outputs ~1.65v for 0deg/sec, then this goes through a
 * 10bit ADC on the Atmega which compares 0-3.3v.
 * The data received is the result of the ADC.
 * 
 * @param imu 
 * @param data Contains RAW data from IMU
 * @param gyro_reading Rate in rad/sec
 * 
 * @return error code
 */
static int imu_comm_gyro_read(imu_t * imu, imu_data_t * data, double * gyro_reading){
    int retval = ERROR_OK, i;
    for(i = 0; i<IMU_GYROS; ++i){
	gyro_reading[i] = data->xyzrpy[IMU_ACCS + i] - imu->null_estimates.xyzrpy[IMU_ACCS + i];
	retval = gyro_scale_adjust(imu,gyro_reading+i);
	err_propagate(retval);
	gyro_reading[i] = grad2rad(gyro_reading[i]);
    }
    return retval;
}
예제 #3
0
void SVGAngle::convertToSpecifiedUnits(unsigned short unitType)
{
    if (m_unitType == unitType)
        return;

    if (m_unitType == SVG_ANGLETYPE_DEG && unitType == SVG_ANGLETYPE_RAD)
        m_valueInSpecifiedUnits = deg2rad(m_valueInSpecifiedUnits);
    else if (m_unitType == SVG_ANGLETYPE_GRAD && unitType == SVG_ANGLETYPE_RAD)
        m_valueInSpecifiedUnits = grad2rad(m_valueInSpecifiedUnits);
    else if (m_unitType == SVG_ANGLETYPE_DEG && unitType == SVG_ANGLETYPE_GRAD)
        m_valueInSpecifiedUnits = deg2grad(m_valueInSpecifiedUnits);
    else if (m_unitType == SVG_ANGLETYPE_RAD && unitType == SVG_ANGLETYPE_GRAD)
        m_valueInSpecifiedUnits = rad2grad(m_valueInSpecifiedUnits);
    else if (m_unitType == SVG_ANGLETYPE_RAD && unitType == SVG_ANGLETYPE_DEG)
        m_valueInSpecifiedUnits = rad2deg(m_valueInSpecifiedUnits);
    else if (m_unitType == SVG_ANGLETYPE_GRAD && unitType == SVG_ANGLETYPE_DEG)
        m_valueInSpecifiedUnits = grad2deg(m_valueInSpecifiedUnits);

    m_unitType = (SVGAngleType)unitType;
}
예제 #4
0
void SVGAngle::convertToSpecifiedUnits(unsigned short unitType, ExceptionCode& ec)
{
    if (unitType == SVG_ANGLETYPE_UNKNOWN || m_unitType == SVG_ANGLETYPE_UNKNOWN || unitType > SVG_ANGLETYPE_GRAD) {
        ec = NOT_SUPPORTED_ERR;
        return;
    }

    if (unitType == m_unitType)
        return;

    switch (m_unitType) {
    case SVG_ANGLETYPE_RAD:
        switch (unitType) {
        case SVG_ANGLETYPE_GRAD:
            m_valueInSpecifiedUnits = rad2grad(m_valueInSpecifiedUnits);
            break;
        case SVG_ANGLETYPE_UNSPECIFIED:
        case SVG_ANGLETYPE_DEG:
            m_valueInSpecifiedUnits = rad2deg(m_valueInSpecifiedUnits);
            break;
        case SVG_ANGLETYPE_RAD:
        case SVG_ANGLETYPE_UNKNOWN:
            ASSERT_NOT_REACHED();
            break;
        }
        break;
    case SVG_ANGLETYPE_GRAD:
        switch (unitType) {
        case SVG_ANGLETYPE_RAD:
            m_valueInSpecifiedUnits = grad2rad(m_valueInSpecifiedUnits);
            break;
        case SVG_ANGLETYPE_UNSPECIFIED:
        case SVG_ANGLETYPE_DEG:
            m_valueInSpecifiedUnits = grad2deg(m_valueInSpecifiedUnits);
            break;
        case SVG_ANGLETYPE_GRAD:
        case SVG_ANGLETYPE_UNKNOWN:
            ASSERT_NOT_REACHED();
            break;
        }
        break;
    case SVG_ANGLETYPE_UNSPECIFIED:
    // Spec: For angles, a unitless value is treated the same as if degrees were specified.
    case SVG_ANGLETYPE_DEG:
        switch (unitType) {
        case SVG_ANGLETYPE_RAD:
            m_valueInSpecifiedUnits = deg2rad(m_valueInSpecifiedUnits);
            break;
        case SVG_ANGLETYPE_GRAD:
            m_valueInSpecifiedUnits = deg2grad(m_valueInSpecifiedUnits);
            break;
        case SVG_ANGLETYPE_UNSPECIFIED:
            break;
        case SVG_ANGLETYPE_DEG:
        case SVG_ANGLETYPE_UNKNOWN:
            ASSERT_NOT_REACHED();
            break;
        }
        break;
    case SVG_ANGLETYPE_UNKNOWN:
        ASSERT_NOT_REACHED();
        break;
    }

    m_unitType = static_cast<SVGAngleType>(unitType);
}
예제 #5
0
파일: examinar.c 프로젝트: nancyocampoo/SGI
void TecladoAmpliado (int tecla, int x, int y)
{
	switch (tecla)
	{
		case GLUT_KEY_PAGE_UP :
			if (modo == FLY)
			{
				beta = beta + 1.50;
				if (beta > 360.0) beta = beta - 360.0;
				iy = oy + PASO * sin (grad2rad (beta));
			}
			break;
		case GLUT_KEY_UP : /* Pulsacion cursor arriba del teclado ampliado */
			if (modo == WALK || modo == FLY)
			{
				ox = ix;
				oz = iz;
				ix = ox + PASO * sin(grad2rad(alfa));
				iz = oz - PASO * cos(grad2rad(alfa));
				if (modo == FLY)
				{
					oy = iy;
					iy = iy + PASO * sin (grad2rad (beta));
				}
			}
			else if (modo == EXAMINAR)
			{
				beta = beta + 1.50;
				if (beta > 360.0) beta = beta - 360.0;
				iy = oy + PASO * sin (grad2rad (beta));
			}
			break;

		case GLUT_KEY_PAGE_DOWN :
			if (modo == FLY)
			{
				beta = beta - 1.50;
				if (beta > 360.0) beta = beta - 360.0;
				iy = oy + PASO * sin (grad2rad (beta));
			}
			break;
		case GLUT_KEY_DOWN : /* Pulsacion cursor abajo del teclado ampliado */
			if (modo == WALK || modo == FLY)
			{
				ox = ox - (ix-ox);
				oz = oz - (iz-oz);
				ix = ox + PASO * sin(grad2rad(alfa));
				iz = oz - PASO * cos(grad2rad(alfa));
				if (modo == FLY)
				{
					iy = oy;
					oy = oy - PASO * sin (grad2rad (beta));
				}
			}
			else if (modo == EXAMINAR)
			{
				beta = beta - 1.50;
				if (beta < 0.0) beta = beta + 360.0;
				iy = oy + PASO * sin (grad2rad (beta));
			}
			break;

		case GLUT_KEY_RIGHT : /* Pulsacion cursor derecha del teclado ampliado */
			alfa = alfa + 15.0;
			if (alfa > 360.0) alfa = alfa - 360.0;
			ix = ox + PASO * sin(grad2rad(alfa));
			iz = oz - PASO * cos(grad2rad(alfa));
			break;

		case GLUT_KEY_LEFT : /* Pulsacion cursor izquierda del teclado ampliado */
			alfa = alfa - 15.0;
			if (alfa < 0.0) alfa = alfa + 360.0;
			ix = ox + PASO * sin(grad2rad(alfa));
			iz = oz - PASO * cos(grad2rad(alfa));
			break;
	}
	glutPostRedisplay ();
}