From d43797628762c75e35d2449ff29623e3377b33f7 Mon Sep 17 00:00:00 2001 From: James Duley Date: Thu, 3 Jul 2014 19:40:41 +1200 Subject: [PATCH 01/25] removed so version number from linux icu libs --- ground/openpilotgcs/copydata.pro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index a60b7b8a6..2805a26f6 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -30,9 +30,9 @@ GCS_LIBRARY_PATH libQt5Qml.so.5 \ libQt5DBus.so.5 \ libQt5QuickParticles.so.5 \ - libicui18n.so.51 \ - libicuuc.so.51 \ - libicudata.so.51 \ + libicui18n.so \ + libicuuc.so \ + libicudata.so \ libqgsttools_p.so.1 data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline() From 90b84b210e616d2ec786875da9960e21a6b752dd Mon Sep 17 00:00:00 2001 From: James Duley Date: Sat, 5 Jul 2014 23:30:40 +1200 Subject: [PATCH 02/25] Revert "removed so version number from linux icu libs" it breaks the build with qt_sdk_install This reverts commit d43797628762c75e35d2449ff29623e3377b33f7. --- ground/openpilotgcs/copydata.pro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 2805a26f6..a60b7b8a6 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -30,9 +30,9 @@ GCS_LIBRARY_PATH libQt5Qml.so.5 \ libQt5DBus.so.5 \ libQt5QuickParticles.so.5 \ - libicui18n.so \ - libicuuc.so \ - libicudata.so \ + libicui18n.so.51 \ + libicuuc.so.51 \ + libicudata.so.51 \ libqgsttools_p.so.1 data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline() From 95676043d7e7dbf78c3fbab0beb1bb09c24d16e5 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 6 Jul 2014 18:55:54 +1200 Subject: [PATCH 03/25] on linux check whether QT_INSTALL_LIBS are /usr/lib/* set copyqt to 0 if that is the case --- ground/openpilotgcs/copydata.pro | 2 +- ground/openpilotgcs/openpilotgcs.pri | 11 ++++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index a60b7b8a6..b277b0f95 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -3,7 +3,7 @@ include(openpilotgcs.pri) TEMPLATE = subdirs # Copy Qt runtime libraries into the build directory (to run or package) -equals(copydata, 1) { +equals(copyqt, 1) { GCS_LIBRARY_PATH diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 31c0ed454..d8c392b9c 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -83,16 +83,26 @@ macx { GCS_DATA_BASENAME = Resources GCS_DOC_PATH = $$GCS_DATA_PATH/doc copydata = 1 + copyqt = 1 } else { + !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 win32 { contains(TEMPLATE, vc.*)|contains(TEMPLATE_PREFIX, vc):vcproj = 1 GCS_APP_TARGET = openpilotgcs + copyqt = $$copydata } else { GCS_APP_WRAPPER = openpilotgcs GCS_APP_TARGET = openpilotgcs.bin GCS_QT_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5 GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml + lib_dir = $$[QT_INSTALL_LIBS] + lib_dir ~= s,/usr/lib/*,/usr/lib + equals(lib_dir, "/usr/lib") { + copyqt = 0 + } else { + copyqt = 1 + } } GCS_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/openpilotgcs GCS_PLUGIN_PATH = $$GCS_LIBRARY_PATH/plugins @@ -100,7 +110,6 @@ macx { GCS_DATA_PATH = $$GCS_BUILD_TREE/share/openpilotgcs GCS_DATA_BASENAME = share/openpilotgcs GCS_DOC_PATH = $$GCS_BUILD_TREE/share/doc - !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 } From 3e2961420d777eee37042576e7e953248430df89 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 15 Jul 2014 09:30:24 +0200 Subject: [PATCH 04/25] Add a 3D mode to path_endpoint. This allows FlyDirect waypoints to do vertical movements without changing position. --- flight/libraries/inc/paths.h | 4 +- flight/libraries/paths.c | 43 ++++++++++++------- .../VtolPathFollower/vtolpathfollower.c | 19 ++++---- 3 files changed, 40 insertions(+), 26 deletions(-) diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index bbb3995fe..4cecbe845 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -30,8 +30,8 @@ struct path_status { float fractional_progress; float error; - float correction_direction[2]; - float path_direction[2]; + float correction_direction[3]; + float path_direction[3]; }; void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 1cbddb7d2..831f3e98f 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -34,7 +34,7 @@ // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); @@ -65,10 +65,13 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc break; case PATHDESIRED_MODE_FLYENDPOINT: + return path_endpoint(start_point, end_point, cur_point, status, true); + + break; case PATHDESIRED_MODE_DRIVEENDPOINT: default: // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status); + return path_endpoint(start_point, end_point, cur_point, status, false); break; } @@ -80,30 +83,33 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc * @param[in] end_point Ending point * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation + * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status) +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) { - float path_north, path_east, diff_north, diff_east; + float path[3], diff[3]; float dist_path, dist_diff; // we do not correct in this mode - status->correction_direction[0] = status->correction_direction[1] = 0; + status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0; // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to end - diff_north = end_point[0] - cur_point[0]; - diff_east = end_point[1] - cur_point[1]; + diff[0] = end_point[0] - cur_point[0]; + diff[1] = end_point[1] - cur_point[1]; + diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; - dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]); + dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]); if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[0] = status->path_direction[1] = status->path_direction[2] = 0; return; } @@ -111,8 +117,9 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point status->error = dist_diff; // Compute direction to travel - status->path_direction[0] = diff_north / dist_diff; - status->path_direction[1] = diff_east / dist_diff; + status->path_direction[0] = diff[0] / dist_diff; + status->path_direction[1] = diff[1] / dist_diff; + status->path_direction[2] = diff[2] / dist_diff; } /** @@ -144,7 +151,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, // if the path is too short, we cannot determine vector direction. // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status); + path_endpoint(start_point, end_point, cur_point, status, false); status->fractional_progress = 1; return; } @@ -159,6 +166,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, // Compute direction to correct error status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; + status->correction_direction[2] = 0; // Now just want magnitude of error status->error = fabs(status->error); @@ -166,6 +174,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, // Compute direction to travel status->path_direction[0] = path_north / dist_path; status->path_direction[1] = path_east / dist_path; + status->path_direction[2] = 0; } /** @@ -200,8 +209,10 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, status->error = radius; status->correction_direction[0] = 0; status->correction_direction[1] = 1; + status->correction_direction[2] = 0; status->path_direction[0] = 1; status->path_direction[1] = 0; + status->path_direction[2] = 0; return; } @@ -246,10 +257,12 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, // Compute direction to correct error status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius; + status->correction_direction[2] = 0; // Compute direction to travel status->path_direction[0] = normal[0]; status->path_direction[1] = normal[1]; + status->path_direction[2] = 0; status->error = fabs(status->error); } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 57d96fad9..34b4bfaea 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -387,35 +387,36 @@ static void updatePathVelocity() cast_struct_to_array(pathDesired.End, pathDesired.End.North), cur, &progress, pathDesired.Mode); - float groundspeed; + float speed; switch (pathDesired.Mode) { case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; + speed = pathDesired.EndingVelocity; break; case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); + speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { - groundspeed = 0; + speed = 0; } break; case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: - groundspeed = pathDesired.StartingVelocity + speed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { - groundspeed = 0; + speed = 0; } break; } VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0] * groundspeed; - velocityDesired.East = progress.path_direction[1] * groundspeed; + velocityDesired.North = progress.path_direction[0] * speed; + velocityDesired.East = progress.path_direction[1] * speed; + velocityDesired.Down = progress.path_direction[2] * speed; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; float correction_velocity[2] = @@ -436,7 +437,7 @@ static void updatePathVelocity() downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, -vtolpathfollowerSettings.VerticalPosPI.ILimit, vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); + downCommand = velocityDesired.Down + (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus From 7da0034775f5919cad2e5e92d267dee12322020c Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 15 Jul 2014 09:39:13 +0200 Subject: [PATCH 05/25] Improved automatic landing algorithm to better track descend speed. Desired descend speed is now a FlightModeSetting. --- flight/libraries/plans.c | 22 ++++++++++++++++--- .../flightmodesettings.xml | 1 + 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 9055419fa..10df7e52c 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -128,9 +128,19 @@ void plan_setup_returnToBase() PathDesiredSet(&pathDesired); } +static PiOSDeltatimeConfig landdT; void plan_setup_land() { + float descendspeed; plan_setup_positionHold(); + + FlightModeSettingsLandingVelocityGet(&descendspeed); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.StartingVelocity = descendspeed; + pathDesired.EndingVelocity = descendspeed; + PathDesiredSet(&pathDesired); + PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } /** @@ -138,12 +148,18 @@ void plan_setup_land() */ void plan_run_land() { + float downPos, descendspeed; PathDesiredEndData pathDesiredEnd; - PathDesiredEndGet(&pathDesiredEnd); + PositionStateDownGet(&downPos); // current down position + PathDesiredEndGet(&pathDesiredEnd); // desired position + PathDesiredEndingVelocityGet(&descendspeed); - PositionStateDownGet(&pathDesiredEnd.Down); - pathDesiredEnd.Down += 5; + // desired position is updated to match the desired descend speed but don't run ahead + // too far if the current position can't keep up. This normaly means we have landed. + if (pathDesiredEnd.Down - downPos < 10) { + pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT); + } PathDesiredEndSet(&pathDesiredEnd); } diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index b4b5a1c50..728fc27c0 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -110,6 +110,7 @@ + From 065ba1a0c81972be3b54914aa46648fb689dc3b2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 29 Jul 2014 13:06:40 +0200 Subject: [PATCH 06/25] Adds a 3D mode to path_vector. This allows vertical path segments without position changes. PathStatus now also contains correction_direction and path_direction to make path following behaviour more transparent. --- flight/libraries/paths.c | 89 ++++++++++++------- .../VtolPathFollower/vtolpathfollower.c | 62 +++++++------ shared/uavobjectdefinition/pathstatus.xml | 7 +- 3 files changed, 97 insertions(+), 61 deletions(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 831f3e98f..d8da67299 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -26,6 +26,7 @@ #include #include +#include #include "paths.h" @@ -35,7 +36,7 @@ // private functions static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); /** @@ -50,8 +51,11 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc { switch (mode) { case PATHDESIRED_MODE_FLYVECTOR: + return path_vector(start_point, end_point, cur_point, status, true); + + break; case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status); + return path_vector(start_point, end_point, cur_point, status, false); break; case PATHDESIRED_MODE_FLYCIRCLERIGHT: @@ -113,7 +117,11 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point return; } - status->fractional_progress = 1 - dist_diff / (1 + dist_path); + if (dist_path + 1 > dist_diff) { + status->fractional_progress = 1 - dist_diff / (1 + dist_path); + } else { + status->fractional_progress = 0; // we don't want fractional_progress to become negative + } status->error = dist_diff; // Compute direction to travel @@ -128,53 +136,70 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point * @param[in] end_point Ending point * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation + * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status) +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) { - float path_north, path_east, diff_north, diff_east; + float path[3], diff[3]; float dist_path; float dot; - float normal[2]; + float track_point[3]; // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to start - diff_north = cur_point[0] - start_point[0]; - diff_east = cur_point[1] - start_point[1]; + diff[0] = cur_point[0] - start_point[0]; + diff[1] = cur_point[1] - start_point[1]; + diff[2] = mode3D ? cur_point[2] - start_point[2]: 0; - dot = path_north * diff_north + path_east * diff_east; - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; + dist_path = sqrtf(path[0] * path[0] + path[1] * path[1] + path[2] * path[2]); - if (dist_path < 1e-6f) { + if (dist_path > 1e-6f) { + // Compute direction to travel & progress + status->path_direction[0] = path[0] / dist_path; + status->path_direction[1] = path[1] / dist_path; + status->path_direction[2] = path[2] / dist_path; + status->fractional_progress = dot / (dist_path * dist_path); + } else { // if the path is too short, we cannot determine vector direction. - // Fly towards the endpoint to prevent flying away, - // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status, false); + // Assume progress=1 and zero-length path. + status->path_direction[0] = 0; + status->path_direction[1] = 0; + status->path_direction[2] = 0; status->fractional_progress = 1; - return; } - // Compute the normal to the path - normal[0] = -path_east / dist_path; - normal[1] = path_north / dist_path; + // Compute point on track that is closest to our current position. + // Limiting fractional_progress makes sure the resulting point is also + // limited to be between start and endpoint. + status->fractional_progress = boundf(status->fractional_progress, 0, 1); - status->fractional_progress = dot / (dist_path * dist_path); - status->error = normal[0] * diff_north + normal[1] * diff_east; + track_point[0] = status->fractional_progress * path[0] + start_point[0]; + track_point[1] = status->fractional_progress * path[1] + start_point[1]; + track_point[2] = status->fractional_progress * path[2] + start_point[2]; - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; - status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; - status->correction_direction[2] = 0; + status->correction_direction[0] = track_point[0] - cur_point[0]; + status->correction_direction[1] = track_point[1] - cur_point[1]; + status->correction_direction[2] = track_point[2] - cur_point[2]; - // Now just want magnitude of error - status->error = fabs(status->error); + status->error = sqrt(status->correction_direction[0] * status->correction_direction[0] + + status->correction_direction[1] * status->correction_direction[1] + + status->correction_direction[2] * status->correction_direction[2]); - // Compute direction to travel - status->path_direction[0] = path_north / dist_path; - status->path_direction[1] = path_east / dist_path; - status->path_direction[2] = 0; + // Normalize correction_direction but avoid division by zero + if (status->error > 1e-6f) { + status->correction_direction[0] /= status->error; + status->correction_direction[1] /= status->error; + status->correction_direction[2] /= status->error; + } else { + status->correction_direction[0] = 0; + status->correction_direction[1] = 0; + status->correction_direction[2] = 1; + } } /** diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 34b4bfaea..7cb3f27db 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -370,22 +370,19 @@ static void updatePOIBearing() static void updatePathVelocity() { float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - float downCommand; PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); PositionStateData positionState; PositionStateGet(&positionState); - float cur[3] = - { positionState.North, positionState.East, positionState.Down }; + float current_position[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; path_progress( cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + current_position, &progress, pathDesired.Mode); float speed; switch (pathDesired.Mode) { @@ -414,36 +411,45 @@ static void updatePathVelocity() } VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0] * speed; - velocityDesired.East = progress.path_direction[1] * speed; - velocityDesired.Down = progress.path_direction[2] * speed; - float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; - float correction_velocity[2] = - { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; + northPosIntegral += progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; + eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; + downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT; - float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { - scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); + + velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral + + progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral + + progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral + + progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; + + // Make sure the desired velocities don't exceed PathFollower limits. + float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2)); + + if (groundspeedDesired > vtolpathfollowerSettings.HorizontalVelMax) { + velocityDesired.North *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; + velocityDesired.East *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; } - velocityDesired.North += progress.correction_direction[0] * error_speed * scale; - velocityDesired.East += progress.correction_direction[1] * error_speed * scale; - - float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1); - - float downError = altitudeSetpoint - positionState.Down; - downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = velocityDesired.Down + (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.path_direction_north = progress.path_direction[0]; + pathStatus.path_direction_east = progress.path_direction[1]; + pathStatus.path_direction_down = progress.path_direction[2]; + pathStatus.correction_direction_north = progress.correction_direction[0]; + pathStatus.correction_direction_east = progress.correction_direction[1]; + pathStatus.correction_direction_down = progress.correction_direction[2]; VelocityDesiredSet(&velocityDesired); } diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml index 52352dc4a..812b7a1cf 100644 --- a/shared/uavobjectdefinition/pathstatus.xml +++ b/shared/uavobjectdefinition/pathstatus.xml @@ -7,7 +7,12 @@ - + + + + + + From 9ab6299247ef3f44b4c50a639f0535038a11a423 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 10:18:39 +0200 Subject: [PATCH 07/25] We don't want zero-length path_direction vector. --- flight/libraries/paths.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index d8da67299..5d438959a 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -113,7 +113,8 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = status->path_direction[2] = 0; + status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[2] = 1; return; } From 70e34f44cf24b3d7caa0637671933558ce0c6008 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 10:52:06 +0200 Subject: [PATCH 08/25] Revert to the original behaviour of calling "fly_endpoint" if we have a zero-length path. --- flight/libraries/paths.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 5d438959a..888fd43ab 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -166,12 +166,11 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, status->path_direction[2] = path[2] / dist_path; status->fractional_progress = dot / (dist_path * dist_path); } else { - // if the path is too short, we cannot determine vector direction. - // Assume progress=1 and zero-length path. - status->path_direction[0] = 0; - status->path_direction[1] = 0; - status->path_direction[2] = 0; + // Fly towards the endpoint to prevent flying away, + // but assume progress=1 either way. + path_endpoint(start_point, end_point, cur_point, status); status->fractional_progress = 1; + return; } // Compute point on track that is closest to our current position. From 446415457d059ba6c7a57d8a5332e7cb38dddf51 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 10:56:25 +0200 Subject: [PATCH 09/25] Don't limit the path to strictly be between start and endpoint. --- flight/libraries/paths.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 888fd43ab..8c6416406 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -174,10 +174,6 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, } // Compute point on track that is closest to our current position. - // Limiting fractional_progress makes sure the resulting point is also - // limited to be between start and endpoint. - status->fractional_progress = boundf(status->fractional_progress, 0, 1); - track_point[0] = status->fractional_progress * path[0] + start_point[0]; track_point[1] = status->fractional_progress * path[1] + start_point[1]; track_point[2] = status->fractional_progress * path[2] + start_point[2]; From f29f7fa6803a270d1e5f55033c1ac0c35c77f4c2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 16:31:16 +0200 Subject: [PATCH 10/25] Missing argument for path_endpoint added. --- flight/libraries/paths.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 8c6416406..19176c443 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -168,7 +168,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, } else { // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status); + path_endpoint(start_point, end_point, cur_point, status, mode3D); status->fractional_progress = 1; return; } From ca1923f76fabec18f1272f4f8a7395831e974c37 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 12:50:38 +0200 Subject: [PATCH 11/25] OP-1156: Added new (stub template) pathFollower Module as well as builkd environment changes to compile it instead of old pathfollowers --- flight/libraries/sanitycheck.c | 1 - flight/modules/PathFollower/pathfollower.c | 138 ++++++++++++++++++ .../boards/discoveryf4bare/firmware/Makefile | 3 +- .../boards/revolution/firmware/Makefile | 3 +- .../boards/revoproto/firmware/Makefile | 3 +- .../targets/boards/simposix/firmware/Makefile | 3 +- shared/uavobjectdefinition/callbackinfo.xml | 3 + shared/uavobjectdefinition/hwsettings.xml | 2 +- shared/uavobjectdefinition/taskinfo.xml | 3 - 9 files changed, 146 insertions(+), 13 deletions(-) create mode 100644 flight/modules/PathFollower/pathfollower.c diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 365e1af7b..0902cbce7 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -149,7 +149,6 @@ int32_t configuration_check() case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); - ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)); ADDSEVERITY(navCapableFusion); break; default: diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c new file mode 100644 index 000000000..3a719ffd7 --- /dev/null +++ b/flight/modules/PathFollower/pathfollower.c @@ -0,0 +1,138 @@ +/** + ****************************************************************************** + * + * @file pathfollower.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * and sets @ref AttitudeDesired. It only does this when the FlightMode field + * of @ref ManualControlCommand is Auto. + * + * @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 + */ + +/** + * Input object: PathDesired + * Input object: PositionState + * Input object: ManualControlCommand + * Output object: StabilizationDesired + * + * This module acts as "autopilot" - it controls the setpoints of stabilization + * based on current flight situation and desired flight path (PathDesired) as + * directed by flightmode selection or pathplanner + * This is a periodic delayed callback module + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include + +#include + +#include +#include +#include + + +#include +#include +#include +#include + + +// Private constants + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define PF_IDLE_UPDATE_RATE_MS 100 + +#define STACK_SIZE_BYTES 2048 +// Private types + +// Private variables +static DelayedCallbackInfo *pathFollowerCBInfo; + +static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; + +// Private functions +static void pathFollowerTask(void); +static void SettingsUpdatedCb(UAVObjEvent *ev); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerStart() +{ + // Start main task + SettingsUpdatedCb(NULL); + PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerInitialize() +{ + // initialize objects + FixedWingPathFollowerSettingsInitialize(); + VtolPathFollowerSettingsInitialize(); + FlightStatusInitialize(); + PathStatusInitialize(); + + // Create object queue + + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); + FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + + return 0; +} +MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); + + +/** + * Module thread, should not return. + */ +static void pathFollowerTask(void) +{ + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + } + + // do pathfollower things here + + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); +} + +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // read in settings (TODO) +} diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 608d2b6cf..bc8e72959 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -35,7 +35,6 @@ USE_DSP_LIB ?= NO #MODULES += Airspeed #MODULES += AltitudeHold #MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Actuator MODULES += GPS @@ -45,7 +44,7 @@ MODULES += Battery MODULES += FirmwareIAP #MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index cb99372f1..52b5f8a64 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -46,7 +45,7 @@ MODULES += Battery MODULES += FirmwareIAP MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 39d2d7f3b..9933124a3 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -45,7 +44,7 @@ MODULES += CameraStab MODULES += Battery MODULES += FirmwareIAP MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 2c89a81ef..d8af95e4a 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -33,8 +33,7 @@ DEBUG ?= YES # List of modules to include MODULES = ManualControl Stabilization GPS MODULES += PathPlanner -MODULES += FixedWingPathFollower -MODULES += VtolPathFollower +MODULES += PathFollower MODULES += CameraStab MODULES += Telemetry MODULES += Logging diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index 8624867c4..598e4988a 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -8,6 +8,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -20,6 +21,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -36,6 +38,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f005a31ab..c41187009 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -22,7 +22,7 @@ - + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index a5fb7ad4b..f8018ab53 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -19,7 +19,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -52,7 +51,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -89,7 +87,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx From 8944419b0fcb361a22816af3a494f2c2cdeca0d7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 19:41:40 +0200 Subject: [PATCH 12/25] OP-1156 refactored pathfollower - new unified module as delayed callback - compiles but untested --- flight/libraries/math/mathmisc.h | 27 + flight/libraries/paths.c | 168 +-- flight/libraries/plans.c | 1 + flight/modules/PathFollower/pathfollower.c | 1084 ++++++++++++++++- .../VtolPathFollower/vtolpathfollower.c | 23 +- .../vtolpathfollowersettings.xml | 25 +- 6 files changed, 1219 insertions(+), 109 deletions(-) diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index b4c8b75b8..42213e504 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -52,4 +52,31 @@ static inline float boundf(float val, float boundary1, float boundary2) return val; } +static inline float squaref(float x) +{ + return x * x; +} + +static inline float vector_lengthf(float *vector, const uint8_t dim) +{ + float length = 0.0f; + + for (int t = 0; t < dim; t++) { + length += squaref(vector[t]); + } + return sqrtf(length); +} + +static inline void vector_normalizef(float *vector, const uint8_t dim) +{ + float length = vector_lengthf(vector, dim); + + if (length <= 0.0f || isnan(length)) { + return; + } + for (int t = 0; t < dim; t++) { + vector[t] /= length; + } +} + #endif /* MATHMISC_H */ diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 19176c443..6c80d0a9d 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -53,7 +53,7 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc case PATHDESIRED_MODE_FLYVECTOR: return path_vector(start_point, end_point, cur_point, status, true); - break; + break; case PATHDESIRED_MODE_DRIVEVECTOR: return path_vector(start_point, end_point, cur_point, status, false); @@ -98,23 +98,23 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0; // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to end - diff[0] = end_point[0] - cur_point[0]; - diff[1] = end_point[1] - cur_point[1]; - diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; + diff[0] = end_point[0] - cur_point[0]; + diff[1] = end_point[1] - cur_point[1]; + diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; - dist_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]); - dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]); + dist_diff = vector_lengthf(diff, 3); + dist_path = vector_lengthf(path, 3); if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; - status->path_direction[2] = 1; + status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[2] = 0; return; } @@ -147,23 +147,23 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, float track_point[3]; // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to start - diff[0] = cur_point[0] - start_point[0]; - diff[1] = cur_point[1] - start_point[1]; - diff[2] = mode3D ? cur_point[2] - start_point[2]: 0; + diff[0] = cur_point[0] - start_point[0]; + diff[1] = cur_point[1] - start_point[1]; + diff[2] = mode3D ? cur_point[2] - start_point[2] : 0; - dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; - dist_path = sqrtf(path[0] * path[0] + path[1] * path[1] + path[2] * path[2]); + dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; + dist_path = vector_lengthf(path, 3); if (dist_path > 1e-6f) { // Compute direction to travel & progress - status->path_direction[0] = path[0] / dist_path; - status->path_direction[1] = path[1] / dist_path; - status->path_direction[2] = path[2] / dist_path; + status->path_direction[0] = path[0] / dist_path; + status->path_direction[1] = path[1] / dist_path; + status->path_direction[2] = path[2] / dist_path; status->fractional_progress = dot / (dist_path * dist_path); } else { // Fly towards the endpoint to prevent flying away, @@ -182,9 +182,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, status->correction_direction[1] = track_point[1] - cur_point[1]; status->correction_direction[2] = track_point[2] - cur_point[2]; - status->error = sqrt(status->correction_direction[0] * status->correction_direction[0] + - status->correction_direction[1] * status->correction_direction[1] + - status->correction_direction[2] * status->correction_direction[2]); + status->error = vector_lengthf(status->correction_direction, 3); // Normalize correction_direction but avoid division by zero if (status->error > 1e-6f) { @@ -194,7 +192,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, } else { status->correction_direction[0] = 0; status->correction_direction[1] = 0; - status->correction_direction[2] = 1; + status->correction_direction[2] = 0; } } @@ -207,7 +205,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, */ static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) { - float radius_north, radius_east, diff_north, diff_east; + float radius_north, radius_east, diff_north, diff_east, diff_down; float radius, cradius; float normal[2]; float progress; @@ -220,70 +218,72 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, // Current location relative to center diff_north = cur_point[0] - end_point[0]; diff_east = cur_point[1] - end_point[1]; + diff_down = cur_point[2] - end_point[2]; - radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); - cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2)); + radius = sqrtf(squaref(radius_north) + squaref(radius_east)); + cradius = sqrtf(squaref(diff_north) + squaref(diff_east)); - if (cradius < 1e-6f) { - // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; - status->error = radius; - status->correction_direction[0] = 0; - status->correction_direction[1] = 1; - status->correction_direction[2] = 0; - status->path_direction[0] = 1; - status->path_direction[1] = 0; - status->path_direction[2] = 0; - return; - } - - if (clockwise) { - // Compute the normal to the radius clockwise - normal[0] = -diff_east / cradius; - normal[1] = diff_north / cradius; - } else { - // Compute the normal to the radius counter clockwise - normal[0] = diff_east / cradius; - normal[1] = -diff_north / cradius; - } - - // normalize progress to 0..1 - a_diff = atan2f(diff_north, diff_east); - a_radius = atan2f(radius_north, radius_east); - - if (a_diff < 0) { - a_diff += 2.0f * M_PI_F; - } - if (a_radius < 0) { - a_radius += 2.0f * M_PI_F; - } - - progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F); - - if (progress < 0) { - progress += 1.0f; - } else if (progress >= 1.0f) { - progress -= 1.0f; - } - - if (clockwise) { - progress = 1 - progress; - } - - status->fractional_progress = progress; + // circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply) + status->path_direction[2] = 0.0f; // error is current radius minus wanted radius - positive if too close status->error = radius - cradius; - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; - status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius; - status->correction_direction[2] = 0; + if (cradius < 1e-6f) { + // cradius is zero, just fly somewhere and make sure correction is still a normal + status->fractional_progress = 1; + status->correction_direction[0] = 0; + status->correction_direction[1] = 1; + status->path_direction[0] = 1; + status->path_direction[1] = 0; + } else { + if (clockwise) { + // Compute the normal to the radius clockwise + normal[0] = -diff_east / cradius; + normal[1] = diff_north / cradius; + } else { + // Compute the normal to the radius counter clockwise + normal[0] = diff_east / cradius; + normal[1] = -diff_north / cradius; + } - // Compute direction to travel - status->path_direction[0] = normal[0]; - status->path_direction[1] = normal[1]; - status->path_direction[2] = 0; + // normalize progress to 0..1 + a_diff = atan2f(diff_north, diff_east); + a_radius = atan2f(radius_north, radius_east); + + if (a_diff < 0) { + a_diff += 2.0f * M_PI_F; + } + if (a_radius < 0) { + a_radius += 2.0f * M_PI_F; + } + + progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F); + + if (progress < 0.0f) { + progress += 1.0f; + } else if (progress >= 1.0f) { + progress -= 1.0f; + } + + if (clockwise) { + progress = 1.0f - progress; + } + + status->fractional_progress = progress; + + // Compute direction to travel + status->path_direction[0] = normal[0]; + status->path_direction[1] = normal[1]; + + // Compute direction to correct error + status->correction_direction[0] = status->error * diff_north / cradius; + status->correction_direction[1] = status->error * diff_east / cradius; + } + + status->correction_direction[2] = -diff_down; + + vector_normalizef(status->correction_direction, 3); status->error = fabs(status->error); } diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 10df7e52c..4df2a9289 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -132,6 +132,7 @@ static PiOSDeltatimeConfig landdT; void plan_setup_land() { float descendspeed; + plan_setup_positionHold(); FlightModeSettingsLandingVelocityGet(&descendspeed); diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 3a719ffd7..93efab3c9 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -51,12 +51,29 @@ #include #include #include +#include +#include +#include +#include #include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Private constants @@ -67,16 +84,53 @@ #define PF_IDLE_UPDATE_RATE_MS 100 #define STACK_SIZE_BYTES 2048 + +#define DEADBAND_HIGH 0.10f +#define DEADBAND_LOW -0.10f // Private types +struct Integrals { + float vel[3]; + float course; + float speed; + float power; + float airspeed; + float poiRadius; + bool vtolEmergencyFallback; +}; + + // Private variables static DelayedCallbackInfo *pathFollowerCBInfo; - static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; +static struct Integrals i; +static PathStatusData pathStatus; +static PathDesiredData pathDesired; +static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; +static VtolPathFollowerSettingsData vtolPathFollowerSettings; + +// correct speed by measured airspeed +static float indicatedAirspeedStateBias = 0.0f; + // Private functions static void pathFollowerTask(void); +static void resetIntegrals(); static void SettingsUpdatedCb(UAVObjEvent *ev); +static uint8_t updateAutoPilotByFrameType(); +static uint8_t updateAutoPilotFixedWing(); +static uint8_t updateAutoPilotVtol(); +static float updateTailInBearing(); +static float updateCourseBearing(); +static float updatePOIBearing(); +static void processPOI(); +static void updatePathVelocity(float kFF, float kH, float kV, bool limited); +static uint8_t updateFixedDesiredAttitude(); +static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); +static void updateFixedAttitude(); +static void updateVtolDesiredAttitudeEmergencyFallback(); +static void airspeedStateUpdatedCb(UAVObjEvent *ev); +static bool correctCourse(float *C, float *V, float *F, float s); /** * Initialise the module, called on startup @@ -85,6 +139,7 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); int32_t PathFollowerStart() { // Start main task + PathStatusGet(&pathStatus); SettingsUpdatedCb(NULL); PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); @@ -99,15 +154,32 @@ int32_t PathFollowerInitialize() { // initialize objects FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); VtolPathFollowerSettingsInitialize(); FlightStatusInitialize(); PathStatusInitialize(); + PathDesiredInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); + VelocityDesiredInitialize(); + StabilizationDesiredInitialize(); + AirspeedStateInitialize(); + AttitudeStateInitialize(); + TakeOffLocationInitialize(); + PoiLocationInitialize(); + ManualControlCommandInitialize(); + SystemSettingsInitialize(); + StabilizationBankInitialize(); + + // reset integrals + resetIntegrals(); // Create object queue - pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + PathDesiredConnectCallback(SettingsUpdatedCb); + AirspeedStateConnectCallback(airspeedStateUpdatedCb); return 0; } @@ -122,17 +194,1021 @@ static void pathFollowerTask(void) FlightStatusData flightStatus; FlightStatusGet(&flightStatus); + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + resetIntegrals(); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; } - // do pathfollower things here + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move that shit into manualcontrol! + processPOI(); + } + + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + { + uint8_t result = updateAutoPilotByFrameType(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + } + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + PathStatusSet(&pathStatus); PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); } + static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - // read in settings (TODO) + FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); + PathDesiredGet(&pathDesired); +} + + +static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + AirspeedStateData airspeedState; + VelocityStateData velocityState; + + AirspeedStateGet(&airspeedState); + VelocityStateGet(&velocityState); + 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 - 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. +} + + +/** + * reset integrals + */ +static void resetIntegrals() +{ + i.vel[0] = 0.0f; + i.vel[1] = 0.0f; + i.vel[2] = 0.0f; + i.course = 0.0f; + i.speed = 0.0f; + i.power = 0.0f; + i.airspeed = 0.0f; + i.poiRadius = 0.0f; + i.vtolEmergencyFallback = 0; +} + +static uint8_t updateAutoPilotByFrameType() +{ + FrameType_t frameType = GetCurrentFrameType(); + + if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) { + switch (vtolPathFollowerSettings.TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + } + } + switch (frameType) { + case FRAME_TYPE_MULTIROTOR: + case FRAME_TYPE_HELI: + updatePeriod = vtolPathFollowerSettings.UpdatePeriod; + return updateAutoPilotVtol(); + + break; + case FRAME_TYPE_FIXED_WING: + default: + updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod; + return updateAutoPilotFixedWing(); + + break; + } +} + +/** + * fixed wing autopilot: + * straight forward: + * 1. update path velocity for limited motion crafts + * 2. update attitude according to default fixed wing pathfollower algorithm + */ +static uint8_t updateAutoPilotFixedWing() +{ + updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, fixedWingPathFollowerSettings.HorizontalPosP, fixedWingPathFollowerSettings.VerticalPosP, true); + return updateFixedDesiredAttitude(); +} + +/** + * vtol autopilot + * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure) + * fall back to emergency fallback autopilot to keep minimum amount of flight control + */ +static uint8_t updateAutoPilotVtol() +{ + if (!i.vtolEmergencyFallback) { + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + updateVtolDesiredAttitudeEmergencyFallback(); + return 1; + } else { + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, false); + uint8_t result = 1; + bool yaw_attitude = true; + float yaw = 0.0f; + switch (vtolPathFollowerSettings.YawControl) { + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + yaw_attitude = false; + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: + yaw = updateTailInBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_COURSE: + yaw = updateCourseBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: + yaw = updatePOIBearing(); + break; + } + updateVtolDesiredAttitude(yaw_attitude, yaw); + if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { + i.vtolEmergencyFallback = true; + } + return result; + } + } else { + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + updateVtolDesiredAttitudeEmergencyFallback(); + return 0; + } +} + + +/** + * Compute bearing of current takeoff location + */ +static float updateTailInBearing() +{ + PositionStateData p; + + PositionStateGet(&p); + TakeOffLocationData t; + TakeOffLocationGet(&t); + // atan2f always returns in between + and - 180 degrees + float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); + // result is in between 0 and 360 degrees + if (yaw < 0.0f) { + yaw += 360.0f; + } + return yaw; +} + +/** + * Compute bearing of current movement direction + */ +static float updateCourseBearing() +{ + VelocityStateData v; + + VelocityStateGet(&v); + // atan2f always returns in between + and - 180 degrees + float yaw = RAD2DEG(atan2f(v.East, v.North)); + // result is in between 0 and 360 degrees + if (yaw < 0.0f) { + yaw += 360.0f; + } + return yaw; +} + +/** + * Compute bearing between current position and POI + */ +static float updatePOIBearing() +{ + PoiLocationData poi; + + PoiLocationGet(&poi); + PositionStateData positionState; + PositionStateGet(&positionState); + + float dT = updatePeriod / 1000.0f; + float dLoc[3]; + float yaw = 0; + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } + + return yaw + (pathAngle / 2.0f); +} + +/** + * process POI control logic TODO: this should most likely go into manualcontrol!!!! + * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently + **/ +static void processPOI() +{ + float dT = updatePeriod / 1000.0f; + + + PositionStateData positionState; + + PositionStateGet(&positionState); + // CameraDesiredData cameraDesired; + // CameraDesiredGet(&cameraDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + PoiLocationData poi; + PoiLocationGet(&poi); + + float dLoc[3]; + float yaw = 0; + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + + // distance + float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f)); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float changeRadius = 0; + // Move closer or further, radially + if (manualControlData.Pitch > DEADBAND_HIGH) { + changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; + } else if (manualControlData.Pitch < DEADBAND_LOW) { + changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; + } + + // move along circular path + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { + // change radius only when not circling + i.poiRadius = distance + changeRadius; + } + + // don't try to move any closer + if (i.poiRadius >= 3.0f || changeRadius > 0) { + if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { + pathDesired.End.North = poi.North + (i.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.East = poi.East + (i.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.StartingVelocity = 1.0f; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } + } + // not above + if (distance >= 3.0f) { + // You can feed this into camerastabilization + /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ + + // cameraDesired.Yaw=yaw; + // cameraDesired.PitchOrServo2=elevation; + + // CameraDesiredSet(&cameraDesired); + } +} + +/** + * Compute desired velocity from the current position and path + */ +static void updatePathVelocity(float kFF, float kH, float kV, bool limited) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; + struct path_status progress; + + path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), + cast_struct_to_array(pathDesired.End, pathDesired.End.North), + cur, &progress, pathDesired.Mode); + + float groundspeed; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + boundf(progress.fractional_progress, 0.0f, 1.0f); + break; + } + // make sure groundspeed is not zero + if (limited && groundspeed < 1e-6f) { + groundspeed = 1e-6f; + } + + // calculate velocity - can be zero if waypoints are too close + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_direction[0]; + velocityDesired.East = progress.path_direction[1]; + velocityDesired.Down = progress.path_direction[2]; + + float error_speed_horizontal = progress.error * kH; + float error_speed_vertical = progress.error * kV; + + if (limited && + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // 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 everything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + // calculating angles < 90 degrees through dot products + ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East + progress.path_direction[2] * velocityState.Down) < 0.0f) && + ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East + progress.correction_direction[2] * velocityState.Down) < 0.0f)) { + error_speed_horizontal = 0.0f; + error_speed_vertical = 0.0f; + } + + // calculate correction - can also be zero if correction vector is 0 or no error present + velocityDesired.North += progress.correction_direction[0] * error_speed_horizontal; + velocityDesired.East += progress.correction_direction[1] * error_speed_horizontal; + velocityDesired.Down += progress.correction_direction[2] * error_speed_vertical; + + // scale to correct length + float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down); + if (l > 1e-9f) { + velocityDesired.North *= groundspeed / l; + velocityDesired.East *= groundspeed / l; + velocityDesired.Down *= groundspeed / l; + } else { + velocityDesired.North = 0.0f; + velocityDesired.East = 0.0f; + velocityDesired.Down = 0.0f; + } + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.path_direction_north = progress.path_direction[0]; + pathStatus.path_direction_east = progress.path_direction[1]; + pathStatus.path_direction_down = progress.path_direction[2]; + + pathStatus.correction_direction_north = progress.correction_direction[0]; + pathStatus.correction_direction_east = progress.correction_direction[1]; + pathStatus.correction_direction_down = progress.correction_direction[2]; + + VelocityDesiredSet(&velocityDesired); +} + + +/** + * Compute desired attitude from the desired velocity for fixed wing craft + */ +static uint8_t updateFixedDesiredAttitude() +{ + uint8_t result = 1; + + const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s] + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + FixedWingPathFollowerStatusData fixedWingPathFollowerStatus; + AirspeedStateData airspeedState; + SystemSettingsData systemSettings; + + float groundspeedProjection; + float indicatedAirspeedState; + float indicatedAirspeedDesired; + float airspeedError; + + float pitchCommand; + + float descentspeedDesired; + float descentspeedError; + float powerCommand; + + float airspeedVector[2]; + float fluidMovement[2]; + float courseComponent[2]; + float courseError; + float courseCommand; + + FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus); + + VelocityStateGet(&velocityState); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + AirspeedStateGet(&airspeedState); + SystemSettingsGet(&systemSettings); + + /** + * 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 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 = groundspeedProjection + 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; + + // Vertical speed error + descentspeedDesired = boundf( + velocityDesired.Down, + -fixedWingPathFollowerSettings.VerticalVelMax, + fixedWingPathFollowerSettings.VerticalVelMax); + descentspeedError = descentspeedDesired - velocityState.Down; + + // Error condition: plane too slow or too fast + fixedWingPathFollowerStatus.Errors.Highspeed = 0; + fixedWingPathFollowerStatus.Errors.Lowspeed = 0; + if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) { + fixedWingPathFollowerStatus.Errors.Overspeed = 1; + result = 0; + } + if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) { + fixedWingPathFollowerStatus.Errors.Highspeed = 1; + result = 0; + } + if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) { + fixedWingPathFollowerStatus.Errors.Lowspeed = 1; + result = 0; + } + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) { + fixedWingPathFollowerStatus.Errors.Stallspeed = 1; + result = 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.0f) { + i.power = boundf(i.power + -descentspeedError * dT, + -fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki, + fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki + ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); + } else { + i.power = 0.0f; + } + + // Compute the cross feed from vertical speed to pitch, with saturation + float speedErrorToPowerCommandComponent = boundf( + (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp, + -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max, + fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max + ); + + // Compute final thrust response + powerCommand = -descentspeedError * fixedWingPathFollowerSettings.PowerPI.Kp + + i.power * fixedWingPathFollowerSettings.PowerPI.Ki + + speedErrorToPowerCommandComponent; + + // Output internal state to telemetry + fixedWingPathFollowerStatus.Error.Power = descentspeedError; + fixedWingPathFollowerStatus.ErrorInt.Power = i.power; + fixedWingPathFollowerStatus.Command.Power = powerCommand; + + // set thrust + stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand, + fixedWingPathFollowerSettings.ThrustLimit.Min, + fixedWingPathFollowerSettings.ThrustLimit.Max); + + // 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.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; + } + // 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.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; + } + + + /** + * Compute desired pitch command + */ + if (fixedWingPathFollowerSettings.SpeedPI.Ki > 0) { + // Integrate with saturation + i.airspeed = boundf(i.airspeed + airspeedError * dT, + -fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki, + fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki); + } + + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp, + -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max, + fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max + ); + + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -(airspeedError * fixedWingPathFollowerSettings.SpeedPI.Kp + + i.airspeed * fixedWingPathFollowerSettings.SpeedPI.Ki + ) + verticalSpeedToPitchCommandComponent; + + fixedWingPathFollowerStatus.Error.Speed = airspeedError; + fixedWingPathFollowerStatus.ErrorInt.Speed = i.airspeed; + fixedWingPathFollowerStatus.Command.Speed = pitchCommand; + + stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand, + fixedWingPathFollowerSettings.PitchLimit.Min, + fixedWingPathFollowerSettings.PitchLimit.Max); + + // 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.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; + } + + /** + * Compute desired roll command + */ + courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + 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; + } + + i.course = boundf(i.course + courseError * dT * fixedWingPathFollowerSettings.CoursePI.Ki, + -fixedWingPathFollowerSettings.CoursePI.ILimit, + fixedWingPathFollowerSettings.CoursePI.ILimit); + courseCommand = (courseError * fixedWingPathFollowerSettings.CoursePI.Kp + + i.course); + + fixedWingPathFollowerStatus.Error.Course = courseError; + fixedWingPathFollowerStatus.ErrorInt.Course = i.course; + fixedWingPathFollowerStatus.Command.Course = courseCommand; + + stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral + + courseCommand, + fixedWingPathFollowerSettings.RollLimit.Min, + fixedWingPathFollowerSettings.RollLimit.Max); + + // TODO: find a check to determine loss of directional control. Likely needs some check of derivative + + + /** + * Compute desired yaw command + */ + // TODO implement raw control mode for yaw and base on Accels.Y + stabDesired.Yaw = 0.0f; + + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + + StabilizationDesiredSet(&stabDesired); + + FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus); + + return result; +} + + +/** + * 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 = vector_lengthf(F, 2); + + // normalize Cn=V/|V|, |V| must be >0 + float v = vector_lengthf(V, 2); + 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; + } +} + +/** + * Compute desired attitude from the desired velocity + * + * Takes in @ref NedState which has the acceleration in the + * NED frame as the feedback term and then compares the + * @ref VelocityState against the @ref VelocityDesired + */ +static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) +{ + float dT = updatePeriod / 1000.0f; + uint8_t result = 1; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + SystemSettingsData systemSettings; + + float northError; + float northCommand; + + float eastError; + float eastCommand; + + float downError; + float downCommand; + + SystemSettingsGet(&systemSettings); + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + // Testing code - refactor into manual control command + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + // Compute desired north command + northError = velocityDesired.North - velocityState.North; + i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, + -vtolPathFollowerSettings.HorizontalVelPI.ILimit, + vtolPathFollowerSettings.HorizontalVelPI.ILimit); + northCommand = (northError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[0] + + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward); + + // Compute desired east command + eastError = velocityDesired.East - velocityState.East; + i.vel[1] = boundf(i.vel[1] + eastError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, + -vtolPathFollowerSettings.HorizontalVelPI.ILimit, + vtolPathFollowerSettings.HorizontalVelPI.ILimit); + eastCommand = (eastError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[1] + + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward); + + // Compute desired down command + downError = velocityDesired.Down - velocityState.Down; + // Must flip this sign + downError = -downError; + i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, + -vtolPathFollowerSettings.VerticalVelPI.ILimit, + vtolPathFollowerSettings.VerticalVelPI.ILimit); + downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + + stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + + + // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) { + attitudeState.Yaw += 120.0f; + if (attitudeState.Yaw > 180.0f) { + attitudeState.Yaw -= 360.0f; + } + } + + if ( // emergency flyaway detection + (fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit + velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees + ) { + result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + } + + // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the + // craft should move similarly for 5 deg roll versus 5 deg pitch + stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), + -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), + -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + + if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Thrust = manualControl.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + if (yaw_attitude) { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Yaw = yaw_direction; + } else { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; + } + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); + + return result; +} + +/** + * Compute desired attitude for vtols - emergency fallback + */ +static void updateVtolDesiredAttitudeEmergencyFallback() +{ + float dT = updatePeriod / 1000.0f; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + + float courseError; + float courseCommand; + + float downError; + float downCommand; + + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + + courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP); + + stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max); + + // Compute desired down command + downError = velocityDesired.Down - velocityState.Down; + // Must flip this sign + downError = -downError; + i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, + -vtolPathFollowerSettings.VerticalVelPI.ILimit, + vtolPathFollowerSettings.VerticalVelPI.ILimit); + downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + + stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + + + stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll; + stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch; + + if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Thrust = manualControl.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); +} + + +/** + * Compute desired attitude from a fixed preset + * + */ +static void updateFixedAttitude(float *attitude) +{ + StabilizationDesiredData stabDesired; + + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Thrust = attitude[3]; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + StabilizationDesiredSet(&stabDesired); } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 7cb3f27db..8d8b753b7 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -372,6 +372,7 @@ static void updatePathVelocity() float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionStateData positionState; PositionStateGet(&positionState); @@ -403,7 +404,7 @@ static void updatePathVelocity() case PATHDESIRED_MODE_DRIVEVECTOR: default: speed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); + + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { speed = 0; } @@ -416,19 +417,19 @@ static void updatePathVelocity() eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT; - northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); + northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral + - progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral + - progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral + - progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; + progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; // Make sure the desired velocities don't exceed PathFollower limits. float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2)); @@ -441,7 +442,7 @@ static void updatePathVelocity() velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; pathStatus.path_direction_north = progress.path_direction[0]; pathStatus.path_direction_east = progress.path_direction[1]; diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 8a7d9991a..da7ebe4fa 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -1,18 +1,23 @@ Settings for the @ref VtolPathFollowerModule - - - - - - - + + + + + + + + + - - + + + + + - + From ea4e7962a67fba3c0d8476598fa1e2ad39e8a73f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 21:17:55 +0200 Subject: [PATCH 13/25] OP-1156 fixed wrong callback ID assignment for pathfollower callback diagnostics --- flight/modules/PathFollower/pathfollower.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 93efab3c9..b5d8e9e11 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -175,7 +175,7 @@ int32_t PathFollowerInitialize() resetIntegrals(); // Create object queue - pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); From 56e4127bca5c9b697fc0d32ea37de20c51960849 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 12 Aug 2014 18:18:07 +0200 Subject: [PATCH 14/25] OP-1156: add path_direction as a source for yaw as suggested by Werner :) --- flight/modules/PathFollower/pathfollower.c | 33 +++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index b5d8e9e11..0ffa2c741 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -122,6 +122,7 @@ static uint8_t updateAutoPilotFixedWing(); static uint8_t updateAutoPilotVtol(); static float updateTailInBearing(); static float updateCourseBearing(); +static float updatePathBearing(); static float updatePOIBearing(); static void processPOI(); static void updatePathVelocity(float kFF, float kH, float kV, bool limited); @@ -355,9 +356,12 @@ static uint8_t updateAutoPilotVtol() case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: yaw = updateTailInBearing(); break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_COURSE: + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: yaw = updateCourseBearing(); break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: + yaw = updatePathBearing(); + break; case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: yaw = updatePOIBearing(); break; @@ -412,6 +416,33 @@ static float updateCourseBearing() return yaw; } +/** + * Compute bearing of current path direction + */ +static float updatePathBearing() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + + float cur[3] = { positionState.North, + positionState.East, + positionState.Down }; + struct path_status progress; + + path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), + cast_struct_to_array(pathDesired.End, pathDesired.End.North), + cur, &progress, pathDesired.Mode); + + // atan2f always returns in between + and - 180 degrees + float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0])); + // result is in between 0 and 360 degrees + if (yaw < 0.0f) { + yaw += 360.0f; + } + return yaw; +} + /** * Compute bearing between current position and POI */ From 4fc071978c694f2f40d656d93f64d266ab842e81 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 12 Aug 2014 18:23:45 +0200 Subject: [PATCH 15/25] OP-1156 commit changed xml file --- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index da7ebe4fa..ebc6d9520 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -12,7 +12,7 @@ - + From 09e8b384070a3068580be55f3f1f47b77950b508 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 12 Aug 2014 19:12:05 +0200 Subject: [PATCH 16/25] OP-1156 fixed EmergencyFlyaway detection algorithm --- flight/modules/PathFollower/pathfollower.c | 29 ++++++++++++++----- .../vtolpathfollowersettings.xml | 1 + 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 0ffa2c741..6db2c2759 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -96,7 +96,8 @@ struct Integrals { float power; float airspeed; float poiRadius; - bool vtolEmergencyFallback; + float vtolEmergencyFallback; + bool vtolEmergencyFallbackSwitch; }; @@ -288,6 +289,7 @@ static void resetIntegrals() i.airspeed = 0.0f; i.poiRadius = 0.0f; i.vtolEmergencyFallback = 0; + i.vtolEmergencyFallbackSwitch = false; } static uint8_t updateAutoPilotByFrameType() @@ -339,7 +341,7 @@ static uint8_t updateAutoPilotFixedWing() */ static uint8_t updateAutoPilotVtol() { - if (!i.vtolEmergencyFallback) { + if (!i.vtolEmergencyFallbackSwitch) { if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); updateVtolDesiredAttitudeEmergencyFallback(); @@ -366,9 +368,9 @@ static uint8_t updateAutoPilotVtol() yaw = updatePOIBearing(); break; } - updateVtolDesiredAttitude(yaw_attitude, yaw); + result = updateVtolDesiredAttitude(yaw_attitude, yaw); if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { - i.vtolEmergencyFallback = true; + i.vtolEmergencyFallbackSwitch = true; } return result; } @@ -1120,10 +1122,23 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) } if ( // emergency flyaway detection - (fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit - velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees + ( // integral already at its limit + vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[0]) < 1e-6f || + vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[1]) < 1e-6f + ) && + // angle between desired and actual velocity >90 degrees (by dot product) + (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && + // quad is moving at significant speed (during flyaway it would keep speeding up) + squaref(velocityState.North) + squaref(velocityState.East) > 1.0f ) { - result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + i.vtolEmergencyFallback += dT; + if (i.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { + // after emergency timeout, trigger alarm - everything else is handled by callers + // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + result = 0; + } + } else { + i.vtolEmergencyFallback = 0.0f; } // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index ebc6d9520..7da8f9091 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -14,6 +14,7 @@ + From b562fcb02ef50d88ddd4ef419a6428170a82c126 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 15 Aug 2014 17:56:57 +0200 Subject: [PATCH 17/25] OP-1156 fix path logic to not deviate from correct altitude too much --- flight/libraries/inc/paths.h | 6 +- flight/libraries/paths.c | 182 ++++++++---------- flight/libraries/plans.c | 19 +- flight/modules/PathFollower/pathfollower.c | 85 +++----- flight/modules/PathPlanner/pathplanner.c | 10 +- .../flightmodesettings.xml | 2 - 6 files changed, 119 insertions(+), 185 deletions(-) diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index 4cecbe845..053b9b474 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -30,10 +30,10 @@ struct path_status { float fractional_progress; float error; - float correction_direction[3]; - float path_direction[3]; + float path_vector[3]; + float correction_vector[3]; }; -void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status); #endif diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 6c80d0a9d..3bcd0a03d 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -28,54 +28,51 @@ #include #include -#include "paths.h" - #include "uavobjectmanager.h" // <--. #include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, +#include "paths.h" // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); -static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); +static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode); +static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode); +static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise); /** * @brief Compute progress along path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired structure * @param[in] cur_point Current location - * @param[in] mode Path following mode * @param[out] status Structure containing progress along path and deviation */ -void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode) +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status) { - switch (mode) { + switch (path->Mode) { case PATHDESIRED_MODE_FLYVECTOR: - return path_vector(start_point, end_point, cur_point, status, true); + return path_vector(path, cur_point, status, true); break; case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status, false); + return path_vector(path, cur_point, status, false); break; case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(start_point, end_point, cur_point, status, 1); + return path_circle(path, cur_point, status, 1); break; case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(start_point, end_point, cur_point, status, 0); + return path_circle(path, cur_point, status, 0); break; case PATHDESIRED_MODE_FLYENDPOINT: - return path_endpoint(start_point, end_point, cur_point, status, true); + return path_endpoint(path, cur_point, status, true); break; case PATHDESIRED_MODE_DRIVEENDPOINT: default: // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status, false); + return path_endpoint(path, cur_point, status, false); break; } @@ -83,127 +80,120 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc /** * @brief Compute progress towards endpoint. Deviation equals distance - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) +static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { - float path[3], diff[3]; + float diff[3]; float dist_path, dist_diff; - // we do not correct in this mode - status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0; - // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + status->path_vector[0] = path->End.North - path->Start.North; + status->path_vector[1] = path->End.East - path->Start.East; + status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f; // Current progress location relative to end - diff[0] = end_point[0] - cur_point[0]; - diff[1] = end_point[1] - cur_point[1]; - diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; + diff[0] = path->End.North - cur_point[0]; + diff[1] = path->End.East - cur_point[1]; + diff[2] = mode3D ? path->End.Down - cur_point[2] : 0.0f; dist_diff = vector_lengthf(diff, 3); - dist_path = vector_lengthf(path, 3); + dist_path = vector_lengthf(status->path_vector, 3); if (dist_diff < 1e-6f) { - status->fractional_progress = 1; - status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; - status->path_direction[2] = 0; + status->fractional_progress = 1; + status->error = 0.0f; + status->correction_vector[0] = status->correction_vector[1] = status->correction_vector[2] = 0.0f; + // we have no base movement direction in this mode + status->path_vector[0] = status->path_vector[1] = status->path_vector[2] = 0.0f; + return; } - if (dist_path + 1 > dist_diff) { - status->fractional_progress = 1 - dist_diff / (1 + dist_path); + if (fmaxf(dist_path, 1.0f) > dist_diff) { + status->fractional_progress = 1 - dist_diff / fmaxf(dist_path, 1.0f); } else { status->fractional_progress = 0; // we don't want fractional_progress to become negative } status->error = dist_diff; - // Compute direction to travel - status->path_direction[0] = diff[0] / dist_diff; - status->path_direction[1] = diff[1] / dist_diff; - status->path_direction[2] = diff[2] / dist_diff; + // Compute correction vector + status->correction_vector[0] = diff[0]; + status->correction_vector[1] = diff[1]; + status->correction_vector[2] = diff[2]; + + // base movement direction in this mode is a constant velocity offset on top of correction in the same direction + status->path_vector[0] = path->EndingVelocity * status->correction_vector[0] / dist_diff; + status->path_vector[1] = path->EndingVelocity * status->correction_vector[1] / dist_diff; + status->path_vector[2] = path->EndingVelocity * status->correction_vector[2] / dist_diff; } /** * @brief Compute progress along path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) +static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { - float path[3], diff[3]; + float diff[3]; float dist_path; float dot; + float velocity; float track_point[3]; // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + status->path_vector[0] = path->End.North - path->Start.North; + status->path_vector[1] = path->End.East - path->Start.East; + status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f; // Current progress location relative to start - diff[0] = cur_point[0] - start_point[0]; - diff[1] = cur_point[1] - start_point[1]; - diff[2] = mode3D ? cur_point[2] - start_point[2] : 0; + diff[0] = cur_point[0] - path->Start.North; + diff[1] = cur_point[1] - path->Start.East; + diff[2] = mode3D ? cur_point[2] - path->Start.Down : 0.0f; - dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; - dist_path = vector_lengthf(path, 3); + dot = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2]; + dist_path = vector_lengthf(status->path_vector, 3); if (dist_path > 1e-6f) { // Compute direction to travel & progress - status->path_direction[0] = path[0] / dist_path; - status->path_direction[1] = path[1] / dist_path; - status->path_direction[2] = path[2] / dist_path; status->fractional_progress = dot / (dist_path * dist_path); } else { // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status, mode3D); + path_endpoint(path, cur_point, status, mode3D); status->fractional_progress = 1; return; } - // Compute point on track that is closest to our current position. - track_point[0] = status->fractional_progress * path[0] + start_point[0]; - track_point[1] = status->fractional_progress * path[1] + start_point[1]; - track_point[2] = status->fractional_progress * path[2] + start_point[2]; + track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North; + track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East; + track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down; - status->correction_direction[0] = track_point[0] - cur_point[0]; - status->correction_direction[1] = track_point[1] - cur_point[1]; - status->correction_direction[2] = track_point[2] - cur_point[2]; + status->correction_vector[0] = track_point[0] - cur_point[0]; + status->correction_vector[1] = track_point[1] - cur_point[1]; + status->correction_vector[2] = track_point[2] - cur_point[2]; - status->error = vector_lengthf(status->correction_direction, 3); + status->error = vector_lengthf(status->correction_vector, 3); - // Normalize correction_direction but avoid division by zero - if (status->error > 1e-6f) { - status->correction_direction[0] /= status->error; - status->correction_direction[1] /= status->error; - status->correction_direction[2] /= status->error; - } else { - status->correction_direction[0] = 0; - status->correction_direction[1] = 0; - status->correction_direction[2] = 0; - } + // correct movement vector to current velocity + velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity); + status->path_vector[0] = velocity * status->path_vector[0] / dist_path; + status->path_vector[1] = velocity * status->path_vector[1] / dist_path; + status->path_vector[2] = velocity * status->path_vector[2] / dist_path; } /** * @brief Compute progress along circular path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Center point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) +static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise) { float radius_north, radius_east, diff_north, diff_east, diff_down; float radius, cradius; @@ -212,30 +202,30 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, float a_diff, a_radius; // Radius - radius_north = end_point[0] - start_point[0]; - radius_east = end_point[1] - start_point[1]; + radius_north = path->End.North - path->Start.North; + radius_east = path->End.East - path->Start.East; // Current location relative to center - diff_north = cur_point[0] - end_point[0]; - diff_east = cur_point[1] - end_point[1]; - diff_down = cur_point[2] - end_point[2]; + diff_north = cur_point[0] - path->End.North; + diff_east = cur_point[1] - path->End.East; + diff_down = cur_point[2] - path->End.Down; radius = sqrtf(squaref(radius_north) + squaref(radius_east)); cradius = sqrtf(squaref(diff_north) + squaref(diff_east)); // circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply) - status->path_direction[2] = 0.0f; + status->path_vector[2] = 0.0f; // error is current radius minus wanted radius - positive if too close status->error = radius - cradius; if (cradius < 1e-6f) { - // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; - status->correction_direction[0] = 0; - status->correction_direction[1] = 1; - status->path_direction[0] = 1; - status->path_direction[1] = 0; + // cradius is zero, just fly somewhere + status->fractional_progress = 1; + status->correction_vector[0] = 0; + status->correction_vector[1] = 0; + status->path_vector[0] = path->EndingVelocity; + status->path_vector[1] = 0; } else { if (clockwise) { // Compute the normal to the radius clockwise @@ -270,20 +260,18 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, progress = 1.0f - progress; } - status->fractional_progress = progress; + status->fractional_progress = progress; // Compute direction to travel - status->path_direction[0] = normal[0]; - status->path_direction[1] = normal[1]; + status->path_vector[0] = normal[0] * path->EndingVelocity; + status->path_vector[1] = normal[1] * path->EndingVelocity; // Compute direction to correct error - status->correction_direction[0] = status->error * diff_north / cradius; - status->correction_direction[1] = status->error * diff_east / cradius; + status->correction_vector[0] = status->error * diff_north / cradius; + status->correction_vector[1] = status->error * diff_east / cradius; } - status->correction_direction[2] = -diff_down; - - vector_normalizef(status->correction_direction, 3); + status->correction_vector[2] = -diff_down; status->error = fabs(status->error); } diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 4df2a9289..0faac2a2a 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -70,8 +70,6 @@ void plan_setup_positionHold() FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; @@ -79,7 +77,7 @@ void plan_setup_positionHold() 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.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -110,8 +108,6 @@ void plan_setup_returnToBase() destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = takeoffLocation.North; pathDesired.End.East = takeoffLocation.East; @@ -121,7 +117,7 @@ void plan_setup_returnToBase() pathDesired.Start.East = takeoffLocation.East; pathDesired.Start.Down = destDown; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -131,15 +127,12 @@ void plan_setup_returnToBase() static PiOSDeltatimeConfig landdT; void plan_setup_land() { - float descendspeed; - plan_setup_positionHold(); - FlightModeSettingsLandingVelocityGet(&descendspeed); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.StartingVelocity = descendspeed; - pathDesired.EndingVelocity = descendspeed; + pathDesired.StartingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; PathDesiredSet(&pathDesired); PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } @@ -383,8 +376,6 @@ void plan_setup_AutoCruise() 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 @@ -404,7 +395,7 @@ void plan_setup_AutoCruise() 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.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 6db2c2759..96ee78a9c 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -62,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -432,12 +432,10 @@ static float updatePathBearing() positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, cur, &progress); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0])); + float yaw = RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); // result is in between 0 and 360 degrees if (yaw < 0.0f) { yaw += 360.0f; @@ -581,40 +579,13 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) positionState.Down + (velocityState.Down * kFF) }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); - - float groundspeed; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - boundf(progress.fractional_progress, 0.0f, 1.0f); - break; - } - // make sure groundspeed is not zero - if (limited && groundspeed < 1e-6f) { - groundspeed = 1e-6f; - } + path_progress(&pathDesired, cur, &progress); // calculate velocity - can be zero if waypoints are too close VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0]; - velocityDesired.East = progress.path_direction[1]; - velocityDesired.Down = progress.path_direction[2]; - - float error_speed_horizontal = progress.error * kH; - float error_speed_vertical = progress.error * kV; + velocityDesired.North = progress.path_vector[0]; + velocityDesired.East = progress.path_vector[1]; + velocityDesired.Down = progress.path_vector[2]; if (limited && // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) @@ -629,39 +600,27 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) // 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) // calculating angles < 90 degrees through dot products - ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East + progress.path_direction[2] * velocityState.Down) < 0.0f) && - ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East + progress.correction_direction[2] * velocityState.Down) < 0.0f)) { - error_speed_horizontal = 0.0f; - error_speed_vertical = 0.0f; - } - - // calculate correction - can also be zero if correction vector is 0 or no error present - velocityDesired.North += progress.correction_direction[0] * error_speed_horizontal; - velocityDesired.East += progress.correction_direction[1] * error_speed_horizontal; - velocityDesired.Down += progress.correction_direction[2] * error_speed_vertical; - - // scale to correct length - float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down); - if (l > 1e-9f) { - velocityDesired.North *= groundspeed / l; - velocityDesired.East *= groundspeed / l; - velocityDesired.Down *= groundspeed / l; + (vector_lengthf(progress.path_vector, 2) > 1e-6f) && + ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && + ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { + ; } else { - velocityDesired.North = 0.0f; - velocityDesired.East = 0.0f; - velocityDesired.Down = 0.0f; + // calculate correction + velocityDesired.North += progress.correction_vector[0] * kH; + velocityDesired.East += progress.correction_vector[1] * kH; } + velocityDesired.Down += progress.correction_vector[2] * kV; // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; - pathStatus.path_direction_north = progress.path_direction[0]; - pathStatus.path_direction_east = progress.path_direction[1]; - pathStatus.path_direction_down = progress.path_direction[2]; + pathStatus.path_direction_north = progress.path_vector[0]; + pathStatus.path_direction_east = progress.path_vector[1]; + pathStatus.path_direction_down = progress.path_vector[2]; - pathStatus.correction_direction_north = progress.correction_direction[0]; - pathStatus.correction_direction_east = progress.correction_direction[1]; - pathStatus.correction_direction_down = progress.correction_direction[2]; + pathStatus.correction_direction_north = progress.correction_vector[0]; + pathStatus.correction_direction_east = progress.correction_vector[1]; + pathStatus.correction_direction_down = progress.correction_vector[2]; VelocityDesiredSet(&velocityDesired); } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 745a8c17e..32d2afbae 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -525,9 +525,8 @@ static uint8_t conditionLegRemaining() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, + cur, &progress); if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { return true; } @@ -550,9 +549,8 @@ static uint8_t conditionBelowError() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, + cur, &progress); if (progress.error <= pathAction.ConditionParameters[0]) { return true; } diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 728fc27c0..5511345ae 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -114,8 +114,6 @@ - - From b1675a2804bafb8bec0d1a94425fd2c99a16a4df Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 15 Aug 2014 19:23:41 +0200 Subject: [PATCH 18/25] OP-1156 added vtolpathfollower velocity limits to attitude control --- flight/modules/PathFollower/pathfollower.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 96ee78a9c..6399fe6b1 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -1044,6 +1044,16 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) ManualControlCommandData manualControlData; ManualControlCommandGet(&manualControlData); + // scale velocity if it is above configured maximum + float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + if (velH > vtolPathFollowerSettings.HorizontalVelMax) { + velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH; + velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH; + } + if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) { + velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down); + } + // Compute desired north command northError = velocityDesired.North - velocityState.North; i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, From 915c24c8b6902aecb269c0329fdb01dabe9f4c0c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 15 Aug 2014 20:36:14 +0200 Subject: [PATCH 19/25] OP-1156 changed PID control loops to use generic pid library, readded unnecessary integral and derivative terms to vtolpathfollower --- flight/modules/PathFollower/pathfollower.c | 189 +++++++++--------- .../fixedwingpathfollowersettings.xml | 8 +- .../vtolpathfollowersettings.xml | 14 +- 3 files changed, 101 insertions(+), 110 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 6399fe6b1..69e3cc7aa 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -89,12 +89,13 @@ #define DEADBAND_LOW -0.10f // Private types -struct Integrals { - float vel[3]; - float course; - float speed; - float power; - float airspeed; +struct Globals { + struct pid PIDposH[2]; + struct pid PIDposV; + struct pid PIDvel[3]; + struct pid PIDcourse; + struct pid PIDspeed; + struct pid PIDpower; float poiRadius; float vtolEmergencyFallback; bool vtolEmergencyFallbackSwitch; @@ -104,7 +105,7 @@ struct Integrals { // Private variables static DelayedCallbackInfo *pathFollowerCBInfo; static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; -static struct Integrals i; +static struct Globals global; static PathStatusData pathStatus; static PathDesiredData pathDesired; static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; @@ -116,7 +117,7 @@ static float indicatedAirspeedStateBias = 0.0f; // Private functions static void pathFollowerTask(void); -static void resetIntegrals(); +static void resetGlobals(); static void SettingsUpdatedCb(UAVObjEvent *ev); static uint8_t updateAutoPilotByFrameType(); static uint8_t updateAutoPilotFixedWing(); @@ -126,7 +127,7 @@ static float updateCourseBearing(); static float updatePathBearing(); static float updatePOIBearing(); static void processPOI(); -static void updatePathVelocity(float kFF, float kH, float kV, bool limited); +static void updatePathVelocity(float kFF, bool limited); static uint8_t updateFixedDesiredAttitude(); static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); static void updateFixedAttitude(); @@ -174,7 +175,7 @@ int32_t PathFollowerInitialize() StabilizationBankInitialize(); // reset integrals - resetIntegrals(); + resetGlobals(); // Create object queue pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); @@ -198,7 +199,7 @@ static void pathFollowerTask(void) FlightStatusGet(&flightStatus); if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - resetIntegrals(); + resetGlobals(); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; @@ -246,7 +247,19 @@ static void pathFollowerTask(void) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); + + pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit); + pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit); + pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit); + + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); + + pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); + pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); + pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); + pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); + PathDesiredGet(&pathDesired); } @@ -278,18 +291,20 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) /** * reset integrals */ -static void resetIntegrals() +static void resetGlobals() { - i.vel[0] = 0.0f; - i.vel[1] = 0.0f; - i.vel[2] = 0.0f; - i.course = 0.0f; - i.speed = 0.0f; - i.power = 0.0f; - i.airspeed = 0.0f; - i.poiRadius = 0.0f; - i.vtolEmergencyFallback = 0; - i.vtolEmergencyFallbackSwitch = false; + pid_zero(&global.PIDposH[0]); + pid_zero(&global.PIDposH[1]); + pid_zero(&global.PIDposV); + pid_zero(&global.PIDvel[0]); + pid_zero(&global.PIDvel[1]); + pid_zero(&global.PIDvel[2]); + pid_zero(&global.PIDcourse); + pid_zero(&global.PIDspeed); + pid_zero(&global.PIDpower); + global.poiRadius = 0.0f; + global.vtolEmergencyFallback = 0; + global.vtolEmergencyFallbackSwitch = false; } static uint8_t updateAutoPilotByFrameType() @@ -330,7 +345,10 @@ static uint8_t updateAutoPilotByFrameType() */ static uint8_t updateAutoPilotFixedWing() { - updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, fixedWingPathFollowerSettings.HorizontalPosP, fixedWingPathFollowerSettings.VerticalPosP, true); + pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f); + updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true); return updateFixedDesiredAttitude(); } @@ -341,13 +359,19 @@ static uint8_t updateAutoPilotFixedWing() */ static uint8_t updateAutoPilotVtol() { - if (!i.vtolEmergencyFallbackSwitch) { + if (!global.vtolEmergencyFallbackSwitch) { if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); updateVtolDesiredAttitudeEmergencyFallback(); return 1; } else { - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, false); + pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false); uint8_t result = 1; bool yaw_attitude = true; float yaw = 0.0f; @@ -370,12 +394,15 @@ static uint8_t updateAutoPilotVtol() } result = updateVtolDesiredAttitude(yaw_attitude, yaw); if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { - i.vtolEmergencyFallbackSwitch = true; + global.vtolEmergencyFallbackSwitch = true; } return result; } } else { - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); updateVtolDesiredAttitudeEmergencyFallback(); return 0; } @@ -454,7 +481,7 @@ static float updatePOIBearing() PositionStateData positionState; PositionStateGet(&positionState); - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; float dLoc[3]; float yaw = 0; /*float elevation = 0;*/ @@ -487,7 +514,7 @@ static float updatePOIBearing() **/ static void processPOI() { - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; PositionStateData positionState; @@ -536,14 +563,14 @@ static void processPOI() pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { // change radius only when not circling - i.poiRadius = distance + changeRadius; + global.poiRadius = distance + changeRadius; } // don't try to move any closer - if (i.poiRadius >= 3.0f || changeRadius > 0) { + if (global.poiRadius >= 3.0f || changeRadius > 0) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End.North = poi.North + (i.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End.East = poi.East + (i.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.StartingVelocity = 1.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -565,7 +592,7 @@ static void processPOI() /** * Compute desired velocity from the current position and path */ -static void updatePathVelocity(float kFF, float kH, float kV, bool limited) +static void updatePathVelocity(float kFF, bool limited) { PositionStateData positionState; @@ -573,10 +600,12 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) VelocityStateData velocityState; VelocityStateGet(&velocityState); + const float dT = updatePeriod / 1000.0f; + // look ahead kFF seconds - float cur[3] = { positionState.North + (velocityState.North * kFF), - positionState.East + (velocityState.East * kFF), - positionState.Down + (velocityState.Down * kFF) }; + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; struct path_status progress; path_progress(&pathDesired, cur, &progress); @@ -606,10 +635,10 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) ; } else { // calculate correction - velocityDesired.North += progress.correction_vector[0] * kH; - velocityDesired.East += progress.correction_vector[1] * kH; + velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT); + velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT); } - velocityDesired.Down += progress.correction_vector[2] * kV; + velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT); // update pathstatus pathStatus.error = progress.error; @@ -750,15 +779,6 @@ static uint8_t updateFixedDesiredAttitude() /** * 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.0f) { - i.power = boundf(i.power + -descentspeedError * dT, - -fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki, - fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki - ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); - } else { - i.power = 0.0f; - } // Compute the cross feed from vertical speed to pitch, with saturation float speedErrorToPowerCommandComponent = boundf( @@ -768,13 +788,12 @@ static uint8_t updateFixedDesiredAttitude() ); // Compute final thrust response - powerCommand = -descentspeedError * fixedWingPathFollowerSettings.PowerPI.Kp + - i.power * fixedWingPathFollowerSettings.PowerPI.Ki + + powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) + speedErrorToPowerCommandComponent; // Output internal state to telemetry fixedWingPathFollowerStatus.Error.Power = descentspeedError; - fixedWingPathFollowerStatus.ErrorInt.Power = i.power; + fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator; fixedWingPathFollowerStatus.Command.Power = powerCommand; // set thrust @@ -803,17 +822,9 @@ static uint8_t updateFixedDesiredAttitude() result = 0; } - /** * Compute desired pitch command */ - if (fixedWingPathFollowerSettings.SpeedPI.Ki > 0) { - // Integrate with saturation - i.airspeed = boundf(i.airspeed + airspeedError * dT, - -fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki, - fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki); - } - // Compute the cross feed from vertical speed to pitch, with saturation float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp, -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max, @@ -821,12 +832,10 @@ static uint8_t updateFixedDesiredAttitude() ); // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -(airspeedError * fixedWingPathFollowerSettings.SpeedPI.Kp - + i.airspeed * fixedWingPathFollowerSettings.SpeedPI.Ki - ) + verticalSpeedToPitchCommandComponent; + pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; fixedWingPathFollowerStatus.Error.Speed = airspeedError; - fixedWingPathFollowerStatus.ErrorInt.Speed = i.airspeed; + fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator; fixedWingPathFollowerStatus.Command.Speed = pitchCommand; stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand, @@ -869,14 +878,10 @@ static uint8_t updateFixedDesiredAttitude() courseError -= 360.0f; } - i.course = boundf(i.course + courseError * dT * fixedWingPathFollowerSettings.CoursePI.Ki, - -fixedWingPathFollowerSettings.CoursePI.ILimit, - fixedWingPathFollowerSettings.CoursePI.ILimit); - courseCommand = (courseError * fixedWingPathFollowerSettings.CoursePI.Kp + - i.course); + courseCommand = pid_apply(&global.PIDcourse, courseError, dT); fixedWingPathFollowerStatus.Error.Course = courseError; - fixedWingPathFollowerStatus.ErrorInt.Course = i.course; + fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator; fixedWingPathFollowerStatus.Command.Course = courseCommand; stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral + @@ -1013,7 +1018,7 @@ static bool correctCourse(float *C, float *V, float *F, float s) */ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) { - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; uint8_t result = 1; VelocityDesiredData velocityDesired; @@ -1056,28 +1061,17 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) // Compute desired north command northError = velocityDesired.North - velocityState.North; - i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, - -vtolPathFollowerSettings.HorizontalVelPI.ILimit, - vtolPathFollowerSettings.HorizontalVelPI.ILimit); - northCommand = (northError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[0] - + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward); + northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward; // Compute desired east command - eastError = velocityDesired.East - velocityState.East; - i.vel[1] = boundf(i.vel[1] + eastError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, - -vtolPathFollowerSettings.HorizontalVelPI.ILimit, - vtolPathFollowerSettings.HorizontalVelPI.ILimit); - eastCommand = (eastError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[1] - + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward); + eastError = velocityDesired.East - velocityState.East; + eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward; // Compute desired down command - downError = velocityDesired.Down - velocityState.Down; + downError = velocityDesired.Down - velocityState.Down; // Must flip this sign - downError = -downError; - i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, - -vtolPathFollowerSettings.VerticalVelPI.ILimit, - vtolPathFollowerSettings.VerticalVelPI.ILimit); - downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + downError = -downError; + downCommand = pid_apply(&global.PIDvel[2], downError, dT); stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); @@ -1092,22 +1086,22 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) if ( // emergency flyaway detection ( // integral already at its limit - vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[0]) < 1e-6f || - vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[1]) < 1e-6f + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f ) && // angle between desired and actual velocity >90 degrees (by dot product) (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && // quad is moving at significant speed (during flyaway it would keep speeding up) squaref(velocityState.North) + squaref(velocityState.East) > 1.0f ) { - i.vtolEmergencyFallback += dT; - if (i.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { + global.vtolEmergencyFallback += dT; + if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { // after emergency timeout, trigger alarm - everything else is handled by callers // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) result = 0; } } else { - i.vtolEmergencyFallback = 0.0f; + global.vtolEmergencyFallback = 0.0f; } // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the @@ -1146,7 +1140,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) */ static void updateVtolDesiredAttitudeEmergencyFallback() { - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; VelocityDesiredData velocityDesired; VelocityStateData velocityState; @@ -1182,10 +1176,7 @@ static void updateVtolDesiredAttitudeEmergencyFallback() downError = velocityDesired.Down - velocityState.Down; // Must flip this sign downError = -downError; - i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, - -vtolPathFollowerSettings.VerticalVelPI.ILimit, - vtolPathFollowerSettings.VerticalVelPI.ILimit); - downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + downCommand = pid_apply(&global.PIDvel[2], downError, dT); stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index 83bc91e25..8a7e5d064 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -15,9 +15,9 @@ - + - + @@ -38,7 +38,7 @@ in relation to vertical speed error (absolute but including crossfeed) --> - + @@ -46,7 +46,7 @@ + defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" /> diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 7da8f9091..fec77cb55 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -2,22 +2,22 @@ Settings for the @ref VtolPathFollowerModule - + - - - - + + + + - + - + From 760ae118e997245e4139a22d3a034267a8d80240 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 16 Aug 2014 20:48:56 +0200 Subject: [PATCH 20/25] OP-1156 changed yaw math from interval [0..360] to [-180..180] --- flight/modules/PathFollower/pathfollower.c | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 69e3cc7aa..fda14d29f 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -420,12 +420,7 @@ static float updateTailInBearing() TakeOffLocationData t; TakeOffLocationGet(&t); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); - // result is in between 0 and 360 degrees - if (yaw < 0.0f) { - yaw += 360.0f; - } - return yaw; + return RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); } /** @@ -437,12 +432,7 @@ static float updateCourseBearing() VelocityStateGet(&v); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(v.East, v.North)); - // result is in between 0 and 360 degrees - if (yaw < 0.0f) { - yaw += 360.0f; - } - return yaw; + return RAD2DEG(atan2f(v.East, v.North)); } /** @@ -462,12 +452,7 @@ static float updatePathBearing() path_progress(&pathDesired, cur, &progress); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); - // result is in between 0 and 360 degrees - if (yaw < 0.0f) { - yaw += 360.0f; - } - return yaw; + return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); } /** From 779eb8d7729c0b575e965a41989e0f571fc8b0be Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 19 Aug 2014 23:14:46 +0200 Subject: [PATCH 21/25] OP-1156 placed Werners positionhold speed offset back in place --- flight/libraries/plans.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 0faac2a2a..2d009f9ea 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -127,12 +127,15 @@ void plan_setup_returnToBase() static PiOSDeltatimeConfig landdT; void plan_setup_land() { + float descendspeed; + plan_setup_positionHold(); + FlightModeSettingsLandingVelocityGet(&descendspeed); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.StartingVelocity = 0.0f; - pathDesired.EndingVelocity = 0.0f; + pathDesired.StartingVelocity = descendspeed; + pathDesired.EndingVelocity = descendspeed; PathDesiredSet(&pathDesired); PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } From 70460e593ec8eeccc344e7cc7be13416dc2cb5d7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 20 Aug 2014 15:29:29 +0200 Subject: [PATCH 22/25] OP-1156 fixes as suggested per review --- flight/modules/PathFollower/pathfollower.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index fda14d29f..1e69ed8b7 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -205,7 +205,7 @@ static void pathFollowerTask(void) return; } - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move that shit into manualcontrol! + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol! processPOI(); } @@ -258,7 +258,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); - pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); PathDesiredGet(&pathDesired); } @@ -505,6 +504,8 @@ static void processPOI() PositionStateData positionState; PositionStateGet(&positionState); + // TODO put commented out camera feature code back in place either + // permanently or optionally or remove it // CameraDesiredData cameraDesired; // CameraDesiredGet(&cameraDesired); StabilizationDesiredData stabDesired; @@ -514,6 +515,7 @@ static void processPOI() float dLoc[3]; float yaw = 0; + // TODO camera feature /*float elevation = 0;*/ dLoc[0] = positionState.North - poi.North; @@ -564,6 +566,7 @@ static void processPOI() } // not above if (distance >= 3.0f) { + // TODO camera feature // You can feed this into camerastabilization /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ From 1540fe07166d6ba50f49f78baa1c037be7f6e7e1 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 20 Aug 2014 16:02:48 +0200 Subject: [PATCH 23/25] OP-1156 cleaned up updateAutoPilotVtol() function --- flight/modules/PathFollower/pathfollower.c | 113 +++++++++++++-------- 1 file changed, 72 insertions(+), 41 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 1e69ed8b7..67226589b 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -358,52 +358,83 @@ static uint8_t updateAutoPilotFixedWing() */ static uint8_t updateAutoPilotVtol() { - if (!global.vtolEmergencyFallbackSwitch) { - if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { - pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); - updateVtolDesiredAttitudeEmergencyFallback(); - return 1; - } else { - pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); - pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); - pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false); - uint8_t result = 1; - bool yaw_attitude = true; - float yaw = 0.0f; - switch (vtolPathFollowerSettings.YawControl) { - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: - yaw_attitude = false; - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: - yaw = updateTailInBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: - yaw = updateCourseBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: - yaw = updatePathBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: - yaw = updatePOIBearing(); - break; - } - result = updateVtolDesiredAttitude(yaw_attitude, yaw); - if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { - global.vtolEmergencyFallbackSwitch = true; - } - return result; - } + enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode; + enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode; + uint8_t result = 0; + + // decide on behaviour based on settings and system state + if (global.vtolEmergencyFallbackSwitch) { + returnmode = RETURN_0; + followermode = FOLLOWER_FALLBACK; } else { + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { + returnmode = RETURN_1; + followermode = FOLLOWER_FALLBACK; + } else { + returnmode = RETURN_RESULT; + followermode = FOLLOWER_REGULAR; + } + } + + // vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + + switch (followermode) { + case FOLLOWER_REGULAR: + { + // horizontal position control PID loop works according to settings in regular mode, allowing integral terms + pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false); + + // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm + bool yaw_attitude = true; + float yaw = 0.0f; + switch (vtolPathFollowerSettings.YawControl) { + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + yaw_attitude = false; + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: + yaw = updateTailInBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: + yaw = updateCourseBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: + yaw = updatePathBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: + yaw = updatePOIBearing(); + break; + } + result = updateVtolDesiredAttitude(yaw_attitude, yaw); + + // switch to emergency follower if follower indicates problems + if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED)) { + global.vtolEmergencyFallbackSwitch = true; + } + } + break; + case FOLLOWER_FALLBACK: + { + // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); + + // emergency follower has no return value updateVtolDesiredAttitudeEmergencyFallback(); - return 0; + } + break; + } + + switch (returnmode) { + case RETURN_RESULT: + return result; + + default: + // returns either 0 or 1 according to enum definition above + return returnmode; } } From 4653c7729616154a9de02234b0dd9ed0afebe23c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 20 Aug 2014 17:00:50 +0200 Subject: [PATCH 24/25] OP-1156 removed old pathfollower implementations --- .../fixedwingpathfollower.c | 772 ------------------ .../VtolPathFollower/inc/vtolpathfollower.h | 31 - .../VtolPathFollower/vtolpathfollower.c | 737 ----------------- 3 files changed, 1540 deletions(-) delete mode 100644 flight/modules/FixedWingPathFollower/fixedwingpathfollower.c delete mode 100644 flight/modules/VtolPathFollower/inc/vtolpathfollower.h delete mode 100644 flight/modules/VtolPathFollower/vtolpathfollower.c diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c deleted file mode 100644 index 556a10db8..000000000 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ /dev/null @@ -1,772 +0,0 @@ -/** - ****************************************************************************** - * - * @file fixedwingpathfollower.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint - * and sets @ref AttitudeDesired. It only does this when the FlightMode field - * of @ref ManualControlCommand is Auto. - * - * @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 - */ - -/** - * Input object: ActiveWaypoint - * Input object: PositionState - * Input object: ManualControlCommand - * Output object: AttitudeDesired - * - * This module will periodically update the value of the AttitudeDesired object. - * - * The module executes in its own thread in this example. - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - -#include - -#include "hwsettings.h" -#include "attitudestate.h" -#include "pathdesired.h" // object that will be updated by the module -#include "positionstate.h" -#include "flightstatus.h" -#include "pathstatus.h" -#include "airspeedstate.h" -#include "fixedwingpathfollowersettings.h" -#include "fixedwingpathfollowerstatus.h" -#include "homelocation.h" -#include "stabilizationdesired.h" -#include "stabilizationsettings.h" -#include "systemsettings.h" -#include "velocitydesired.h" -#include "velocitystate.h" -#include "taskinfo.h" -#include -#include - -#include "sin_lookup.h" -#include "paths.h" -#include "CoordinateConversions.h" - -// Private constants -#define MAX_QUEUE_SIZE 4 -#define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) - -// Private variables -static bool followerEnabled = false; -static xTaskHandle pathfollowerTaskHandle; -static PathDesiredData pathDesired; -static PathStatusData pathStatus; -static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; - -// Private functions -static void pathfollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent *ev); -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 - * \returns 0 on success or -1 if initialisation failed - */ -int32_t FixedWingPathFollowerStart() -{ - if (followerEnabled) { - // Start main task - xTaskCreate(pathfollowerTask, "PathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); - } - - return 0; -} - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t FixedWingPathFollowerInitialize() -{ - HwSettingsInitialize(); - HwSettingsOptionalModulesData optionalModules; - HwSettingsOptionalModulesGet(&optionalModules); - FrameType_t frameType = GetCurrentFrameType(); - - if ((optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) || - (frameType == FRAME_TYPE_FIXED_WING)) { - followerEnabled = true; - FixedWingPathFollowerSettingsInitialize(); - FixedWingPathFollowerStatusInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - AirspeedStateInitialize(); - } else { - followerEnabled = false; - } - return 0; -} -MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart); - -static float northVelIntegral = 0.0f; -static float eastVelIntegral = 0.0f; -static float downVelIntegral = 0.0f; - -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.0f; - -/** - * Module thread, should not return. - */ -static void pathfollowerTask(__attribute__((unused)) void *parameters) -{ - SystemSettingsData systemSettings; - FlightStatusData flightStatus; - - portTickType lastUpdateTime; - - AirspeedStateConnectCallback(airspeedStateUpdatedCb); - FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); - PathDesiredConnectCallback(SettingsUpdatedCb); - - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); - - // Main task loop - lastUpdateTime = xTaskGetTickCount(); - while (1) { - // Conditions when this runs: - // 1. Must have FixedWing type airframe - // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - - SystemSettingsGet(&systemSettings); - if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL)) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } - - // Continue collecting data if not enough time - vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); - - - FlightStatusGet(&flightStatus); - PathStatusGet(&pathStatus); - - uint8_t result; - // Check the combinations of flightmode and pathdesired mode - if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) { - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - } - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - } - } else { - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - } - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - } - } - } else { - // Be cleaner and get rid of global variables - northVelIntegral = 0.0f; - eastVelIntegral = 0.0f; - downVelIntegral = 0.0f; - courseIntegral = 0.0f; - speedIntegral = 0.0f; - powerIntegral = 0.0f; - } - PathStatusSet(&pathStatus); - } -} - -/** - * Compute desired velocity from the current position and path - * - * Takes in @ref PositionState and compares it to @ref PathDesired - * and computes @ref VelocityDesired - */ -static void updatePathVelocity() -{ - PositionStateData positionState; - - PositionStateGet(&positionState); - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - - // 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(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); - - float groundspeed; - float altitudeSetpoint; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - altitudeSetpoint = pathDesired.End.Down; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - boundf(progress.fractional_progress, 0.0f, 1.0f); - altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * - boundf(progress.fractional_progress, 0.0f, 1.0f); - break; - } - // make sure groundspeed is not zero - if (groundspeed < 1e-6f) { - groundspeed = 1e-6f; - } - - // calculate velocity - can be zero if waypoints are too close - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0]; - velocityDesired.East = progress.path_direction[1]; - - float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; - - // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) - // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector - // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) - // leading to an S-shape snake course the wrong way - // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't - // turn steep unless there is enough space complete the turn before crossing the flightpath - // 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 everything ) - // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - 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 - velocityDesired.North += progress.correction_direction[0] * error_speed; - velocityDesired.East += progress.correction_direction[1] * error_speed; - - // scale to correct length - float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - if (l > 1e-9f) { - velocityDesired.North *= groundspeed / l; - velocityDesired.East *= groundspeed / l; - } - - float downError = altitudeSetpoint - positionState.Down; - velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; - - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; - - VelocityDesiredSet(&velocityDesired); -} - - -/** - * Compute desired attitude from a fixed preset - * - */ -static void updateFixedAttitude(float *attitude) -{ - StabilizationDesiredData stabDesired; - - StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} - -/** - * Compute desired attitude from the desired velocity - * - * Takes in @ref NedState which has the acceleration in the - * NED frame as the feedback term and then compares the - * @ref VelocityState against the @ref VelocityDesired - */ -static uint8_t updateFixedDesiredAttitude() -{ - uint8_t result = 1; - - float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s] - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - AttitudeStateData attitudeState; - StabilizationSettingsData stabSettings; - FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; - AirspeedStateData airspeedState; - SystemSettingsData systemSettings; - - float groundspeedProjection; - float indicatedAirspeedState; - float indicatedAirspeedDesired; - float airspeedError; - - float pitchCommand; - - float descentspeedDesired; - float descentspeedError; - float powerCommand; - - float airspeedVector[2]; - float fluidMovement[2]; - float courseComponent[2]; - float courseError; - float courseCommand; - - FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); - - VelocityStateGet(&velocityState); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeStateGet(&attitudeState); - StabilizationSettingsGet(&stabSettings); - AirspeedStateGet(&airspeedState); - SystemSettingsGet(&systemSettings); - - /** - * 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 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 = groundspeedProjection + 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; - - // Vertical speed error - descentspeedDesired = boundf( - velocityDesired.Down, - -fixedwingpathfollowerSettings.VerticalVelMax, - fixedwingpathfollowerSettings.VerticalVelMax); - descentspeedError = descentspeedDesired - velocityState.Down; - - // Error condition: plane too slow or too fast - fixedwingpathfollowerStatus.Errors.Highspeed = 0; - fixedwingpathfollowerStatus.Errors.Lowspeed = 0; - if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.Overspeed) { - fixedwingpathfollowerStatus.Errors.Overspeed = 1; - result = 0; - } - if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.Highspeed) { - fixedwingpathfollowerStatus.Errors.Highspeed = 1; - result = 0; - } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.Lowspeed) { - fixedwingpathfollowerStatus.Errors.Lowspeed = 1; - result = 0; - } - if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.Stallspeed) { - fixedwingpathfollowerStatus.Errors.Stallspeed = 1; - result = 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.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.0f; - } - - // Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = boundf( - (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp, - -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max, - fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max - ); - - // Compute final thrust response - powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp + - powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki + - speedErrorToPowerCommandComponent; - - // Output internal state to telemetry - fixedwingpathfollowerStatus.Error.Power = descentspeedError; - fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral; - fixedwingpathfollowerStatus.Command.Power = powerCommand; - - // set thrust - stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand, - fixedwingpathfollowerSettings.ThrustLimit.Min, - fixedwingpathfollowerSettings.ThrustLimit.Max); - - // 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.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; - } - // 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.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; - } - - - /** - * Compute desired pitch command - */ - if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) { - // Integrate with saturation - airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki, - fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki); - } - - // Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp, - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max, - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max - ); - - // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp - + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.Ki - ) + verticalSpeedToPitchCommandComponent; - - fixedwingpathfollowerStatus.Error.Speed = airspeedError; - fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt; - fixedwingpathfollowerStatus.Command.Speed = pitchCommand; - - stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit.Min, - fixedwingpathfollowerSettings.PitchLimit.Max); - - // 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.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; - } - - /** - * Compute desired roll command - */ - courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; - - if (courseError < -180.0f) { - courseError += 360.0f; - } - if (courseError > 180.0f) { - 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); - courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp + - courseIntegral); - - fixedwingpathfollowerStatus.Error.Course = courseError; - fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral; - fixedwingpathfollowerStatus.Command.Course = courseCommand; - - stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral + - courseCommand, - fixedwingpathfollowerSettings.RollLimit.Min, - fixedwingpathfollowerSettings.RollLimit.Max); - - // TODO: find a check to determine loss of directional control. Likely needs some check of derivative - - - /** - * Compute desired yaw command - */ - // TODO implement raw control mode for yaw and base on Accels.Y - stabDesired.Yaw = 0.0f; - - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - - StabilizationDesiredSet(&stabDesired); - - FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); - - return result; -} - -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); -} - -static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - AirspeedStateData airspeedState; - VelocityStateData velocityState; - - AirspeedStateGet(&airspeedState); - VelocityStateGet(&velocityState); - 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 - 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/VtolPathFollower/inc/vtolpathfollower.h b/flight/modules/VtolPathFollower/inc/vtolpathfollower.h deleted file mode 100644 index f97f1588b..000000000 --- a/flight/modules/VtolPathFollower/inc/vtolpathfollower.h +++ /dev/null @@ -1,31 +0,0 @@ -/** - ****************************************************************************** - * - * @file vtolpathfollower.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Module to perform path following for VTOL. - * - * @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 VTOLPATHFOLLOWER_H -#define VTOLPATHFOLLOWER_H - -int32_t VtolPathFollowerInitialize(void); - -#endif // VTOLPATHFOLLOWER_H diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c deleted file mode 100644 index 8d8b753b7..000000000 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ /dev/null @@ -1,737 +0,0 @@ -/** - ****************************************************************************** - * - * @file vtolpathfollower.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief This module compared @ref PositionState to @ref PathDesired - * and sets @ref Stabilization. It only does this when the FlightMode field - * of @ref FlightStatus is PathPlanner or RTH. - * - * @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 - */ - -/** - * Input object: FlightStatus - * Input object: PathDesired - * Input object: PositionState - * Output object: StabilizationDesired - * - * This module will periodically update the value of the @ref StabilizationDesired object based on - * @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported - * by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be - * writing to @ref StabilizationDesired. - * - * The module executes in its own thread in this example. - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - -#include -#include -#include "vtolpathfollower.h" - -#include "accelstate.h" -#include "attitudestate.h" -#include "hwsettings.h" -#include "pathdesired.h" // object that will be updated by the module -#include "positionstate.h" -#include "manualcontrolcommand.h" -#include "flightstatus.h" -#include "pathstatus.h" -#include "gpsvelocitysensor.h" -#include "gpspositionsensor.h" -#include "homelocation.h" -#include "vtolpathfollowersettings.h" -#include "nedaccel.h" -#include "stabilizationdesired.h" -#include "stabilizationsettings.h" -#include "stabilizationbank.h" -#include "systemsettings.h" -#include "velocitydesired.h" -#include "velocitystate.h" -#include "taskinfo.h" - -#include "paths.h" -#include "CoordinateConversions.h" -#include - -#include "cameradesired.h" -#include "poilearnsettings.h" -#include "poilocation.h" -#include "accessorydesired.h" - -// Private constants -#define MAX_QUEUE_SIZE 4 -#define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) - -// Private types - -// Private variables -static xTaskHandle pathfollowerTaskHandle; -static PathStatusData pathStatus; -static VtolPathFollowerSettingsData vtolpathfollowerSettings; -static float poiRadius; - -// Private functions -static void vtolPathFollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent *ev); -static void updateNedAccel(); -static void updatePOIBearing(); -static void updatePathVelocity(); -static void updateEndpointVelocity(); -static void updateFixedAttitude(float *attitude); -static void updateVtolDesiredAttitude(bool yaw_attitude); -static bool vtolpathfollower_enabled; -static void accessoryUpdated(UAVObjEvent *ev); - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t VtolPathFollowerStart() -{ - if (vtolpathfollower_enabled) { - // Start main task - xTaskCreate(vtolPathFollowerTask, "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); - } - - return 0; -} - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t VtolPathFollowerInitialize() -{ - HwSettingsOptionalModulesData optionalModules; - - HwSettingsOptionalModulesGet(&optionalModules); - FrameType_t frameType = GetCurrentFrameType(); - - if ((optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) || - (frameType == FRAME_TYPE_MULTIROTOR)) { - VtolPathFollowerSettingsInitialize(); - NedAccelInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - CameraDesiredInitialize(); - AccessoryDesiredInitialize(); - PoiLearnSettingsInitialize(); - PoiLocationInitialize(); - vtolpathfollower_enabled = true; - } else { - vtolpathfollower_enabled = false; - } - - return 0; -} - -MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart); - -static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; - -static float northPosIntegral = 0; -static float eastPosIntegral = 0; -static float downPosIntegral = 0; - -static float thrustOffset = 0; -/** - * Module thread, should not return. - */ -static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) -{ - SystemSettingsData systemSettings; - FlightStatusData flightStatus; - - portTickType lastUpdateTime; - - VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); - AccessoryDesiredConnectCallback(accessoryUpdated); - - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - - // Main task loop - lastUpdateTime = xTaskGetTickCount(); - while (1) { - // Conditions when this runs: - // 1. Must have VTOL type airframe - // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - - SystemSettingsGet(&systemSettings); - if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX)) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } - - // Continue collecting data if not enough time - vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); - - // Convert the accels into the NED frame - updateNedAccel(); - - FlightStatusGet(&flightStatus); - PathStatusGet(&pathStatus); - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - - // Check the combinations of flightmode and pathdesired mode - if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) { - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(true); - updatePOIBearing(); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - } - } else { - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - } - } - } else { - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - } - PathStatusSet(&pathStatus); - } - } else { - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - northPosIntegral = 0; - eastPosIntegral = 0; - downPosIntegral = 0; - - // Track thrust before engaging this mode. Cheap system ident - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - thrustOffset = stabDesired.Thrust; - } - - AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); - } -} - -/** - * Compute bearing and elevation between current position and POI - */ -static void updatePOIBearing() -{ - const float DEADBAND_HIGH = 0.10f; - const float DEADBAND_LOW = -0.10f; - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - PositionStateData positionState; - PositionStateGet(&positionState); - CameraDesiredData cameraDesired; - CameraDesiredGet(&cameraDesired); - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - PoiLocationData poi; - PoiLocationGet(&poi); - - float dLoc[3]; - float yaw = 0; - /*float elevation = 0;*/ - - dLoc[0] = positionState.North - poi.North; - dLoc[1] = positionState.East - poi.East; - dLoc[2] = positionState.Down - poi.Down; - - if (dLoc[1] < 0) { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; - } else { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; - } - - // distance - float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f)); - - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - float changeRadius = 0; - // Move closer or further, radially - if (manualControlData.Pitch > DEADBAND_HIGH) { - changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; - } else if (manualControlData.Pitch < DEADBAND_LOW) { - changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; - } - - // move along circular path - float pathAngle = 0; - if (manualControlData.Roll > DEADBAND_HIGH) { - pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; - } else if (manualControlData.Roll < DEADBAND_LOW) { - pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; - } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { - // change radius only when not circling - poiRadius = distance + changeRadius; - } - - // don't try to move any closer - if (poiRadius >= 3.0f || changeRadius > 0) { - if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.StartingVelocity = 1.0f; - pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - } - } - // not above - if (distance >= 3.0f) { - // You can feed this into camerastabilization - /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ - - stabDesired.Yaw = yaw + (pathAngle / 2.0f); - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - - // cameraDesired.Yaw=yaw; - // cameraDesired.PitchOrServo2=elevation; - - CameraDesiredSet(&cameraDesired); - StabilizationDesiredSet(&stabDesired); - } -} - -/** - * Compute desired velocity from the current position and path - * - * Takes in @ref PositionState and compares it to @ref PathDesired - * and computes @ref VelocityDesired - */ -static void updatePathVelocity() -{ - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - PositionStateData positionState; - PositionStateGet(&positionState); - - float current_position[3] = { positionState.North, positionState.East, positionState.Down }; - struct path_status progress; - - path_progress( - cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - current_position, &progress, pathDesired.Mode); - - float speed; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - speed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) { - speed = 0; - } - break; - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - speed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) { - speed = 0; - } - break; - } - - VelocityDesiredData velocityDesired; - - northPosIntegral += progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; - eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; - downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT; - - northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - - velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral + - progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; - velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral + - progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; - velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral + - progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; - - // Make sure the desired velocities don't exceed PathFollower limits. - float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2)); - - if (groundspeedDesired > vtolpathfollowerSettings.HorizontalVelMax) { - velocityDesired.North *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; - velocityDesired.East *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; - } - - velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); - - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; - pathStatus.path_direction_north = progress.path_direction[0]; - pathStatus.path_direction_east = progress.path_direction[1]; - pathStatus.path_direction_down = progress.path_direction[2]; - - pathStatus.correction_direction_north = progress.correction_direction[0]; - pathStatus.correction_direction_east = progress.correction_direction[1]; - pathStatus.correction_direction_down = progress.correction_direction[2]; - VelocityDesiredSet(&velocityDesired); -} - -/** - * Compute desired velocity from the current position - * - * Takes in @ref PositionState and compares it to @ref PositionDesired - * and computes @ref VelocityDesired - */ -void updateEndpointVelocity() -{ - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - - PositionStateData positionState; - VelocityDesiredData velocityDesired; - - PositionStateGet(&positionState); - VelocityDesiredGet(&velocityDesired); - - float northError; - float eastError; - float downError; - float northCommand; - float eastCommand; - float downCommand; - - // Compute desired north command - northError = pathDesired.End.North - positionState.North; - northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral); - - eastError = pathDesired.End.East - positionState.East; - eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral); - - // Limit the maximum velocity - float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { - scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; - } - - velocityDesired.North = northCommand * scale; - velocityDesired.East = eastCommand * scale; - - downError = pathDesired.End.Down - positionState.Down; - downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); - - VelocityDesiredSet(&velocityDesired); -} - -/** - * Compute desired attitude from a fixed preset - * - */ -static void updateFixedAttitude(float *attitude) -{ - StabilizationDesiredData stabDesired; - - StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} - -/** - * Compute desired attitude from the desired velocity - * - * Takes in @ref NedState which has the acceleration in the - * NED frame as the feedback term and then compares the - * @ref VelocityState against the @ref VelocityDesired - */ -static void updateVtolDesiredAttitude(bool yaw_attitude) -{ - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - AttitudeStateData attitudeState; - NedAccelData nedAccel; - StabilizationBankData stabSettings; - SystemSettingsData systemSettings; - - float northError; - float northCommand; - - float eastError; - float eastCommand; - - float downError; - float downCommand; - - SystemSettingsGet(&systemSettings); - VelocityStateGet(&velocityState); - VelocityDesiredGet(&velocityDesired); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeStateGet(&attitudeState); - StabilizationBankGet(&stabSettings); - NedAccelGet(&nedAccel); - - float northVel = 0, eastVel = 0, downVel = 0; - switch (vtolpathfollowerSettings.VelocitySource) { - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION: - northVel = velocityState.North; - eastVel = velocityState.East; - downVel = velocityState.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED: - { - GPSVelocitySensorData gpsVelocity; - GPSVelocitySensorGet(&gpsVelocity); - northVel = gpsVelocity.North; - eastVel = gpsVelocity.East; - downVel = gpsVelocity.Down; - } - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED: - { - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); - eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); - downVel = velocityState.Down; - } - break; - default: - PIOS_Assert(0); - break; - } - - // Testing code - refactor into manual control command - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - // Compute desired north command - northError = velocityDesired.North - northVel; - northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.ILimit); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd - + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); - - // Compute desired east command - eastError = velocityDesired.East - eastVel; - eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.ILimit); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd - + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); - - // Compute desired down command - downError = velocityDesired.Down - downVel; - // Must flip this sign - downError = -downError; - downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, - -vtolpathfollowerSettings.VerticalVelPID.ILimit, - vtolpathfollowerSettings.VerticalVelPID.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd); - - stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1); - - // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the - // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - - if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) { - // For now override thrust with manual control. Disable at your risk, quad goes to China. - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - stabDesired.Thrust = manualControl.Thrust; - } - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - if (yaw_attitude) { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - } else { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; - } - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} - -/** - * Keep a running filtered version of the acceleration in the NED frame - */ -static void updateNedAccel() -{ - float accel[3]; - float q[4]; - float Rbe[3][3]; - float accel_ned[3]; - - // Collect downsampled attitude data - AccelStateData accelState; - - AccelStateGet(&accelState); - accel[0] = accelState.x; - accel[1] = accelState.y; - accel[2] = accelState.z; - - // rotate avg accels into earth frame and store it - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - for (uint8_t i = 0; i < 3; i++) { - accel_ned[i] = 0; - for (uint8_t j = 0; j < 3; j++) { - accel_ned[i] += Rbe[j][i] * accel[j]; - } - } - accel_ned[2] += 9.81f; - - NedAccelData accelData; - NedAccelGet(&accelData); - accelData.North = accel_ned[0]; - accelData.East = accel_ned[1]; - accelData.Down = accel_ned[2]; - NedAccelSet(&accelData); -} - -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); -} - -static void accessoryUpdated(UAVObjEvent *ev) -{ - if (ev->obj != AccessoryDesiredHandle()) { - return; - } - - AccessoryDesiredData accessory; - PoiLearnSettingsData poiLearn; - PoiLearnSettingsGet(&poiLearn); - - if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { - if (accessory.AccessoryVal < -0.5f) { - PositionStateData positionState; - PositionStateGet(&positionState); - PoiLocationData poi; - PoiLocationGet(&poi); - poi.North = positionState.North; - poi.East = positionState.East; - poi.Down = positionState.Down; - PoiLocationSet(&poi); - } - } - } -} From 41fdf4e81f778452620810f7ba87d3bda17ed212 Mon Sep 17 00:00:00 2001 From: James Duley Date: Thu, 21 Aug 2014 08:49:39 +1200 Subject: [PATCH 25/25] OP-1338 changed to check if qt in tools --- ground/openpilotgcs/openpilotgcs.pri | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index d8c392b9c..fd97c3c96 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -96,12 +96,12 @@ macx { GCS_QT_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5 GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml - lib_dir = $$[QT_INSTALL_LIBS] - lib_dir ~= s,/usr/lib/*,/usr/lib - equals(lib_dir, "/usr/lib") { - copyqt = 0 - } else { + lib_dir_is_in_tools = $$[QT_INSTALL_LIBS] + lib_dir_is_in_tools ~= s,$$(TOOLS_DIR)*,TRUE + equals(lib_dir_is_in_tools, "TRUE") { copyqt = 1 + } else { + copyqt = 0 } } GCS_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/openpilotgcs