diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp
index 1109a3641b..acebc1f208 100644
--- a/libraries/APO/AP_MavlinkCommand.cpp
+++ b/libraries/APO/AP_MavlinkCommand.cpp
@@ -32,7 +32,7 @@ AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) :
 	}
 }
 
-AP_MavlinkCommand::AP_MavlinkCommand(mavlink_waypoint_t cmd) :
+AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) :
 	_data(k_commands + cmd.seq), _seq(cmd.seq) {
 	setCommand(MAV_CMD(cmd.command));
 	setAutocontinue(cmd.autocontinue);
@@ -76,7 +76,7 @@ AP_MavlinkCommand::AP_MavlinkCommand(mavlink_waypoint_t cmd) :
 	Serial.flush();
 }
 
-mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) {
+mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) const {
 	mavlink_waypoint_t mavCmd;
 	mavCmd.seq = getSeq();
 	mavCmd.command = getCommand();
@@ -95,7 +95,7 @@ mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) {
 	return mavCmd;
 }
 
-float AP_MavlinkCommand::bearingTo(AP_MavlinkCommand next) const {
+float AP_MavlinkCommand::bearingTo(const AP_MavlinkCommand & next) const {
 	float deltaLon = next.getLon() - getLon();
 	/*
 	 Serial.print("Lon: "); Serial.println(getLon());
@@ -122,7 +122,7 @@ float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const {
 	return bearing;
 }
 
-float AP_MavlinkCommand::distanceTo(AP_MavlinkCommand next) const {
+float AP_MavlinkCommand::distanceTo(const AP_MavlinkCommand & next) const {
 	float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2);
 	float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2);
 	float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
@@ -151,8 +151,8 @@ float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) cons
 }
 
 //calculates cross track of a current location
-float AP_MavlinkCommand::crossTrack(AP_MavlinkCommand previous,
-		int32_t lat_degInt, int32_t lon_degInt) {
+float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
+		int32_t lat_degInt, int32_t lon_degInt) const {
 		float d = previous.distanceTo(lat_degInt, lon_degInt);
 		float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
 		float bNext = previous.bearingTo(*this);
@@ -161,8 +161,8 @@ float AP_MavlinkCommand::crossTrack(AP_MavlinkCommand previous,
 }
 
 // calculates along  track distance of a current location
-float AP_MavlinkCommand::alongTrack(AP_MavlinkCommand previous,
-		int32_t lat_degInt, int32_t lon_degInt) {
+float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
+		int32_t lat_degInt, int32_t lon_degInt) const {
 	// ignores lat/lon since single prec.
 	float dXt = this->crossTrack(previous,lat_degInt, lon_degInt);
 	float d = previous.distanceTo(lat_degInt, lon_degInt);
diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h
index 58335abe53..a50002cb95 100644
--- a/libraries/APO/AP_MavlinkCommand.h
+++ b/libraries/APO/AP_MavlinkCommand.h
@@ -50,7 +50,7 @@ public:
 	 * Constructor for copying/ saving from a mavlink waypoint.
 	 * @param cmd The mavlink_waopint_t structure for the command.
 	 */
-	AP_MavlinkCommand(mavlink_waypoint_t cmd);
+	AP_MavlinkCommand(const mavlink_waypoint_t & cmd);
 
 	bool save() {
 		return _data.save();
@@ -64,7 +64,7 @@ public:
 	bool getAutocontinue() const {
 		return _data.get().autocontinue;
 	}
-	void setAutocontinue(bool val) {
+	void setAutocontinue( bool val) {
 		_data.get().autocontinue = val;
 	}
 	void setSeq(uint8_t val) {
@@ -294,14 +294,14 @@ public:
 	 * conversion for outbound packets to ground station
 	 * @return output the mavlink_waypoint_t packet
 	 */
-	mavlink_waypoint_t convert(uint8_t current);
+	mavlink_waypoint_t convert(uint8_t current) const;
 
 	/**
 	 * Calculate the bearing from this command to the next command
 	 * @param next The command to calculate the bearing to.
 	 * @return the bearing
 	 */
-	float bearingTo(AP_MavlinkCommand next) const;
+	float bearingTo(const AP_MavlinkCommand & next) const;
 
 	/**
 	 * Bearing form this command to a gps coordinate in integer units
@@ -316,7 +316,7 @@ public:
 	 * @param next The command to measure to.
 	 * @return The distance in meters.
 	 */
-	float distanceTo(AP_MavlinkCommand next) const;
+	float distanceTo(const AP_MavlinkCommand & next) const;
 
 	/**
 	 * Distance to a gps coordinate in integer units
@@ -363,10 +363,10 @@ public:
 	}
 
 	//calculates cross track of a current location
-	float crossTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt);
+	float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
 
 	// calculates along  track distance of a current location
-	float alongTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt);
+	float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
 };
 
 } // namespace apo