Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Waypoint code rework. Upload/download mission works. Waypoint actions RTH and WAYPOINT are supported (untested)

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-01 17:02:58 +10:00
parent ce18dc94eb
commit 5ef05268de
5 changed files with 74 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/config/config.c
View file

@ -233,8 +233,6 @@ void resetNavConfig(navConfig_t * navConfig)
navConfig->inav.w_xy_gps_p = 1.0f;
navConfig->inav.w_xy_gps_v = 2.0f;
navConfig->inav.w_xy_dr_v = 0.25f;
navConfig->inav.w_z_res_v = 0.5f;
navConfig->inav.w_xy_res_v = 0.5f;

75
src/main/flight/navigation_rewrite.c
View file

@ -74,6 +74,7 @@ static void setupAltitudeController(void);
void resetNavigation(void);
static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint);
static void calcualteAndSetActiveWaypointToLocalPosition(t_fp_vector * pos);
void calculateInitialHoldPosition(t_fp_vector * pos);
/*************************************************************************************************/
@ -99,6 +100,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
@ -418,6 +420,19 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
}
},
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
@ -449,7 +464,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
@ -828,12 +843,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
setupAltitudeController();
posControl.activeWaypointIndex = 0;
calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
{
/* A helper function to do waypoint-specific action */
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_RTH:
default:
calcualteAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos);
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
};
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -843,14 +873,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_RTH:
default:
if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) {
// Waypoint reached
return NAV_FSM_EVENT_SUCCESS;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
// Update XY-position target to active waypoint
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE;
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
}
}
@ -862,14 +897,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// This is the last waypoint - finalize
// Last waypoint reached
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
}
else {
// Waypoint reached, do something and move on to next waypoint
posControl.activeWaypointIndex++;
calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
}
}
@ -1527,7 +1561,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
wpData->alt = wpLLH.alt;
}
// WP #1 - #15 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && posControl.waypointListValid) {
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount) {
*wpData = posControl.waypointList[wpNumber - 1];
}
@ -1583,21 +1617,28 @@ void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
}
}
static void calcualteAndSetActiveWaypointToLocalPosition(t_fp_vector * pos)
{
posControl.activeWaypoint.pos = *pos;
// Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
// Set desired position to next waypoint (XYZ-controller)
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
}
static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint)
{
gpsLocation_t wpLLH;
t_fp_vector localPos;
wpLLH.lat = waypoint->lat;
wpLLH.lon = waypoint->lon;
wpLLH.alt = waypoint->alt;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &posControl.activeWaypoint.pos);
// Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
posControl.activeWaypoint.yaw = calculateBearingToDestination(&posControl.activeWaypoint.pos);
// Set desired position to next waypoint (XYZ-controller)
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &localPos);
calcualteAndSetActiveWaypointToLocalPosition(&localPos);
}
/**

2
src/main/flight/navigation_rewrite.h
View file

@ -83,8 +83,6 @@ typedef struct navConfig_s {
float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements
float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements
float w_xy_dr_v; // When we are using dead reckoning, loosely assume that we don't move and stay at the same place
float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight
float w_xy_res_v;

13
src/main/flight/navigation_rewrite_private.h
View file

@ -174,13 +174,14 @@ typedef enum {
NAV_STATE_RTH_3D_FINISHED, // 21
NAV_STATE_WAYPOINT_INITIALIZE, // 22
NAV_STATE_WAYPOINT_IN_PROGRESS, // 23
NAV_STATE_WAYPOINT_REACHED, // 24
NAV_STATE_WAYPOINT_FINISHED, // 25
NAV_STATE_WAYPOINT_PRE_ACTION, // 23
NAV_STATE_WAYPOINT_IN_PROGRESS, // 24
NAV_STATE_WAYPOINT_REACHED, // 25
NAV_STATE_WAYPOINT_FINISHED, // 26
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 26
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 27
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 28
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 27
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 28
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 29
NAV_STATE_COUNT,
} navigationFSMState_t;

1
src/main/io/serial_cli.c
View file

@ -544,7 +544,6 @@ const clivalue_t valueTable[] = {
{ "inav_w_z_gps_v", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_z_gps_v, .config.minmax = { 0, 10 }, 0 },
{ "inav_w_xy_gps_p", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_gps_p, .config.minmax = { 0, 10 }, 0 },
{ "inav_w_xy_gps_v", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_gps_v, .config.minmax = { 0, 10 }, 0 },
{ "inav_w_xy_dr_v", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_dr_v, .config.minmax = { 0, 10 }, 0 },
{ "inav_w_z_res_v", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_z_res_v, .config.minmax = { 0, 10 }, 0 },
{ "inav_w_xy_res_v", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_res_v, .config.minmax = { 0, 10 }, 0 },
{ "inav_w_acc_bias", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_acc_bias, .config.minmax = { 0, 1 }, 0 },