libraries: use new math functions

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -512,7 +512,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     float earth_error_Z = error.z;
 
     // equation 10
-    float tilt = sqrt(sq(GA_e.x) + sq(GA_e.y));
+    float tilt = pythagorous2(GA_e.x, GA_e.y);
 
     // equation 11
     float theta = atan2(GA_b.y, GA_b.x);

libraries/AP_Camera/AP_Camera.cpp

@@ -53,7 +53,7 @@ AP_Camera::servo_pic()
 	RC_Channel_aux::set_radio(RC_Channel_aux::k_cam_trigger, _servo_on_pwm);
 
 	// leave a message that it should be active for this many loops (assumes 50hz loops)
-	_trigger_counter = constrain(_trigger_duration*5,0,255);
+	_trigger_counter = constrain_int16(_trigger_duration*5,0,255);
 }
 
 /// basic relay activation
@@ -63,7 +63,7 @@ AP_Camera::relay_pic()
     relay.on();
 
     // leave a message that it should be active for this many loops (assumes 50hz loops)
-    _trigger_counter = constrain(_trigger_duration*5,0,255);
+    _trigger_counter = constrain_int16(_trigger_duration*5,0,255);
 }
 
 /// pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle.

libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde

@@ -46,10 +46,11 @@ void loop()
         hal.console->print(gps.ground_speed / 100.0);
         hal.console->print(" COG:");
         hal.console->print(gps.ground_course / 100.0, DEC);
+		Vector2f vel = Vector2f(gps.velocity_north(), gps.velocity_east());
         hal.console->printf(" VEL: %.2f %.2f %.2f",
-                      gps.velocity_north(),
-                      gps.velocity_east(),
-                      sqrt(sq(gps.velocity_north())+sq(gps.velocity_east())));
+							vel.x,
+							vel.y,
+							vel.length());
         hal.console->print(" SAT:");
         hal.console->print(gps.num_sats, DEC);
         hal.console->print(" FIX:");

libraries/AP_Limits/AP_Limit_Geofence.cpp

@@ -88,7 +88,7 @@ bool AP_Limit_Geofence::triggered() {
 
     // simple mode, pointers to current and home exist.
     if (_simple && _current_loc && _home) {
-        distance = (uint32_t) get_distance_meters(_current_loc, _home);
+        distance = (uint32_t) get_distance(_current_loc, _home);
         if (distance > 0 &&  distance > (uint16_t) _radius) {
 
             // TRIGGER
@@ -130,28 +130,6 @@ bool AP_Limit_Geofence::triggered() {
 
 
 
-uint32_t get_distance_meters(struct Location *loc1, struct Location *loc2)  // distance in meters between two locations
-{
-    // pointers missing (dangling)
-    if (!loc1 || !loc2) {
-        return -1;  
-    }
-    // do not trigger a false positive on erroneous location data
-    if(loc1->lat == 0 || loc1->lng == 0) {
-        return -1;
-    }
-    // do not trigger a false positive on erroneous location data
-    if(loc2->lat == 0 || loc2->lng == 0) {
-        return -1;
-    }
-
-    float dlat  = (float)(loc2->lat - loc1->lat);
-    float dlong = (float)(loc2->lng - loc1->lng);
-    return (sqrt(sq(dlat) + sq(dlong)) * .01113195);
-}
-
-
-
 AP_Int8 AP_Limit_Geofence::fence_total() {
     return _fence_total;
 }

libraries/AP_Mount/AP_Mount.cpp

@@ -534,7 +534,7 @@ AP_Mount::calc_GPS_target_angle(struct Location *target)
     float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)*.00000005))*.01113195;
     float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195;
     float GPS_vector_z = (target->alt-_current_loc->alt);                 // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
-    float target_distance = 100.0*sqrt(GPS_vector_x*GPS_vector_x + GPS_vector_y*GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
+    float target_distance = 100.0*pythagorous2(GPS_vector_x, GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
     _roll_control_angle  = 0;
     _tilt_control_angle  = atan2(GPS_vector_z, target_distance);
     _pan_control_angle   = atan2(GPS_vector_x, GPS_vector_y);

libraries/AP_Param/AP_Param.cpp

@@ -1006,3 +1006,4 @@ void AP_Param::show_all(void)
         }
     }
 }
+

libraries/RC_Channel/RC_Channel.cpp

@@ -175,7 +175,7 @@ RC_Channel::calc_pwm(void)
         radio_out       = pwm_out + radio_trim;
     }
 
-    radio_out = constrain(radio_out, radio_min.get(), radio_max.get());
+    radio_out = constrain_int16(radio_out, radio_min.get(), radio_max.get());
 }
 
 // ------------------------------------------
@@ -262,7 +262,7 @@ RC_Channel::angle_to_pwm()
 int16_t
 RC_Channel::pwm_to_range()
 {
-    int16_t r_in = constrain(radio_in, radio_min.get(), radio_max.get());
+    int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());
 
     if (_reverse == -1) {
 	    r_in = radio_max.get() - (r_in - radio_min.get());

libraries/RC_Channel/RC_Channel_aux.cpp

@@ -111,7 +111,7 @@ RC_Channel_aux::set_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t
 {
     for (uint8_t i = 0; i < 7; i++) {
         if (_aux_channels[i] && _aux_channels[i]->function.get() == function) {
-			_aux_channels[i]->radio_out = constrain(value,_aux_channels[i]->radio_min,_aux_channels[i]->radio_max);
+			_aux_channels[i]->radio_out = constrain_int16(value,_aux_channels[i]->radio_min,_aux_channels[i]->radio_max);
             _aux_channels[i]->output();
 		}
     }