diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 641331393..feacd5170 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -130,7 +130,7 @@ static float northVelIntegral = 0; static float eastVelIntegral = 0; static float downVelIntegral = 0; -static float bearingIntegral = 0; +static float courseIntegral = 0; static float speedIntegral = 0; static float powerIntegral = 0; static float airspeedErrorInt = 0; @@ -231,7 +231,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) northVelIntegral = 0; eastVelIntegral = 0; downVelIntegral = 0; - bearingIntegral = 0; + courseIntegral = 0; speedIntegral = 0; powerIntegral = 0; @@ -255,10 +255,10 @@ static void updatePathVelocity() VelocityStateData velocityState; VelocityStateGet(&velocityState); - // look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds - float cur[3] = { positionState.North + (velocityState.North * fixedwingpathfollowerSettings.HeadingFeedForward), - positionState.East + (velocityState.East * fixedwingpathfollowerSettings.HeadingFeedForward), - positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.HeadingFeedForward) }; + // look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds + float cur[3] = { positionState.North + (velocityState.North * fixedwingpathfollowerSettings.CourseFeedForward), + positionState.East + (velocityState.East * fixedwingpathfollowerSettings.CourseFeedForward), + positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) }; struct path_status progress; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); @@ -384,6 +384,7 @@ static uint8_t updateFixedDesiredAttitude() StabilizationSettingsData stabSettings; FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; AirspeedStateData airspeedState; + SystemSettingsData systemSettings; float groundspeedState; float groundspeedDesired; @@ -397,8 +398,12 @@ static uint8_t updateFixedDesiredAttitude() float descentspeedError; float powerCommand; - float bearingError; - float bearingCommand; + float bearing; + float heading; + float headingError; + float course; + float courseError; + float courseCommand; FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); @@ -408,6 +413,7 @@ static uint8_t updateFixedDesiredAttitude() AttitudeStateGet(&attitudeState); StabilizationSettingsGet(&stabSettings); AirspeedStateGet(&airspeedState); + SystemSettingsGet(&systemSettings); /** @@ -424,8 +430,8 @@ static uint8_t updateFixedDesiredAttitude() // Desired ground speed groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias, - fixedwingpathfollowerSettings.BestClimbRateSpeed, - fixedwingpathfollowerSettings.CruiseSpeed); + fixedwingpathfollowerSettings.HorizontalVelMin, + fixedwingpathfollowerSettings.HorizontalVelMax); // Airspeed error airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; @@ -437,32 +443,22 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerSettings.VerticalVelMax); descentspeedError = descentspeedDesired - velocityState.Down; - // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; - if (groundspeedDesired - indicatedAirspeedStateBias <= 0) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; - result = 0; - } // Error condition: plane too slow or too fast fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; - if (indicatedAirspeedState > fixedwingpathfollowerSettings.AirSpeedMax) { + if (indicatedAirspeedState > systemSettings.AirSpeedMax) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; result = 0; } - if (indicatedAirspeedState > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { + if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * 1.2f) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; result = 0; } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF + if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * 0.8f) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; result = 0; } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; - result = 0; - } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { + if (indicatedAirspeedState < systemSettings.AirSpeedMin) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; result = 0; } @@ -487,7 +483,7 @@ static uint8_t updateFixedDesiredAttitude() // Compute the cross feed from vertical speed to pitch, with saturation float speedErrorToPowerCommandComponent = bound( - (airspeedError / fixedwingpathfollowerSettings.BestClimbRateSpeed) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP], + (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP], -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] ); @@ -567,35 +563,58 @@ static uint8_t updateFixedDesiredAttitude() 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[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; + if (headingError > 90.0f || headingError < -90.0f) { + // we are flying backwards + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; + result = 0; + } + /** * Compute desired roll command */ if (groundspeedDesired > 1e-6f) { - bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); + course = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North)); + + courseError = course - heading; } else { // if we are not supposed to move, run in a circle - bearingError = -90.0f; + courseError = -90.0f; + result = 0; } - if (bearingError < -180.0f) { - bearingError += 360.0f; + if (courseError < -180.0f) { + courseError += 360.0f; } - if (bearingError > 180.0f) { - bearingError -= 360.0f; + if (courseError > 180.0f) { + courseError -= 360.0f; } - bearingIntegral = bound(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI], - -fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT], - fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]); - bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] + - bearingIntegral); + courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI], + -fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT], + fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]); + courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] + + courseIntegral); - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand; + fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_COURSE] = courseError; + fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_COURSE] = courseIntegral; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_COURSE] = courseCommand; stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + - bearingCommand, + courseCommand, fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX]); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 0c0d07a04..495f1d4a1 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -61,7 +61,7 @@ #if defined(PIOS_MANUAL_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #else -#define STACK_SIZE_BYTES 1024 +#define STACK_SIZE_BYTES 1152 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 4) @@ -323,9 +323,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters) cmd.Yaw = 0; cmd.Pitch = 0; cmd.Collective = 0; - // cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning - // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, - // or leave throttle at IDLE speed or above when going into AUTO-failsafe. + if (settings.FailsafeBehavior != MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_NONE) { + FlightStatusGet(&flightStatus); + + flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1]; + FlightStatusSet(&flightStatus); + } AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AccessoryDesiredData accessory; @@ -724,15 +727,17 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * // Simple Return To Base mode - keep altitude the same, fly to home position PositionStateData positionState; PositionStateGet(&positionState); + ManualControlSettingsData settings; + ManualControlSettingsGet(&settings); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = 0; pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - 10; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - 10; + pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -937,6 +942,9 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) */ static bool okToArm(void) { + // update checks + configuration_check(); + // read alarms SystemAlarmsData alarms; diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index f4ece087b..c7be26409 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -371,7 +371,7 @@ static uint8_t conditionDistanceToTarget() if (pathAction.ConditionParameters[1] > 0.5f) { distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) + powf(waypoint.Position[1] - positionState.East, 2) - + powf(waypoint.Position[1] - positionState.Down, 2)); + + powf(waypoint.Position[2] - positionState.Down, 2)); } else { distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) + powf(waypoint.Position[1] - positionState.East, 2)); diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index 4f0afeac5..c7602426b 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -41,7 +41,7 @@ GPSPosNorth diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index f792fd7dc..8c50108a2 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -3,48 +3,42 @@ Settings for the @ref FixedWingPathFollowerModule - - - + - + - - - - - + - + - + - + - + - + - + diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml index 7bdca082d..fe31830db 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml @@ -1,9 +1,9 @@ Object Storing Debugging Information on PID internals - - - + + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 52a45ef0f..d286c16dd 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -71,7 +71,8 @@ %0903NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/> - + + diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index 88f6af2da..983863650 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -23,7 +23,7 @@ - + diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml index 95144e4e6..fc7c9d462 100644 --- a/shared/uavobjectdefinition/systemsettings.xml +++ b/shared/uavobjectdefinition/systemsettings.xml @@ -3,6 +3,12 @@ Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand + + + + + diff --git a/shared/uavobjectdefinition/waypoint.xml b/shared/uavobjectdefinition/waypoint.xml index bcadf49ac..2a27f7fd5 100644 --- a/shared/uavobjectdefinition/waypoint.xml +++ b/shared/uavobjectdefinition/waypoint.xml @@ -5,7 +5,7 @@ - +