/** * || ____ _ __ * +------+ / __ )(_) /_______________ _____ ___ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ * * Copyright (C) 2013 Bitcraze AB * * Crazyflie Nano Quadcopter Client * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * */ package se.bitcraze.crazyfliecontrol.controller; import se.bitcraze.crazyfliecontrol2.MainActivity; import android.content.Context; import android.hardware.Sensor; import android.hardware.SensorEvent; import android.hardware.SensorEventListener; import android.hardware.SensorManager; import android.util.Log; import com.MobileAnarchy.Android.Widgets.Joystick.JoystickView; /** * The GyroscopeController extends the TouchController and uses the gyroscope sensors * of the device to control the roll and pitch values. * The yaw and thrust values are still controlled by the touch controls according * to the chosen "mode" setting. * */ public class GyroscopeController extends TouchController { private SensorManager mSensorManager; private Sensor mSensor = null; private SensorEventListener mSeListener = null; private float mSensorRoll = 0; private float mSensorPitch = 0; public GyroscopeController(Controls controls, MainActivity activity, JoystickView joystickviewLeft, JoystickView joystickviewRight) { super(controls, activity, joystickviewLeft, joystickviewRight); mSensorManager = (SensorManager) activity.getSystemService(Context.SENSOR_SERVICE); if (mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR) != null) { mSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR); mSeListener = new RotationVectorListener(); } else if (mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER) != null){ mSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER); mSeListener = new AccelerometerListener(); } if (mSensor != null) { Log.i("GyroscopeController", "Gyro sensor type: " + mSensor.getName()); } } @Override public void enable() { super.enable(); if (mSensor != null && mSeListener != null) { mSensorManager.registerListener(mSeListener, mSensor, 100000); // mSensorManager.registerListener(mSeListener, mSensor, SensorManager.SENSOR_DELAY_UI); } } @Override public void disable() { mSensorRoll = 0; mSensorPitch = 0; if (mSeListener != null) { mSensorManager.unregisterListener(mSeListener); } super.disable(); } public String getControllerName() { return "gyroscope controller"; } class AccelerometerListener implements SensorEventListener { //It divide back the 90 degree. // private final float AMPLIFICATION = 1.5f; private final float AMPLIFICATION = mControls.getGyroAmplification(); @Override public void onAccuracyChanged(Sensor sensor, int arg1) { } @Override public void onSensorChanged(SensorEvent event) { float d = (float) Math.max(Math.sqrt(event.values[0] * event.values[0] + event.values[1] * event.values[1] + event.values[2] * event.values[2]),0.001); mSensorPitch = event.values[0] / d * -1f * AMPLIFICATION; mSensorRoll = event.values[1] / d * AMPLIFICATION; updateFlightData(); } } class RotationVectorListener implements SensorEventListener { @Override public void onAccuracyChanged(Sensor arg0, int arg1) { } @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) { if (event.values.length > 4) { float[] truncatedRotationVector = new float[4]; System.arraycopy(event.values, 0, truncatedRotationVector, 0, 4); update(truncatedRotationVector); } else { update(event.values); } } } } private static final int FROM_RADS_TO_DEGS = -57; private void update(float[] vectors) { int AMP_MAX = 50; int AMPLIFICATION = AMP_MAX / mControls.getGyroAmplification(); float[] rotationMatrix = new float[9]; SensorManager.getRotationMatrixFromVector(rotationMatrix, vectors); int worldAxisX = SensorManager.AXIS_X; int worldAxisY = SensorManager.AXIS_Y; float[] adjustedRotationMatrix = new float[9]; SensorManager.remapCoordinateSystem(rotationMatrix, worldAxisX, worldAxisY, adjustedRotationMatrix); float[] orientation = new float[3]; SensorManager.getOrientation(adjustedRotationMatrix, orientation); float pitch = (orientation[2] * FROM_RADS_TO_DEGS * -1) / AMPLIFICATION; float roll = (orientation[1] * FROM_RADS_TO_DEGS) / AMPLIFICATION; mSensorRoll = roll; mSensorPitch = pitch; updateFlightData(); } // overwrite getRoll() and getPitch() to only use values from gyro sensors public float getRoll() { float roll = mSensorRoll; //Filter the overshoot roll = (float) Math.min(1.0, Math.max(-1, roll+mControls.getRollTrim())); return (roll + mControls.getRollTrim()) * mControls.getRollPitchFactor() * mControls.getDeadzone(roll); } public float getPitch() { float pitch = mSensorPitch; //Filter the overshoot pitch = (float) Math.min(1.0, Math.max(-1, pitch+mControls.getPitchTrim())); return (pitch + mControls.getPitchTrim()) * mControls.getRollPitchFactor() * mControls.getDeadzone(pitch); } // map Yaw to the second touch pad public float getYaw() { float yaw = 0; yaw = (mControls.getMode() == 1 || mControls.getMode() == 2) ? mControls.getRightAnalog_X() : mControls.getLeftAnalog_X(); return yaw * mControls.getYawFactor() * mControls.getDeadzone(yaw); } }