APM_Control: use a ahrs reference, not pointer

saves pointer check

libraries/APM_Control/AP_PitchController.cpp

@@ -106,11 +106,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 	}
 	_last_t = tnow;
 	
-	if(_ahrs == NULL) return 0;
 	float delta_time    = (float)dt * 0.001f;
 	
 	// Get body rate vector (radians/sec)
-	float omega_y = _ahrs->get_gyro().y;
+	float omega_y = _ahrs.get_gyro().y;
 	
 	// Calculate the pitch rate error (deg/sec) and scale
 	float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
@@ -143,7 +142,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 
 	// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
     // No conversion is required for K_D
-	float kp_ff = max((_K_P - _K_I * _tau) * _tau  - _K_D , 0) / _ahrs->get_EAS2TAS();
+	float kp_ff = max((_K_P - _K_I * _tau) * _tau  - _K_D , 0) / _ahrs.get_EAS2TAS();
 	
 	// Calculate the demanded control surface deflection
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
@@ -180,7 +179,7 @@ int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler)
 float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
 {
 	float rate_offset;
-	float bank_angle = _ahrs->roll;
+	float bank_angle = _ahrs.roll;
 
 	// limit bank angle between +- 80 deg if right way up
 	if (fabsf(bank_angle) < radians(90))	{
@@ -194,11 +193,11 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
 			bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
 		}
 	}
-	if (!_ahrs->airspeed_estimate(&aspeed)) {
+	if (!_ahrs.airspeed_estimate(&aspeed)) {
 	    // If no airspeed available use average of min and max
         aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
 	}
-    if (abs(_ahrs->pitch_sensor) > 7000) {
+    if (abs(_ahrs.pitch_sensor) > 7000) {
         // don't do turn coordination handling when at very high pitch angles
         rate_offset = 0;
     } else {

libraries/APM_Control/AP_PitchController.h

@@ -10,14 +10,13 @@
 
 class AP_PitchController {
 public:
-	AP_PitchController(const AP_SpdHgtControl::AircraftParameters &parms) :
-		aparm(parms)
+	AP_PitchController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
+		aparm(parms),
+        _ahrs(ahrs)
     { 
 		AP_Param::setup_object_defaults(this, var_info);
 	}
 
-	void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
-
 	int32_t get_rate_out(float desired_rate, float scaler);
 	int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator);
     float   get_coordination_rate_offset(void) const;
@@ -44,7 +43,7 @@ private:
 	int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
     float   _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
 	
-	AP_AHRS *_ahrs;
+	AP_AHRS &_ahrs;
 	
 };
 

libraries/APM_Control/AP_RollController.cpp

@@ -82,16 +82,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
 	}
 	_last_t = tnow;
 	
-
-	if (_ahrs == NULL) {
-        // can't control without a reference
-        return 0;
-    }
-
 	// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
     // No conversion is required for K_D
 	float ki_rate = _K_I * _tau;
-	float kp_ff = max((_K_P - _K_I * _tau) * _tau  - _K_D , 0)/_ahrs->get_EAS2TAS();
+	float kp_ff = max((_K_P - _K_I * _tau) * _tau  - _K_D , 0)/_ahrs.get_EAS2TAS();
 	float delta_time    = (float)dt * 0.001f;
 	
 	// Limit the demanded roll rate
@@ -102,14 +96,14 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
     }
 	
     // Get body rate vector (radians/sec)
-	float omega_x = _ahrs->get_gyro().x;
+	float omega_x = _ahrs.get_gyro().x;
 	
 	// Calculate the roll rate error (deg/sec) and apply gain scaler
 	float rate_error = (desired_rate - ToDeg(omega_x)) * scaler;
 	
 	// Get an airspeed estimate - default to zero if none available
 	float aspeed;
-	if (!_ahrs->airspeed_estimate(&aspeed)) {
+	if (!_ahrs.airspeed_estimate(&aspeed)) {
         aspeed = 0.0f;
     }
 

libraries/APM_Control/AP_RollController.h

@@ -10,14 +10,13 @@
 
 class AP_RollController {
 public:
-	AP_RollController(const AP_SpdHgtControl::AircraftParameters &parms) :
-		aparm(parms)
+	AP_RollController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
+		aparm(parms),
+        _ahrs(ahrs)
     { 
 		AP_Param::setup_object_defaults(this, var_info);
 	}
 
-	void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
-
 	int32_t get_rate_out(float desired_rate, float scaler);
 	int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator);
 
@@ -40,7 +39,7 @@ private:
 
 	int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
 
-	AP_AHRS *_ahrs;
+	AP_AHRS &_ahrs;
 
 };
 

libraries/APM_Control/AP_YawController.cpp

@@ -71,9 +71,6 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
 	}
 	_last_t = tnow;
 	
-	if(_ins == NULL) { // can't control without a reference
-		return 0; 
-	}
 
     int16_t aspd_min = aparm.airspeed_min;
     if (aspd_min < 1) {
@@ -85,22 +82,22 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
 	// Calculate yaw rate required to keep up with a constant height coordinated turn
 	float aspeed;
 	float rate_offset;
-	float bank_angle = _ahrs->roll;
+	float bank_angle = _ahrs.roll;
 	// limit bank angle between +- 80 deg if right way up
 	if (fabsf(bank_angle) < 1.5707964f)	{
 	    bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);
 	}
-	if (!_ahrs->airspeed_estimate(&aspeed)) {
+	if (!_ahrs.airspeed_estimate(&aspeed)) {
 	    // If no airspeed available use average of min and max
         aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max));
 	}
     rate_offset = (GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
 
     // Get body rate vector (radians/sec)
-	float omega_z = _ahrs->get_gyro().z;
+	float omega_z = _ahrs.get_gyro().z;
 	
 	// Get the accln vector (m/s^2)
-	float accel_y = _ins->get_accel().y;
+	float accel_y = _ahrs.get_ins()->get_accel().y;
 
 	// Subtract the steady turn component of rate from the measured rate
 	// to calculate the rate relative to the turn requirement in degrees/sec

libraries/APM_Control/AP_YawController.h

@@ -10,17 +10,13 @@
 
 class AP_YawController {
 public:                      
-	AP_YawController(const AP_SpdHgtControl::AircraftParameters &parms) :
-		aparm(parms)
+	AP_YawController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
+		aparm(parms),
+        _ahrs(ahrs)
 	{
 		AP_Param::setup_object_defaults(this, var_info);
 	}
 
-	void set_ahrs(AP_AHRS *ahrs) { 
-		_ahrs = ahrs; 
-		_ins = _ahrs->get_ins();
-	}
-
 	int32_t get_servo_out(float scaler, bool disable_integrator);
 
 	void reset_I();
@@ -43,9 +39,7 @@ private:
 
 	float _integrator;
 
-	AP_AHRS *_ahrs;
-	AP_InertialSensor *_ins;
-
+	AP_AHRS &_ahrs;
 };
 
 #endif // __AP_YAW_CONTROLLER_H__