/** @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);
}
/**
* 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;
}
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;
}
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);
}
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 ();
}