diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 8c5f55858..cc32cf442 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -75,5 +75,27 @@ void quat_inverse(float q[4]); void quat_copy(const float q[4], float qnew[4]); void quat_mult(const float q1[4], const float q2[4], float qout[4]); void rot_mult(float R[3][3], const float vec[3], float vec_out[3]); +/** + * matrix_mult_3x3f - perform a multiplication between two 3x3 float matrices + * result = a*b + * @param a + * @param b + * @param result + */ +inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3]) +{ + result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2]; + result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2]; + result[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2]; + + result[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2]; + result[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2]; + result[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2]; + + result[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2]; + result[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2]; + result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2]; +} + #endif // COORDINATECONVERSIONS_H_ diff --git a/flight/libraries/inc/plans.h b/flight/libraries/inc/plans.h new file mode 100644 index 000000000..1d8b6b344 --- /dev/null +++ b/flight/libraries/inc/plans.h @@ -0,0 +1,69 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotLibraries OpenPilot Libraries + * @{ + * @addtogroup Navigation + * @brief setups RTH/PH and other pathfollower/pathplanner status + * @{ + * + * @file plan.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * 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 3 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., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PLANS_H_ +#define PLANS_H_ +#include + +/** \page standard Plans + How to use this library + \li Call plan_initialize() to init all needed struct and uavos at startup.
+ It may be safely called more than once.
+ + \li Functions called plan_setup_* needs to be called once, every time the requested function is engaged.
+ + \li Functions called plan_run_* are to be periodically called while the requested mode is engaged.
+ */ + +/** + * @brief initialize UAVOs and structs used by this library + */ +void plan_initialize(); + +/** + * @brief setup pathplanner/pathfollower for positionhold + */ +void plan_setup_positionHold(); + +/** + * @brief setup pathplanner/pathfollower for return to base + */ +void plan_setup_returnToBase(); + +/** + * @brief setup pathplanner/pathfollower for land + */ +void plan_setup_land(); + +/** + * @brief execute land + */ +void plan_run_land(); +#endif /* PLANS_H_ */ diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c new file mode 100644 index 000000000..c2225564f --- /dev/null +++ b/flight/libraries/plans.c @@ -0,0 +1,130 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotLibraries OpenPilot Libraries + * @{ + * @addtogroup Navigation + * @brief setups RTH/PH and other pathfollower/pathplanner status + * @{ + * + * @file plan.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * 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 3 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., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief initialize UAVOs and structs used by this library + */ +void plan_initialize() +{ + TakeOffLocationInitialize(); + PositionStateInitialize(); + PathDesiredInitialize(); + FlightModeSettingsInitialize(); +} + +/** + * @brief setup pathplanner/pathfollower for positionhold + */ +void plan_setup_positionHold() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + + PathDesiredSet(&pathDesired); +} + +/** + * @brief setup pathplanner/pathfollower for return to base + */ +void plan_setup_returnToBase() +{ + // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position + float positionStateDown; + + PositionStateDownGet(&positionStateDown); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + + TakeOffLocationData takeoffLocation; + TakeOffLocationGet(&takeoffLocation); + + // TODO: right now VTOLPF does fly straight to destination altitude. + // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin) + + float destDown; + FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown); + destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; + + pathDesired.Start.North = takeoffLocation.North; + pathDesired.Start.East = takeoffLocation.East; + pathDesired.Start.Down = destDown; + + pathDesired.End.North = takeoffLocation.North; + pathDesired.End.East = takeoffLocation.East; + pathDesired.End.Down = destDown; + + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + + PathDesiredSet(&pathDesired); +} + +void plan_setup_land() +{ + plan_setup_positionHold(); +} + +/** + * @brief execute land + */ +void plan_run_land() +{ + PathDesiredEndData pathDesiredEnd; + + PathDesiredEndGet(&pathDesiredEnd); + + PositionStateDownGet(&pathDesiredEnd.Down); + pathDesiredEnd.Down += 5; + + PathDesiredEndSet(&pathDesiredEnd); +} diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index d381f27ff..417665b6c 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -63,6 +63,7 @@ #include "taskinfo.h" #include +#include "sin_lookup.h" #include "paths.h" #include "CoordinateConversions.h" @@ -85,6 +86,7 @@ static void updatePathVelocity(); static uint8_t updateFixedDesiredAttitude(); static void updateFixedAttitude(); static void airspeedStateUpdatedCb(UAVObjEvent *ev); +static bool correctCourse(float *C, float *V, float *F, float s); /** * Initialise the module, called on startup @@ -125,17 +127,17 @@ int32_t FixedWingPathFollowerInitialize() } MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart); -static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; +static float northVelIntegral = 0.0f; +static float eastVelIntegral = 0.0f; +static float downVelIntegral = 0.0f; -static float courseIntegral = 0; -static float speedIntegral = 0; -static float powerIntegral = 0; -static float airspeedErrorInt = 0; +static float courseIntegral = 0.0f; +static float speedIntegral = 0.0f; +static float powerIntegral = 0.0f; +static float airspeedErrorInt = 0.0f; // correct speed by measured airspeed -static float indicatedAirspeedStateBias = 0; +static float indicatedAirspeedStateBias = 0.0f; /** * Module thread, should not return. @@ -225,12 +227,12 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) } } else { // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - courseIntegral = 0; - speedIntegral = 0; - powerIntegral = 0; + northVelIntegral = 0.0f; + eastVelIntegral = 0.0f; + downVelIntegral = 0.0f; + courseIntegral = 0.0f; + speedIntegral = 0.0f; + powerIntegral = 0.0f; } PathStatusSet(&pathStatus); } @@ -276,14 +278,14 @@ static void updatePathVelocity() case PATHDESIRED_MODE_DRIVEVECTOR: default: groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - boundf(progress.fractional_progress, 0, 1); + boundf(progress.fractional_progress, 0.0f, 1.0f); altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * - boundf(progress.fractional_progress, 0, 1); + boundf(progress.fractional_progress, 0.0f, 1.0f); break; } // make sure groundspeed is not zero - if (groundspeed < 1e-2f) { - groundspeed = 1e-2f; + if (groundspeed < 1e-6f) { + groundspeed = 1e-6f; } // calculate velocity - can be zero if waypoints are too close @@ -302,24 +304,12 @@ static void updatePathVelocity() // in this case the plane effectively needs to be turned around // indicators: // difference between correction_direction and velocitystate >90 degrees and - // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from eerything ) + // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North)); - float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North)); - if (angle1 < -180.0f) { - angle1 += 360.0f; - } - if (angle1 > 180.0f) { - angle1 -= 360.0f; - } - if (angle2 < -180.0f) { - angle2 += 360.0f; - } - if (angle2 > 180.0f) { - angle2 -= 360.0f; - } - if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) { - error_speed = 0; + if ( // calculating angles < 90 degrees through dot products + ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) && + ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) { + error_speed = 0.0f; } // calculate correction - can also be zero if correction vector is 0 or no error present @@ -328,14 +318,16 @@ static void updatePathVelocity() // scale to correct length float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - velocityDesired.North *= groundspeed / l; - velocityDesired.East *= groundspeed / l; + if (l > 1e-9f) { + velocityDesired.North *= groundspeed / l; + velocityDesired.East *= groundspeed / l; + } float downError = altitudeSetpoint - positionState.Down; - velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; + velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; VelocityDesiredSet(&velocityDesired); @@ -384,8 +376,7 @@ static uint8_t updateFixedDesiredAttitude() AirspeedStateData airspeedState; SystemSettingsData systemSettings; - float groundspeedState; - float groundspeedDesired; + float groundspeedProjection; float indicatedAirspeedState; float indicatedAirspeedDesired; float airspeedError; @@ -396,10 +387,9 @@ static uint8_t updateFixedDesiredAttitude() float descentspeedError; float powerCommand; - float bearing; - float heading; - float headingError; - float course; + float airspeedVector[2]; + float fluidMovement[2]; + float courseComponent[2]; float courseError; float courseCommand; @@ -413,24 +403,54 @@ static uint8_t updateFixedDesiredAttitude() AirspeedStateGet(&airspeedState); SystemSettingsGet(&systemSettings); - /** - * Compute speed error (required for thrust and pitch) + * Compute speed error and course */ + // missing sensors for airspeed-direction we have to assume within + // reasonable error that measured airspeed is actually the airspeed + // component in forward pointing direction + // airspeedVector is normalized + airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); + airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); - // Current ground speed - groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North); - // note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, - // but thanks to accelerometers, groundspeed reacts faster to changes in direction + // current ground speed projected in forward direction + groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, + // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction // than airspeed and gps sensors alone - indicatedAirspeedState = groundspeedState + indicatedAirspeedStateBias; + indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; - // Desired ground speed - groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias, + // fluidMovement is a vector describing the aproximate movement vector of + // the surrounding fluid in 2d space (aka wind vector) + fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); + fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); + + // calculate the movement vector we need to fly to reach velocityDesired - + // taking fluidMovement into account + courseComponent[0] = velocityDesired.North - fluidMovement[0]; + courseComponent[1] = velocityDesired.East - fluidMovement[1]; + + indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), fixedwingpathfollowerSettings.HorizontalVelMin, fixedwingpathfollowerSettings.HorizontalVelMax); + // if we could fly at arbitrary speeds, we'd just have to move towards the + // courseComponent vector as previously calculated and we'd be fine + // unfortunately however we are bound by min and max air speed limits, so + // we need to recalculate the correct course to meet at least the + // velocityDesired vector direction at our current speed + // this overwrites courseComponent + bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); + + // Error condition: wind speed too high, we can't go where we want anymore + fixedwingpathfollowerStatus.Errors.Wind = 0; + if ((!valid) && + fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.Wind = 1; + result = 0; + } + // Airspeed error airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; @@ -461,23 +481,18 @@ static uint8_t updateFixedDesiredAttitude() result = 0; } - if (indicatedAirspeedState < 1e-6f) { - // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes - // also we cannot handle planes flying backwards, lets just wait until the nose drops - fixedwingpathfollowerStatus.Errors.Lowspeed = 1; - return 0; - } - /** * Compute desired thrust command */ // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) { + if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) { powerIntegral = boundf(powerIntegral + -descentspeedError * dT, -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki, fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); - } else { powerIntegral = 0; } + } else { + powerIntegral = 0.0f; + } // Compute the cross feed from vertical speed to pitch, with saturation float speedErrorToPowerCommandComponent = boundf( @@ -504,9 +519,9 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: plane cannot hold altitude at current speed. fixedwingpathfollowerStatus.Errors.Lowpower = 0; if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum - velocityState.Down > 0 && // we ARE going down - descentspeedDesired < 0 && // we WANT to go up - airspeedError > 0 && // we are too slow already + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError > 0.0f && // we are too slow already fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on fixedwingpathfollowerStatus.Errors.Lowpower = 1; result = 0; @@ -514,9 +529,9 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: plane keeps climbing despite minimum thrust (opposite of above) fixedwingpathfollowerStatus.Errors.Highpower = 0; if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum - velocityState.Down < 0 && // we ARE going up - descentspeedDesired > 0 && // we WANT to go down - airspeedError < 0 && // we are too fast already + velocityState.Down < 0.0f && // we ARE going up + descentspeedDesired > 0.0f && // we WANT to go down + airspeedError < 0.0f && // we are too fast already fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on fixedwingpathfollowerStatus.Errors.Highpower = 1; result = 0; @@ -526,7 +541,6 @@ static uint8_t updateFixedDesiredAttitude() /** * Compute desired pitch command */ - if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) { // Integrate with saturation airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT, @@ -556,48 +570,18 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: high speed dive fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0; if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up - velocityState.Down > 0 && // we ARE going down - descentspeedDesired < 0 && // we WANT to go up - airspeedError < 0 && // we are too fast already + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError < 0.0f && // we are too fast already fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1; result = 0; } - /** - * Calculate where we are heading and why (wind issues) - */ - bearing = attitudeState.Yaw; - heading = RAD2DEG(atan2f(velocityState.East, velocityState.North)); - headingError = heading - bearing; - if (headingError < -180.0f) { - headingError += 360.0f; - } - if (headingError > 180.0f) { - headingError -= 360.0f; - } - // Error condition: wind speed is higher than airspeed. We are forced backwards! - fixedwingpathfollowerStatus.Errors.Wind = 0; - if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind || - headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) && - fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on - // we are flying backwards - fixedwingpathfollowerStatus.Errors.Wind = 1; - result = 0; - } - /** * Compute desired roll command */ - if (groundspeedDesired > 1e-6f) { - course = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North)); - - courseError = course - heading; - } else { - // if we are not supposed to move, run in a circle - courseError = -90.0f; - result = 0; - } + courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; if (courseError < -180.0f) { courseError += 360.0f; @@ -606,6 +590,19 @@ static uint8_t updateFixedDesiredAttitude() courseError -= 360.0f; } + // overlap calculation. Theres a dead zone behind the craft where the + // counter-yawing of some craft while rolling could render a desired right + // turn into a desired left turn. Making the turn direction based on + // current roll angle keeps the plane committed to a direction once chosen + if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f) + && attitudeState.Roll > 0.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f) + && attitudeState.Roll < 0.0f) { + courseError -= 360.0f; + } + courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki, -fixedwingpathfollowerSettings.CoursePI.ILimit, fixedwingpathfollowerSettings.CoursePI.ILimit); @@ -628,7 +625,7 @@ static uint8_t updateFixedDesiredAttitude() * Compute desired yaw command */ // TODO implement raw control mode for yaw and base on Accels.Y - stabDesired.Yaw = 0; + stabDesired.Yaw = 0.0f; stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; @@ -656,10 +653,116 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) AirspeedStateGet(&airspeedState); VelocityStateGet(&velocityState); - float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North); + float airspeedVector[2]; + float yaw; + AttitudeStateYawGet(&yaw); + airspeedVector[0] = cos_lookup_deg(yaw); + airspeedVector[1] = sin_lookup_deg(yaw); + // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement + float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; - - indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed; - // note - we do fly by Indicated Airspeed (== calibrated airspeed) - // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. + indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) however + // since airspeed is updated less often than groundspeed, we use sudden + // changes to groundspeed to offset the airspeed by the same measurement. + // This has a side effect that in the absence of any airspeed updates, the + // pathfollower will fly using groundspeed. +} + + +/** + * Function to calculate course vector C based on airspeed s, fluid movement F + * and desired movement vector V + * parameters in: V,F,s + * parameters out: C + * returns true if a valid solution could be found for V,F,s, false if not + * C will be set to a best effort attempt either way + */ +static bool correctCourse(float *C, float *V, float *F, float s) +{ + // Approach: + // Let Sc be a circle around origin marking possible movement vectors + // of the craft with airspeed s (all possible options for C) + // Let Vl be a line through the origin along movement vector V where fr any + // point v on line Vl v = k * (V / |V|) = k' * V + // Let Wl be a line parallel to Vl where for any point v on line Vl exists + // a point w on WL with w = v - F + // Then any intersection between circle Sc and line Wl represents course + // vector which would result in a movement vector + // V' = k * ( V / |V|) = k' * V + // If there is no intersection point, S is insufficient to compensate + // for F and we can only try to fly in direction of V (thus having wind drift + // but at least making progress orthogonal to wind) + + s = fabsf(s); + float f = sqrtf(F[0] * F[0] + F[1] * F[1]); + + // normalize Cn=V/|V|, |V| must be >0 + float v = sqrtf(V[0] * V[0] + V[1] * V[1]); + if (v < 1e-6f) { + // if |V|=0, we aren't supposed to move, turn into the wind + // (this allows hovering) + C[0] = -F[0]; + C[1] = -F[1]; + // if desired airspeed matches fluidmovement a hover is actually + // intended so return true + return fabsf(f - s) < 1e-3f; + } + float Vn[2] = { V[0] / v, V[1] / v }; + + // project F on V + float fp = F[0] * Vn[0] + F[1] * Vn[1]; + + // find component Fo of F that is orthogonal to V + // (which is exactly the distance between Vl and Wl) + float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) }; + float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1]; + + // find k where k * Vn = C - Fo + // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo + // so k^2 + fo^2 = s^2 (since |Vn|=1) + float k2 = s * s - fo2; + if (k2 <= -1e-3f) { + // there is no solution, we will be drifted off either way + // fallback: fly stupidly in direction of V and hope for the best + C[0] = V[0]; + C[1] = V[1]; + return false; + } else if (k2 <= 1e-3f) { + // there is exactly one solution: -Fo + C[0] = -Fo[0]; + C[1] = -Fo[1]; + return true; + } + // we have two possible solutions k positive and k negative as there are + // two intersection points between Wl and Sc + // which one is better? two criteria: + // 1. we MUST move in the right direction, if any k leads to -v its invalid + // 2. we should minimize the speed error + float k = sqrt(k2); + float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] }; + float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] }; + // project C+F on Vn to find signed resulting movement vector length + float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1]; + float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1]; + if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) { + // in this case the angle between course and resulting movement vector + // is greater than 90 degrees - so we actually fly backwards + C[0] = C1[0]; + C[1] = C1[1]; + return true; + } + C[0] = C2[0]; + C[1] = C2[1]; + if (vp2 >= 0.0f) { + // in this case the angle between course and movement vector is less than + // 90 degrees, but we do move in the right direction + return true; + } else { + // in this case we actually get driven in the opposite direction of V + // with both solutions for C + // this might be reached in headwind stronger than maximum allowed + // airspeed. + return false; + } } diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 0d5c00d72..1b431d521 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -75,6 +75,18 @@ void pathFollowerHandler(bool newinit); */ void pathPlannerHandler(bool newinit); +/** + * @brief Handler to setup takeofflocation on arming. it is set up during Arming + * @input: NONE: + * @output: NONE + */ +void takeOffLocationHandler(); + +/** + * @brief Initialize TakeoffLocationHanlder + */ +void takeOffLocationHandlerInit(); + /* * These are assumptions we make in the flight code about the order of settings and their consistency between * objects. Please keep this synchronized to the UAVObjects diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 1bd29329f..4c29949d5 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -134,6 +134,9 @@ int32_t ManualControlStart() // Make sure unarmed on power up armHandler(true); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + takeOffLocationHandlerInit(); +#endif // Start main task PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); @@ -167,7 +170,9 @@ static void manualControlTask(void) { // Process Arming armHandler(false); - +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + takeOffLocationHandler(); +#endif // Process flight mode FlightStatusData flightStatus; diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c index 0f9bfe902..2fbfafaea 100644 --- a/flight/modules/ManualControl/pathfollowerhandler.c +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -35,8 +35,9 @@ #include #include - #if defined(REVOLUTION) +#include + // Private constants // Private types @@ -51,47 +52,30 @@ void pathFollowerHandler(bool newinit) { if (newinit) { - PathDesiredInitialize(); - PositionStateInitialize(); + plan_initialize(); } - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + uint8_t flightMode; + FlightStatusFlightModeGet(&flightMode); if (newinit) { // After not being in this mode for a while init at current height - PositionStateData positionState; - PositionStateGet(&positionState); - FlightModeSettingsData settings; - FlightModeSettingsGet(&settings); - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - switch (flightStatus.FlightMode) { + switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - // Simple Return To Base mode - keep altitude the same, fly to home position - - - pathDesired.Start.North = 0; - pathDesired.Start.East = 0; - pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End.North = 0; - pathDesired.End.East = 0; - pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + plan_setup_returnToBase(); break; - default: - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; - pathDesired.End.North = positionState.North; - pathDesired.End.East = positionState.East; - pathDesired.End.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + plan_setup_positionHold(); + break; + + case FLIGHTSTATUS_FLIGHTMODE_LAND: + plan_setup_land(); + break; + + default: + plan_setup_positionHold(); + /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. } else { PathDesiredData pathDesired; @@ -103,17 +87,15 @@ void pathFollowerHandler(bool newinit) */ break; } - PathDesiredSet(&pathDesired); } + switch (flightMode) { // special handling of autoland - behaves like positon hold but with slow altitude decrease - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { - PositionStateData positionState; - PositionStateGet(&positionState); - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End.Down = positionState.Down + 5; - PathDesiredSet(&pathDesired); + case FLIGHTSTATUS_FLIGHTMODE_LAND: + plan_run_land(); + break; + default: + break; } } diff --git a/flight/modules/ManualControl/takeofflocationhandler.c b/flight/modules/ManualControl/takeofflocationhandler.c new file mode 100644 index 000000000..42a10777d --- /dev/null +++ b/flight/modules/ManualControl/takeofflocationhandler.c @@ -0,0 +1,125 @@ +/** + ****************************************************************************** + * + * @file takeofflocationhandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief handles TakeOffLocation + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 3 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., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include "inc/manualcontrol.h" +#include +#include +#include +#include + +// Private constants + +// Private types + +// Private variables +static bool locationSet; +// Private functions +static void SetTakeOffLocation(); + +void takeOffLocationHandlerInit() +{ + TakeOffLocationInitialize(); + // check whether there is a preset/valid takeoff location + uint8_t mode; + uint8_t status; + TakeOffLocationModeGet(&mode); + TakeOffLocationStatusGet(&status); + // preset with invalid location will actually behave like FirstTakeoff + if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) { + locationSet = true; + } else { + locationSet = false; + status = TAKEOFFLOCATION_STATUS_INVALID; + TakeOffLocationStatusSet(&status); + } +} +/** + * Handles TakeOffPosition location setup + * @param newinit + */ +void takeOffLocationHandler() +{ + uint8_t armed; + uint8_t status; + + FlightStatusArmedGet(&armed); + + // Location already acquired/preset + if (armed == FLIGHTSTATUS_ARMED_ARMED && locationSet) { + return; + } + + TakeOffLocationStatusGet(&status); + + switch (armed) { + case FLIGHTSTATUS_ARMED_ARMING: + case FLIGHTSTATUS_ARMED_ARMED: + if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) { + uint8_t mode; + TakeOffLocationModeGet(&mode); + + if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) { + SetTakeOffLocation(); + } else { + locationSet = true; + } + } + break; + + case FLIGHTSTATUS_ARMED_DISARMED: + // unset if location is to be acquired at each arming + if (locationSet) { + uint8_t mode; + TakeOffLocationModeGet(&mode); + if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) { + locationSet = false; + status = TAKEOFFLOCATION_STATUS_INVALID; + TakeOffLocationStatusSet(&status); + } + } + break; + } +} + +/** + * Retrieve TakeOffLocation from current PositionStatus + */ +void SetTakeOffLocation() +{ + TakeOffLocationData takeOffLocation; + + TakeOffLocationGet(&takeOffLocation); + + PositionStateData positionState; + PositionStateGet(&positionState); + + takeOffLocation.North = positionState.North; + takeOffLocation.East = positionState.East; + takeOffLocation.Down = positionState.Down; + takeOffLocation.Status = TAKEOFFLOCATION_STATUS_VALID; + + TakeOffLocationSet(&takeOffLocation); + locationSet = true; +} diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 8cc50c4aa..c25d3a0b7 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -45,6 +45,7 @@ #include "flightmodesettings.h" #include #include "paths.h" +#include "plans.h" // Private constants #define STACK_SIZE_BYTES 1024 @@ -89,6 +90,7 @@ static bool pathplanner_active = false; */ int32_t PathPlannerStart() { + plan_initialize(); // when the active waypoint changes, update pathDesired WaypointConnectCallback(commandUpdated); WaypointActiveConnectCallback(commandUpdated); @@ -169,21 +171,7 @@ static void pathPlannerTask() if (!failsafeRTHset) { failsafeRTHset = 1; // copy pasta: same calculation as in manualcontrol, set return to home coordinates - PositionStateData positionState; - PositionStateGet(&positionState); - FlightModeSettingsData settings; - FlightModeSettingsGet(&settings); - - pathDesired.Start.North = 0; - pathDesired.Start.East = 0; - pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End.North = 0; - pathDesired.End.East = 0; - pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); + plan_setup_positionHold(); } AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR); diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index c1c76ee8f..bd35945fb 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -62,6 +62,7 @@ #include #include +#include // Private constants #define STACK_SIZE_BYTES 1000 @@ -474,35 +475,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); - mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 + - R[1][0] * cal.mag_transform.r0c1 + - R[2][0] * cal.mag_transform.r0c2; - mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 + - R[1][1] * cal.mag_transform.r0c1 + - R[2][1] * cal.mag_transform.r0c2; - mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 + - R[1][2] * cal.mag_transform.r0c1 + - R[2][2] * cal.mag_transform.r0c2; - - mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 + - R[1][0] * cal.mag_transform.r1c1 + - R[2][0] * cal.mag_transform.r1c2; - mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 + - R[1][1] * cal.mag_transform.r1c1 + - R[2][1] * cal.mag_transform.r1c2; - mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 + - R[1][2] * cal.mag_transform.r1c1 + - R[2][2] * cal.mag_transform.r1c2; - - mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 + - R[1][0] * cal.mag_transform.r2c1 + - R[2][0] * cal.mag_transform.r2c2; - mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 + - R[1][1] * cal.mag_transform.r2c1 + - R[2][1] * cal.mag_transform.r2c2; - mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 + - R[1][2] * cal.mag_transform.r2c1 + - R[2][2] * cal.mag_transform.r2c2; + matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform); } /** * @} diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 8b14a385e..7472d176a 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -82,6 +82,7 @@ ifndef TESTAPP ## Misc library functions SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 44c43d080..3905af42f 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 88b8dd6b6..8c3337a1a 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -84,6 +84,7 @@ ifndef TESTAPP ## Misc library functions SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 44c43d080..3905af42f 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index affe5a570..39d2d7f3b 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -83,6 +83,7 @@ ifndef TESTAPP ## Misc library functions SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 44c43d080..3905af42f 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 8d77fd4f1..9eaa00725 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -94,6 +94,7 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c SRC += $(FLIGHTLIB)/paths.c +SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(MATHLIB)/sin_lookup.c diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 724adc0a6..907ff6833 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -111,6 +111,7 @@ UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp index 0bc943ff4..6025eecc1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp @@ -197,7 +197,6 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c QString sec2 = ""; // after &zoom=... GetSecGoogleWords(pos, sec1, sec2); TryCorrectGoogleVersions(); - QString VersionGoogleSatellite = "132"; return QString("https://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2); } break; diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp index 4de1f311b..c6ceb0f01 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp @@ -44,7 +44,7 @@ CoordinateConversions::CoordinateConversions() * @param[in] LLA Longitude latitude altitude for this location * @param[out] Rne[3][3] Rotation matrix */ -void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3]) +void CoordinateConversions::RneFromLLA(double LLA[3], float Rne[3][3]) { float sinLat, sinLon, cosLat, cosLon; @@ -134,7 +134,7 @@ int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], // stored value is in cm, convert to m double BaseLLA[3]; double ECEF[3]; - double Rne[3][3]; + float Rne[3][3]; // Get LLA address to compute conversion matrix ECEF2LLA(BaseECEFm, BaseLLA); diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h index de6ac7568..23b28fe47 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h @@ -41,7 +41,7 @@ public: CoordinateConversions(); int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]); int NED2LLA_HomeLLA(double LLA[3], double NED[3], double position[3]); - void RneFromLLA(double LLA[3], double Rne[3][3]); + void RneFromLLA(double LLA[3], float Rne[3][3]); void LLA2ECEF(double LLA[3], double ECEF[3]); int ECEF2LLA(double ECEF[3], double LLA[3]); void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]); diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 1b4b2a8da..eb53b75fb 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -268,12 +268,12 @@ void FGSimulator::processUpdate(const QByteArray & inp) float temperature = fields[19].toFloat(); // Get pressure (kpa) float pressure = fields[20].toFloat() * INHG2KPA; - // Get VelocityState Down (cm/s) - float velocityStateDown = -fields[21].toFloat() * FPS2CMPS; - // Get VelocityState East (cm/s) - float velocityStateEast = fields[22].toFloat() * FPS2CMPS; - // Get VelocityState Down (cm/s) - float velocityStateNorth = fields[23].toFloat() * FPS2CMPS; + // Get VelocityState Down (m/s) + float velocityStateDown = -fields[21].toFloat() * FPS2CMPS * 1e-2f; + // Get VelocityState East (m/s) + float velocityStateEast = fields[22].toFloat() * FPS2CMPS * 1e-2f; + // Get VelocityState Down (m/s) + float velocityStateNorth = fields[23].toFloat() * FPS2CMPS * 1e-2f; // Get UDP packets received by FG int n = fields[24].toInt(); @@ -286,16 +286,15 @@ void FGSimulator::processUpdate(const QByteArray & inp) Output2Hardware out; memset(&out, 0, sizeof(Output2Hardware)); + HomeLocation::DataFields homeData = posHome->getData(); + double HomeLLA[3] = { (double)homeData.Latitude * 1e-7, (double)homeData.Longitude * 1e-7, homeData.Altitude }; + double HomeECEF[3]; + float HomeRNE[3][3]; + double LLA[3] = { latitude, longitude, altitude_msl }; float NED[3]; - // convert from cm back to meters - - double LLA[3] = { latitude, longitude, altitude_msl }; - double ECEF[3]; - double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF); - Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED); - + Utils::CoordinateConversions().RneFromLLA(HomeLLA, HomeRNE); + Utils::CoordinateConversions().LLA2ECEF(HomeLLA, HomeECEF); + Utils::CoordinateConversions().LLA2Base(LLA, HomeECEF, HomeRNE, NED); // Update GPS Position objects out.latitude = latitude * 1e7; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 6d74e9fa6..3b8a61432 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -125,7 +125,8 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ $$UAVOBJECT_SYNTHETICS/waypoint.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ - $$UAVOBJECT_SYNTHETICS/mpu6000settings.h + $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \ + $$UAVOBJECT_SYNTHETICS/takeofflocation.h SOURCES += \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \ @@ -227,4 +228,5 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/waypoint.cpp \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ - $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp + $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \ + $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index 9be020de8..83bc91e25 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -12,6 +12,8 @@ + + diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index af68ab276..9110d7d52 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -108,7 +108,7 @@ - + diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index 6a7b690ce..785a152b9 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -45,8 +45,8 @@ - - + + diff --git a/shared/uavobjectdefinition/takeofflocation.xml b/shared/uavobjectdefinition/takeofflocation.xml new file mode 100644 index 000000000..93eadddfc --- /dev/null +++ b/shared/uavobjectdefinition/takeofflocation.xml @@ -0,0 +1,14 @@ + + + TakeOffLocation setting which contains the destination of a ReturnToBase operation + + + + + + + + + + +