double JoyControlStick::calculateYDistanceFromDeadZone()
{
double distance = 0.0;
int axis2Value = axisY->getCurrentRawValue();
double relativeAngle = calculateBearing();
if (relativeAngle > 180)
{
relativeAngle = relativeAngle - 180;
}
int deadY = abs(floor(deadZone * cos(relativeAngle * PI / 180.0)) + 0.5);
distance = (abs(axis2Value) - deadY)/(double)(maxZone - deadY);
if (distance > 1.0)
{
distance = 1.0;
}
else if (distance < 0.0)
{
distance = 0.0;
}
return distance;
}
void geo_send_heading()
{
can_msg_t geo_msg;
geo_spd_angle geo_data;
if(a == GEO_DATA_TO_SEND)
{
static imu& imu_handle = IMUInterface; // Handle to singleton IMU object
geo_data.bearing = calculateBearing(current_checkpoint_data); // put bearing here
geo_data.heading = imu_handle.getHeading();
#if 1
if(geo_data.heading < HEADING_OFFSET2 && geo_data.heading >= 0)
{
geo_data.heading = 360 - HEADING_OFFSET2 + (geo_data.heading);
}
else
{
geo_data.heading = geo_data.heading - HEADING_OFFSET2;
}
#endif
geo_data.speed = speed_gps;
geo_data.distance = (uint16_t)calculateDistance(current_checkpoint_data);
}
if(a == GEO_DO_NOT_SEND)
{
geo_data.bearing = 0; // put bearing here
geo_data.heading = 0;
geo_data.speed = 0;
geo_data.distance = 0;
}
geo_msg.msg_id = GEO_SPEED_ANGLE_ID;
geo_msg.frame_fields.is_29bit = 0;
geo_msg.frame_fields.data_len = sizeof(geo_data);
memcpy(&geo_msg.data.qword,&geo_data, sizeof(geo_data));
// CAN_Tx() will only time out when TX queue is full and that will only
// happen when CAN Bus turns off too long for us to empty the TX queue
CAN_tx(GEO_CNTL_CANBUS, &geo_msg, GEO_CNTL_CAN_TIMEOUT);
/*
if( !can_status )
{
LOG_ERROR("ERROR!!! Geo controller CAN message: IMU data not sent!!");
LE.on(GEO_CAN_ERR_LED);
}
else
LE.off(GEO_CAN_ERR_LED);
*/
}
void calculateTrackingVariables(float lon1, float lat1, float lon2, float lat2, int alt) {
// (homelon, homelat, uavlon, uavlat, uavalt )
// Return Bearing & Elevation angles in degree
// converting to radian
lon1 = toRad(lon1);
lat1 = toRad(lat1);
lon2 = toRad(lon2);
lat2 = toRad(lat2);
//calculating bearing in degree decimal
trackingBearing = calculateBearing(lon1, lat1, lon2, lat2);
//calculating distance between uav & home
trackingElevation = calculateElevation(lon1, lat1, lon2, lat2, alt);
}
double JoyControlStick::calculateDirectionalDistance(JoyControlStickButton *button, JoyButton::JoyMouseMovementMode mouseMode)
{
Q_UNUSED(mouseMode);
double finalDistance = 0.0;
if (currentDirection == StickUp)
{
finalDistance = calculateYDistanceFromDeadZone();
}
else if (currentDirection == StickRightUp)
{
if (activeButton1 && activeButton1 == button)
{
finalDistance = calculateXDistanceFromDeadZone();
}
else if (activeButton2 && activeButton2 == button)
{
finalDistance = calculateYDistanceFromDeadZone();
}
else if (activeButton3 && activeButton3 == button)
{
double radius = getDistanceFromDeadZone();
double bearing = calculateBearing();
int relativeBearing = (int)floor(bearing + 0.5) % 90;
//bearing = round(bearing) % 90;
int diagonalAngle = relativeBearing;
if (relativeBearing > 45)
{
diagonalAngle = 90 - relativeBearing;
}
finalDistance = radius * (diagonalAngle / 45.0);
}
}
else if (currentDirection == StickRight)
{
finalDistance = calculateXDistanceFromDeadZone();
}
else if (currentDirection == StickRightDown)
{
if (activeButton1 && activeButton1 == button)
{
finalDistance = calculateXDistanceFromDeadZone();
}
else if (activeButton2 && activeButton2 == button)
{
finalDistance = calculateYDistanceFromDeadZone();
}
else if (activeButton3 && activeButton3 == button)
{
double radius = getDistanceFromDeadZone();
double bearing = calculateBearing();
int relativeBearing = (int)floor(bearing + 0.5) % 90;
//bearing = round(bearing) % 90;
int diagonalAngle = relativeBearing;
if (relativeBearing > 45)
{
diagonalAngle = 90 - relativeBearing;
}
finalDistance = radius * (diagonalAngle / 45.0);
}
}
else if (currentDirection == StickDown)
{
finalDistance = calculateYDistanceFromDeadZone();
}
else if (currentDirection == StickLeftDown)
{
if (activeButton1 && activeButton1 == button)
{
finalDistance = calculateXDistanceFromDeadZone();
}
else if (activeButton2 && activeButton2 == button)
{
finalDistance = calculateYDistanceFromDeadZone();
}
else if (activeButton3 && activeButton3 == button)
{
double radius = getDistanceFromDeadZone();
double bearing = calculateBearing();
int relativeBearing = (int)floor(bearing + 0.5) % 90;
//bearing = round(bearing) % 90;
int diagonalAngle = relativeBearing;
if (relativeBearing > 45)
{
diagonalAngle = 90 - relativeBearing;
}
finalDistance = radius * (diagonalAngle / 45.0);
}
}
else if (currentDirection == StickLeft)
{
finalDistance = calculateXDistanceFromDeadZone();
}
else if (currentDirection == StickLeftUp)
{
if (activeButton1 && activeButton1 == button)
{
finalDistance = calculateXDistanceFromDeadZone();
}
else if (activeButton2 && activeButton2 == button)
{
finalDistance = calculateYDistanceFromDeadZone();
}
else if (activeButton3 && activeButton3 == button)
{
double radius = getDistanceFromDeadZone();
double bearing = calculateBearing();
int relativeBearing = (int)floor(bearing + 0.5) % 90;
//bearing = round(bearing) % 90;
int diagonalAngle = relativeBearing;
if (relativeBearing > 45)
{
diagonalAngle = 90 - relativeBearing;
}
finalDistance = radius * (diagonalAngle / 45.0);
}
}
return finalDistance;
}
void JoyControlStick::createDeskEvent(bool ignoresets)
{
JoyControlStickButton *eventbutton1 = 0;
JoyControlStickButton *eventbutton2 = 0;
JoyControlStickButton *eventbutton3 = 0;
if (safezone)
{
double bearing = calculateBearing();
QList<int> anglesList = getDiagonalZoneAngles();
int initialLeft = anglesList.value(0);
int initialRight = anglesList.value(1);
int upRightInitial = anglesList.value(2);
int rightInitial = anglesList.value(3);
int downRightInitial = anglesList.value(4);
int downInitial = anglesList.value(5);
int downLeftInitial = anglesList.value(6);
int leftInitial = anglesList.value(7);
int upLeftInitial = anglesList.value(8);
bearing = floor(bearing + 0.5);
if (bearing <= initialRight || bearing >= initialLeft)
{
currentDirection = StickUp;
eventbutton2 = buttons.value(StickUp);
}
else if (bearing >= upRightInitial && bearing < rightInitial)
{
currentDirection = StickRightUp;
if (currentMode == EightWayMode && buttons.contains(StickRightUp))
{
eventbutton3 = buttons.value(StickRightUp);
}
else
{
eventbutton1 = buttons.value(StickRight);
eventbutton2 = buttons.value(StickUp);
}
}
else if (bearing >= rightInitial && bearing < downRightInitial)
{
currentDirection = StickRight;
eventbutton1 = buttons.value(StickRight);
}
else if (bearing >= downRightInitial && bearing < downInitial)
{
currentDirection = StickRightDown;
if (currentMode == EightWayMode && buttons.contains(StickRightDown))
{
eventbutton3 = buttons.value(StickRightDown);
}
else
{
eventbutton1 = buttons.value(StickRight);
eventbutton2 = buttons.value(StickDown);
}
}
else if (bearing >= downInitial && bearing < downLeftInitial)
{
currentDirection = StickDown;
eventbutton2 = buttons.value(StickDown);
}
else if (bearing >= downLeftInitial && bearing < leftInitial)
{
currentDirection = StickLeftDown;
if (currentMode == EightWayMode && buttons.contains(StickLeftDown))
{
eventbutton3 = buttons.value(StickLeftDown);
}
else
{
eventbutton1 = buttons.value(StickLeft);
eventbutton2 = buttons.value(StickDown);
}
}
else if (bearing >= leftInitial && bearing < upLeftInitial)
{
currentDirection = StickLeft;
eventbutton1 = buttons.value(StickLeft);
}
else if (bearing >= upLeftInitial && bearing < initialLeft)
{
currentDirection = StickLeftUp;
if (currentMode == EightWayMode && buttons.contains(StickLeftUp))
{
eventbutton3 = buttons.value(StickLeftUp);
}
else
{
eventbutton1 = buttons.value(StickLeft);
eventbutton2 = buttons.value(StickUp);
}
}
}
// Release any currently active stick buttons
if (!eventbutton1 && activeButton1)
{
// Currently in deadzone. Disable currently active button.
performButtonRelease(activeButton1, ignoresets);
}
else if (eventbutton1 && activeButton1 && eventbutton1 != activeButton1)
{
// Deadzone skipped. Button for new event is not the currently
// active button. Disable the active button.
performButtonRelease(activeButton1, ignoresets);
}
if (!eventbutton2 && activeButton2)
{
// Currently in deadzone. Disable currently active button.
performButtonRelease(activeButton2, ignoresets);
}
else if (eventbutton2 && activeButton2 && eventbutton2 != activeButton2)
{
// Deadzone skipped. Button for new event is not the currently
// active button. Disable the active button.
performButtonRelease(activeButton2, ignoresets);
}
if (!eventbutton3 && activeButton3)
{
// Currently in deadzone. Disable currently active button.
performButtonRelease(activeButton3, ignoresets);
}
else if (eventbutton3 && activeButton3 && eventbutton3 != activeButton3)
{
// Deadzone skipped. Button for new event is not the currently
// active button. Disable the active button.
performButtonRelease(activeButton3, ignoresets);
}
// Enable stick buttons
if (eventbutton1 && !activeButton1)
{
// There is no active button. Call joyEvent and set current
// button as active button
performButtonPress(eventbutton1, activeButton1, ignoresets);
}
else if (eventbutton1 && activeButton1 && eventbutton1 == activeButton1)
{
// Button is currently active. Just pass current value
performButtonPress(eventbutton1, activeButton1, ignoresets);
}
if (eventbutton2 && !activeButton2)
{
// There is no active button. Call joyEvent and set current
// button as active button
performButtonPress(eventbutton2, activeButton2, ignoresets);
}
else if (eventbutton2 && activeButton2 && eventbutton2 == activeButton2)
{
// Button is currently active. Just pass current value
performButtonPress(eventbutton2, activeButton2, ignoresets);
}
if (eventbutton3 && !activeButton3)
{
// There is no active button. Call joyEvent and set current
// button as active button
performButtonPress(eventbutton3, activeButton3, ignoresets);
}
else if (eventbutton3 && activeButton3 && eventbutton3 == activeButton3)
{
// Button is currently active. Just pass current value
performButtonPress(eventbutton3, activeButton3, ignoresets);
}
/*if (eventbutton2 || activeButton2)
{
changeButtonEvent(eventbutton2, activeButton2, ignoresets);
}
if (eventbutton1 || activeButton1)
{
changeButtonEvent(eventbutton1, activeButton1, ignoresets);
}
if (eventbutton3 || activeButton3)
{
changeButtonEvent(eventbutton3, activeButton3, ignoresets);
}*/
}
