/* $Id: NavigatorCommands.java 606 2010-11-02 19:52:33Z parizek $
*
* This file is a part of jPapaBench providing a Java implementation
* of PapaBench project.
* Copyright (C) 2010 Michal Malohlava <michal.malohlava_at_d3s.mff.cuni.cz>
*
* 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 2
* 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
*/
package papabench.core.commons.data.impl;
import static papabench.core.commons.conf.AirframeParametersConf.CARROT;
import static papabench.core.commons.conf.AirframeParametersConf.NOMINAL_AIRSPEED;
import papabench.core.autopilot.conf.LateralFlightMode;
import papabench.core.autopilot.conf.VerticalFlightMode;
import papabench.core.autopilot.data.Position3D;
import papabench.core.autopilot.modules.AutopilotStatus;
import papabench.core.autopilot.modules.Estimator;
import papabench.core.autopilot.modules.Navigator;
import papabench.core.utils.LogUtils;
import papabench.core.utils.MathUtils;
/**
* Generalized implementation of navigator commands.
*
* FIXME AbstractFLightPlan should inherit from this method!
* NOTE: this file is partly based on Paparazzi code (method prefix nav) and partly on Papabench code.
*
* @author Michal Malohlava
*
*/
public abstract class NavigatorCommands {
protected abstract Navigator navigator();
protected abstract Estimator estimator();
protected abstract AutopilotStatus status();
protected abstract Position3D WP(int n);
protected abstract int getLastWPNumber();
protected abstract Position3D getLastPosition();
private float carrot = 0;
/**
* Set the climb control to auto-throttle with the specified
* pitchpre-command
*/
protected final void navVerticalAutoThrottleMode(float pitch) {
// FIXME v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \
// navigator().setDesiredPitch(pitch);
status().setVerticalFlightMode(VerticalFlightMode.AUTO_GAZ);
status().setPitch(pitch);
}
/**
* Set the climb control to auto-pitch with the specified throttle
* pre-command
*/
protected final void navVerticalAutoPitchMode(int throttle) {
// _ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH;
// nav_throttle_setpoint = _throttle;
navigator().setAutoPitch(true);
navigator().setDesiredGaz(throttle);
}
/**
* Set the vertical mode to altitude control with the specified altitude
* setpoint and climb pre-command.
*/
protected final void navVerticalAltitudeMode(float alt, float preClimb) {
// v_ctl_mode = V_CTL_MODE_AUTO_ALT;
status().setVerticalFlightMode(VerticalFlightMode.AUTO_ALTITUDE);
navigator().setDesiredAltitude(alt);
navigator().setPreClimb(preClimb);
}
/** Set the vertical mode to climb control with the specified climb setpoint */
protected final void navVerticalClimbMode(float climb) {
// v_ctl_mode = V_CTL_MODE_AUTO_CLIMB;
// v_ctl_climb_setpoint = _climb;
status().setVerticalFlightMode(VerticalFlightMode.AUTO_CLIMB); // AUTO_CLIMB -> navigator has to obtain values
status().setClimb(climb);
}
/** Set the vertical mode to fixed throttle with the specified setpoint */
protected final void navVerticalThrottleMode(int throttle) {
// v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
// nav_throttle_setpoint = _throttle;
status().setVerticalFlightMode(VerticalFlightMode.AUTO_GAZ);
navigator().setDesiredGaz(throttle);
}
protected final void navHeading(float course) {
// lateral_mode = LATERAL_MODE_COURSE;
// h_ctl_course_setpoint = _course;
status().setLateralFlightMode(LateralFlightMode.COURSE);
navigator().setDesiredCourse(course);
}
protected final void navAttitude(float roll) {
// lateral_mode = LATERAL_MODE_ROLL;
// h_ctl_roll_setpoint = _roll;
status().setLateralFlightMode(LateralFlightMode.ROLL);
status().setRoll(roll);
}
/**
* Decide if the UAV is approaching the current waypoint.
* Computes dist2_to_wp and compare it to square carrot.
* Return true if it is smaller. Else computes by scalar products if
* uav has not gone past waypoint.
* Return true if it is the case.
*/
protected final boolean navApproachingFrom(int toWP, int fromWP, float approachingTime) {
return navApproachingFrom(WP(toWP), WP(fromWP), approachingTime);
}
protected final boolean navApproachingFrom(Position3D toWP, Position3D fromWP, float approachingTime) {
float pwX = toWP.x - estimator().getPosition().x;
float pwY = toWP.y - estimator().getPosition().y;
float dist2ToWP = pwX*pwX + pwY*pwY;
float minDist = approachingTime * estimator().getHorizontalSpeed().module;
if (dist2ToWP < minDist*minDist) {
return true;
}
float scalarProduct = MathUtils.scalarProduct(toWP, fromWP, estimator().getPosition());
return (scalarProduct < 0.);
}
protected final void navGotoWaypoint(int wp) {
// horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
flyToWP(WP(wp));
}
protected final void navSegment(Position3D startWP, Position3D endWP) {
navRouteXY(startWP, endWP);
}
protected final void navRouteXY(Position3D startWP, Position3D endWP) {
float legX = endWP.x - startWP.x;
float legY = endWP.y - startWP.y;
float leg2 = Math.max(legX*legX+legY*legY,1f);
float navLegProgress = ((estimator().getPosition().x - startWP.x)*legX + (estimator().getPosition().y - startWP.y)*legY) / leg2;
float navLegLength = (float) Math.sqrt(leg2);
/* distance of CARROT */
float carrot = CARROT * NOMINAL_AIRSPEED;
// navLegProgress += Math.max(carrot/navLegLength, 0);
// navInSegment = true;
// navSegment.1 = startWP;
// navSegment.2 = endWP;
//
// horizontal_mode = ROUTE;
//
// flyToXY(startWP.x+navLegProgress*legX + navShift*legY/navLegLength, startWP.y+navLegProgress*legY-navShift*legX/navLegLength);
}
protected final void killThrottle() {
}
protected final boolean approaching(int wp) {
return approaching(WP(wp));
}
protected final boolean approaching(Position3D wp) {
return approaching(wp, getLastPosition());
}
protected final boolean approaching(Position3D toWP, Position3D fromWP) {
//LogUtils.log(this, "Approaching to=" + toWP + " from="+fromWP);
float pwX = toWP.x - estimator().getPosition().x;
float pwY = toWP.y - estimator().getPosition().y;
float dist2ToWP = pwX*pwX + pwY*pwY;
carrot = CARROT * estimator().getHorizontalSpeed().module;
carrot = carrot < 40 ? 40 : carrot; // FIXME what is number 10 (40 in original version)? replace by constant with user friendly name
if (dist2ToWP < carrot*carrot) {
return true;
}
float scalarProduct = MathUtils.scalarProduct(toWP, fromWP, estimator().getPosition());
return (scalarProduct < 0.);
}
protected final void flyToWP(int wp) {
flyToWP(WP(wp));
}
protected final void flyToWP(Position3D wp) {
flyToXY(wp.x, wp.y);
}
// WARNING - this method is implemented according to PapaBench code (it is not compatible with paparazzi)
protected final void flyToXY(float x, float y) {
navigator().setDesiredPosition(x,y);
navigator().setDesiredCourse((float) (Math.PI/2 - Math.atan2(y-estimator().getPosition().y, x-estimator().getPosition().x)));
//LogUtils.log(this, "FlyToWP: (" + x + ", "+ y + "), course = " + Math.toDegrees(navigator().getDesiredCourse()));
}
protected final void routeTo(int fromWP, int toWP) {
routeTo(WP(fromWP), WP(toWP));
}
protected final void routeTo(Position3D fromWP, Position3D toWP) {
float legX = toWP.x - fromWP.x;
float legY = toWP.y - fromWP.y;
float leg2 = Math.max(legX*legX+legY*legY,1f);
float alpha = ((estimator().getPosition().x - fromWP.x) * legX + (estimator().getPosition().y - fromWP.y)*legY) / leg2;
alpha = Math.max(alpha, 0);
float leg = (float) Math.sqrt(leg2);
alpha += Math.max(carrot/2, 0f ); /* carrot computed in approaching() */
alpha = Math.min(1, alpha);
flyToXY(fromWP.x + alpha*legX, fromWP.y + alpha*legY);
}
}