diff --git a/flight/libraries/inc/plans.h b/flight/libraries/inc/plans.h index 1d8b6b344..207567b3a 100644 --- a/flight/libraries/inc/plans.h +++ b/flight/libraries/inc/plans.h @@ -66,4 +66,29 @@ void plan_setup_land(); * @brief execute land */ void plan_run_land(); + +/** + * @brief setup pathfollower for positionvario + */ +void plan_setup_PositionVarioFPV(); +void plan_setup_PositionVarioLOS(); +void plan_setup_PositionVarioNSEW(); + +/** + * @brief run for positionvario + */ +void plan_run_PositionVarioFPV(); +void plan_run_PositionVarioLOS(); +void plan_run_PositionVarioNSEW(); + +/** + * @brief setup pathplanner/pathfollower for AutoCruise + */ +void plan_setup_AutoCruise(); + +/** + * @brief execute AutoCruise + */ +void plan_run_AutoCruise(); + #endif /* PLANS_H_ */ diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index c2225564f..9055419fa 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -35,6 +35,14 @@ #include #include #include +#include +#include +#include + +#define UPDATE_EXPECTED 0.02f +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f /** * @brief initialize UAVOs and structs used by this library @@ -45,6 +53,8 @@ void plan_initialize() PositionStateInitialize(); PathDesiredInitialize(); FlightModeSettingsInitialize(); + AttitudeStateInitialize(); + ManualControlCommandInitialize(); } /** @@ -58,14 +68,19 @@ void plan_setup_positionHold() PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + float startingVelocity; + FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); + pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; pathDesired.End.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.StartingVelocity = startingVelocity; + pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); @@ -93,17 +108,21 @@ void plan_setup_returnToBase() float destDown; FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown); destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; - - pathDesired.Start.North = takeoffLocation.North; - pathDesired.Start.East = takeoffLocation.East; - pathDesired.Start.Down = destDown; + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + float startingVelocity; + FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = takeoffLocation.North; pathDesired.End.East = takeoffLocation.East; pathDesired.End.Down = destDown; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.Start.North = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter + pathDesired.Start.East = takeoffLocation.East; + pathDesired.Start.Down = destDown; + + pathDesired.StartingVelocity = startingVelocity; + pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); @@ -128,3 +147,319 @@ void plan_run_land() PathDesiredEndSet(&pathDesiredEnd); } + + +/** + * @brief positionvario functionality + */ +static bool vario_hold = true; +static float hold_position[3]; + +static void plan_setup_PositionVario() +{ + vario_hold = true; + plan_setup_positionHold(); +} + +void plan_setup_PositionVarioFPV() +{ + plan_setup_PositionVario(); +} + +void plan_setup_PositionVarioLOS() +{ + plan_setup_PositionVario(); +} + +void plan_setup_PositionVarioNSEW() +{ + plan_setup_PositionVario(); +} + + +#define DEADBAND 0.1f +static bool normalizeDeadband(float controlVector[4]) +{ + bool moving = false; + + // roll, pitch, yaw between -1 and +1 + // thrust between 0 and 1 mapped to -1 to +1 + controlVector[3] = (2.0f * controlVector[3]) - 1.0f; + int t; + + for (t = 0; t < 4; t++) { + if (controlVector[t] < -DEADBAND) { + moving = true; + controlVector[t] += DEADBAND; + } else if (controlVector[t] > DEADBAND) { + moving = true; + controlVector[t] -= DEADBAND; + } else { + controlVector[t] = 0.0f; + } + // deadband has been cut out, scale value back to [-1,+1] + controlVector[t] *= (1.0f / (1.0f - DEADBAND)); + controlVector[t] = boundf(controlVector[t], -1.0f, 1.0f); + } + + return moving; +} + +typedef enum { FPV, LOS, NSEW } vario_type; + +static void getVector(float controlVector[4], vario_type type) +{ + FlightModeSettingsPositionHoldOffsetData offset; + + FlightModeSettingsPositionHoldOffsetGet(&offset); + + // scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive + controlVector[3] *= offset.Vertical / offset.Horizontal; + + float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]); + + if (length <= 1e-9f) { + length = 1.0f; // should never happen as getVector is not called if control within deadband + } + { + float direction[3] = { + controlVector[1] / length, // pitch is north + controlVector[0] / length, // roll is east + controlVector[3] / length // thrust is down + }; + controlVector[0] = direction[0]; + controlVector[1] = direction[1]; + controlVector[2] = direction[2]; + } + controlVector[3] = length * offset.Horizontal; + + // rotate north and east - rotation angle based on type + float angle; + switch (type) { + case NSEW: + angle = 0.0f; + // NSEW no rotation takes place + break; + case FPV: + // local rotation, using current yaw + AttitudeStateYawGet(&angle); + break; + case LOS: + // determine location based on vector from takeoff to current location + { + PositionStateData positionState; + PositionStateGet(&positionState); + TakeOffLocationData takeoffLocation; + TakeOffLocationGet(&takeoffLocation); + angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North)); + } + break; + } + // rotate horizontally by angle + { + float rotated[2] = { + controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle), + controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle) + }; + controlVector[0] = rotated[0]; + controlVector[1] = rotated[1]; + } +} + + +static void plan_run_PositionVario(vario_type type) +{ + float controlVector[4]; + PathDesiredData pathDesired; + + PathDesiredGet(&pathDesired); + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + + + ManualControlCommandRollGet(&controlVector[0]); + ManualControlCommandPitchGet(&controlVector[1]); + ManualControlCommandYawGet(&controlVector[2]); + ManualControlCommandThrustGet(&controlVector[3]); + + // check if movement is desired + if (normalizeDeadband(controlVector) == false) { + // no movement desired, re-enter positionHold at current start-position + if (!vario_hold) { + vario_hold = true; + + // new hold position is the position that was previously the start position + pathDesired.End.North = hold_position[0]; + pathDesired.End.East = hold_position[1]; + pathDesired.End.Down = hold_position[2]; + // while the new start position has the same offset as in position hold + pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter + pathDesired.Start.East = pathDesired.End.East; + pathDesired.Start.Down = pathDesired.End.Down; + PathDesiredSet(&pathDesired); + } + } else { + PositionStateData positionState; + PositionStateGet(&positionState); + + // flip pitch to have pitch down (away) point north + controlVector[1] = -controlVector[1]; + getVector(controlVector, type); + + // layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4} + if (vario_hold) { + // start position is the position that was previously the hold position + vario_hold = false; + hold_position[0] = pathDesired.End.North; + hold_position[1] = pathDesired.End.East; + hold_position[2] = pathDesired.End.Down; + } else { + // start position is advanced according to movement - in the direction of ControlVector only + // projection using scalar product + float kp = (positionState.North - hold_position[0]) * controlVector[0] + + (positionState.East - hold_position[1]) * controlVector[1] + + (positionState.Down - hold_position[2]) * -controlVector[2]; + if (kp > 0.0f) { + hold_position[0] += kp * controlVector[0]; + hold_position[1] += kp * controlVector[1]; + hold_position[2] += kp * -controlVector[2]; + } + } + // new destination position is advanced based on controlVector + pathDesired.End.North = hold_position[0] + controlVector[0] * controlVector[3]; + pathDesired.End.East = hold_position[1] + controlVector[1] * controlVector[3]; + pathDesired.End.Down = hold_position[2] - controlVector[2] * controlVector[3]; + // the new start position has the same offset as in position hold + pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter + pathDesired.Start.East = pathDesired.End.East; + pathDesired.Start.Down = pathDesired.End.Down; + PathDesiredSet(&pathDesired); + } +} +void plan_run_PositionVarioFPV() +{ + plan_run_PositionVario(FPV); +} + +void plan_run_PositionVarioLOS() +{ + plan_run_PositionVario(LOS); +} + +void plan_run_PositionVarioNSEW() +{ + plan_run_PositionVario(NSEW); +} + + +/** + * @brief setup pathplanner/pathfollower for AutoCruise + */ +static PiOSDeltatimeConfig actimeval; +void plan_setup_AutoCruise() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + float startingVelocity; + FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); + + // initialization is flight in direction of the nose. + // the velocity is not relevant, as it will be reset by the run function even during first call + float angle; + AttitudeStateYawGet(&angle); + float vector[2] = { + cos_lookup_deg(angle), + sin_lookup_deg(angle) + }; + hold_position[0] = positionState.North; + hold_position[1] = positionState.East; + hold_position[2] = positionState.Down; + pathDesired.End.North = hold_position[0] + vector[0]; + pathDesired.End.East = hold_position[1] + vector[1]; + pathDesired.End.Down = hold_position[2]; + // start position has the same offset as in position hold + pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter + pathDesired.Start.East = pathDesired.End.East; + pathDesired.Start.Down = pathDesired.End.Down; + pathDesired.StartingVelocity = startingVelocity; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + + PathDesiredSet(&pathDesired); + + // re-iniztializing deltatime is valid and also good practice here since + // getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode. + PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); +} + +/** + * @brief execute autocruise + */ +void plan_run_AutoCruise() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + + float controlVector[4]; + ManualControlCommandRollGet(&controlVector[0]); + ManualControlCommandPitchGet(&controlVector[1]); + ManualControlCommandYawGet(&controlVector[2]); + controlVector[3] = 0.5f; // dummy, thrust is normalized separately + normalizeDeadband(controlVector); // return value ignored + ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity + controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction + + // normalize old desired movement vector + float vector[3] = { pathDesired.End.North - hold_position[0], + pathDesired.End.East - hold_position[1], + pathDesired.End.Down - hold_position[2] }; + float length = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]); + if (length < 1e-9f) { + length = 1.0f; // should not happen since initialized properly in setup() + } + vector[0] /= length; + vector[1] /= length; + vector[2] /= length; + + // start position is advanced according to actual movement - in the direction of desired vector only + // projection using scalar product + float kp = (positionState.North - hold_position[0]) * vector[0] + + (positionState.East - hold_position[1]) * vector[1] + + (positionState.Down - hold_position[2]) * vector[2]; + if (kp > 0.0f) { + hold_position[0] += kp * vector[0]; + hold_position[1] += kp * vector[1]; + hold_position[2] += kp * vector[2]; + } + + // new angle is equal to old angle plus offset depending on yaw input and time + // (controlVector is normalized with a deadband, change is zero within deadband) + float angle = RAD2DEG(atan2f(vector[1], vector[0])); + float dT = PIOS_DELTATIME_GetAverageSeconds(&actimeval); + angle += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings + + // resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0] + vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3]; + vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3]; + vector[2] = -controlVector[1] * offset.Vertical * controlVector[3]; + + pathDesired.End.North = hold_position[0] + vector[0]; + pathDesired.End.East = hold_position[1] + vector[1]; + pathDesired.End.Down = hold_position[2] + vector[2]; + // start position has the same offset as in position hold + pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter + pathDesired.Start.East = pathDesired.End.East; + pathDesired.Start.Down = pathDesired.End.Down; + PathDesiredSet(&pathDesired); +} diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 8343c41d3..270543f24 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -151,9 +151,13 @@ int32_t configuration_check() } // intentionally no break as this also needs pathfollower case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOFPV: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOLOS: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIONSEW: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)); ADDSEVERITY(navCapableFusion); @@ -226,7 +230,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter // coptercontrol cannot do altitude holding if (coptercontrol) { if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD - || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY + || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO ) { return false; } @@ -235,14 +239,14 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter // check that thrust modes are only set to thrust axis for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) { if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD - || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY + || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO ) { return false; } } if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD - || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY + || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL )) { return false; diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 1b431d521..e3a1d6405 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -103,7 +103,7 @@ void takeOffLocationHandlerInit(); ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ 1 \ ) @@ -120,7 +120,7 @@ void takeOffLocationHandlerInit(); ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ 1 \ ) @@ -137,7 +137,7 @@ void takeOffLocationHandlerInit(); ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ 1 \ ) @@ -154,7 +154,7 @@ void takeOffLocationHandlerInit(); ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ 1 \ ) @@ -171,7 +171,7 @@ void takeOffLocationHandlerInit(); ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ 1 \ ) @@ -188,7 +188,7 @@ void takeOffLocationHandlerInit(); ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \ - ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEVARIO == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) && \ ((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \ 1 \ ) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 4c29949d5..64167f978 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -205,9 +205,13 @@ static void manualControlTask(void) break; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POI: + case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: handler = &handler_PATHFOLLOWER; break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c index 2fbfafaea..a0ba7d1e6 100644 --- a/flight/modules/ManualControl/pathfollowerhandler.c +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -68,32 +68,45 @@ void pathFollowerHandler(bool newinit) case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: plan_setup_positionHold(); break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV: + plan_setup_PositionVarioFPV(); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS: + plan_setup_PositionVarioLOS(); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW: + plan_setup_PositionVarioNSEW(); + break; case FLIGHTSTATUS_FLIGHTMODE_LAND: plan_setup_land(); break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: + plan_setup_AutoCruise(); + 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; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - */ break; } } switch (flightMode) { - // special handling of autoland - behaves like positon hold but with slow altitude decrease + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV: + plan_run_PositionVarioFPV(); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS: + plan_run_PositionVarioLOS(); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW: + plan_run_PositionVarioNSEW(); + break; case FLIGHTSTATUS_FLIGHTMODE_LAND: plan_run_land(); break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: + plan_run_AutoCruise(); + break; default: break; } diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index d80c00185..b5ffb4d35 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -247,7 +247,7 @@ static void stabilizationOuterloopTask() case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit); break; - case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY: + case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO: rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit); break; #endif /* REVOLUTION */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 15b8c3caf..b9ff0541a 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -174,8 +174,8 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE; cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; break; - case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY: - cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY; + case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO: + cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO; cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; break; case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL: diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 9110d7d52..9bb82e566 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -7,69 +7,69 @@ @@ -78,37 +78,42 @@ units="" type="enum" elements="6" - options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,ReturnToBase,Land,PathPlanner,POI" + options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,Autotune,PositionHold,PositionVarioFPV,PositionVarioLOS,PositionVarioNSEW,ReturnToBase,Land,PathPlanner,POI,AutoCruise" defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6" limits="\ - %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0903NE:Autotune:AutoCruise;\ \ - %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0903NE:Autotune:AutoCruise;\ \ - %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0903NE:Autotune:AutoCruise;\ \ - %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0903NE:Autotune:AutoCruise;\ \ - %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI;\ + %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0903NE:Autotune:AutoCruise;\ \ - %0401NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0402NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI,\ - %0903NE:Autotune:PositionHold:ReturnToBase:Land:PathPlanner:POI"/> + %0401NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0402NE:Autotune:PositionHold:PositionVarioFPV:PositionVarioLOS:PositionVarioNSEW:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ + %0903NE:Autotune:AutoCruise"/> + + + + diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 494cf474e..794d02b17 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/pathdesired.xml b/shared/uavobjectdefinition/pathdesired.xml index 76f958779..a70604aa9 100644 --- a/shared/uavobjectdefinition/pathdesired.xml +++ b/shared/uavobjectdefinition/pathdesired.xml @@ -23,7 +23,7 @@ - - + + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 963dd1440..44cab2fcd 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index 0a3788b76..e0283c1cb 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch