Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Update floating point calculations to use floats instead of doubles.

Browse Source 
- Allows use of hardware floating point on the Cortex-M4.
- Added "f" suffix to floating point literals.
- Call floating point versions of stdlib math functions.
This commit is contained in:
James Bielman 2013-01-10 10:42:24 -08:00 committed by Andrew Tridgell
parent d00b06d449
commit 5631f865b2
74 changed files with 538 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/Attitude.pde
View File

@ -23,7 +23,7 @@ static void throttle_slew_limit(int16_t last_throttle)
// if slew limit rate is set to zero then do not slew limit // if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) { if (g.throttle_slewrate) {
// limit throttle change by the given percentage per second // limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01 * fabs(g.channel_throttle.radio_max - g.channel_throttle.radio_min); float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min);
// allow a minimum change of 1 PWM per cycle // allow a minimum change of 1 PWM per cycle
if (temp < 1) { if (temp < 1) {
temp = 1; temp = 1;

42
APMrover2/GCS_Mavlink.pde
View File

@ -1361,9 +1361,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_MISSION: case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL:
{ {
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm tell_command.alt = packet.z*1.0e2f; // in as m converted to cm
tell_command.options = 0; // absolute altitude tell_command.options = 0; // absolute altitude
break; break;
} }
@ -1371,10 +1371,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#ifdef MAV_FRAME_LOCAL_NED #ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position) case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{ {
tell_command.lat = 1.0e7*ToDeg(packet.x/ tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = -packet.z*1.0e2; tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break; break;
} }
@ -1383,10 +1383,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#ifdef MAV_FRAME_LOCAL #ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position) case MAV_FRAME_LOCAL: // local (relative to home position)
{ {
tell_command.lat = 1.0e7*ToDeg(packet.x/ tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2; tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break; break;
} }
@ -1394,9 +1394,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{ {
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2; tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break; break;
} }
@ -1557,7 +1557,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else if (var_type == AP_PARAM_INT32) { } else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition; float v = packet.param_value+rounding_addition;
v = constrain(v, -2147483648.0, 2147483647.0); v = constrain(v, -2147483648.0f, 2147483647.0f);
((AP_Int32 *)vp)->set_and_save(v); ((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) { } else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
@ -1634,12 +1634,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_hil_state_decode(msg, &packet); mavlink_msg_hil_state_decode(msg, &packet);
float vel = pythagorous2(packet.vx, packet.vy); float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360_cd(ToDeg(atan2(packet.vy, packet.vx)) * 100); float cog = wrap_360_cd(ToDeg(atan2f(packet.vy, packet.vx)) * 100);
// set gps hil sensor // set gps hil sensor
g_gps->setHIL(packet.time_usec/1000, g_gps->setHIL(packet.time_usec/1000,
packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, packet.lat*1.0e-7f, packet.lon*1.0e-7f, packet.alt*1.0e-3f,
vel*1.0e-2, cog*1.0e-2, 0, 10); vel*1.0e-2f, cog*1.0e-2f, 0, 10);
// rad/sec // rad/sec
Vector3f gyros; Vector3f gyros;
@ -1649,9 +1649,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// m/s/s // m/s/s
Vector3f accels; Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0); accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(gyros); ins.set_gyro(gyros);

2
APMrover2/navigation.pde
View File

@ -71,7 +71,7 @@ static void update_crosstrack(void)
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
} }

8
ArduCopter/ArduCopter.pde
View File

@ -630,7 +630,7 @@ static int8_t CH7_wp_index;
// Battery Sensors // Battery Sensors
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Battery Voltage of battery, initialized above threshold for filter // Battery Voltage of battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; static float battery_voltage1 = LOW_VOLTAGE * 1.05f;
// refers to the instant amp draw – based on an Attopilot Current sensor // refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps1; static float current_amps1;
// refers to the total amps drawn – based on an Attopilot Current sensor // refers to the total amps drawn – based on an Attopilot Current sensor
@ -1741,11 +1741,11 @@ void update_simple_mode(void)
if (simple_counter == 1) { if (simple_counter == 1) {
// roll // roll
simple_cos_x = sin(radians(90 - delta)); simple_cos_x = sinf(radians(90 - delta));
}else if (simple_counter > 2) { }else if (simple_counter > 2) {
// pitch // pitch
simple_sin_y = cos(radians(90 - delta)); simple_sin_y = cosf(radians(90 - delta));
simple_counter = 0; simple_counter = 0;
} }
@ -2132,7 +2132,7 @@ static void update_altitude_est()
} }
static void tuning(){ static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0; tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1 g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1
switch(g.radio_tuning) { switch(g.radio_tuning) {

10
ArduCopter/Attitude.pde
View File

@ -708,7 +708,7 @@ static void update_throttle_cruise(int16_t throttle)
} }
// calc average throttle if we are in a level hover // calc average throttle if we are in a level hover
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_avg = throttle_avg * .99 + (float)throttle * .01; throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f;
g.throttle_cruise = throttle_avg; g.throttle_cruise = throttle_avg;
} }
} }
@ -802,7 +802,7 @@ get_throttle_accel(int16_t z_target_accel)
z_accel_error = 0; z_accel_error = 0;
} else { } else {
// calculate accel error and Filter with fc = 2 Hz // calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error); z_accel_error = z_accel_error + 0.11164f * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
} }
last_call_ms = now; last_call_ms = now;
@ -937,7 +937,7 @@ get_throttle_rate(int16_t z_target_speed)
z_rate_error = 0; z_rate_error = 0;
} else { } else {
// calculate rate error and filter with cut off frequency of 2 Hz // calculate rate error and filter with cut off frequency of 2 Hz
z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error); z_rate_error = z_rate_error + 0.20085f * ((z_target_speed - climb_rate) - z_rate_error);
} }
last_call_ms = now; last_call_ms = now;
@ -1039,7 +1039,7 @@ get_throttle_althold_with_slew(int16_t target_alt, int16_t min_climb_rate, int16
static void static void
get_throttle_rate_stabilized(int16_t target_rate) get_throttle_rate_stabilized(int16_t target_rate)
{ {
controller_desired_alt += target_rate * 0.02; controller_desired_alt += target_rate * 0.02f;
// do not let target altitude get too far from current altitude // do not let target altitude get too far from current altitude
controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
@ -1098,7 +1098,7 @@ get_throttle_surface_tracking(int16_t target_rate)
} }
last_call_ms = now; last_call_ms = now;
target_sonar_alt += target_rate * 0.02; target_sonar_alt += target_rate * 0.02f;
distance_error = (target_sonar_alt-sonar_alt); distance_error = (target_sonar_alt-sonar_alt);
sonar_induced_slew_rate = constrain(fabs(THR_SURFACE_TRACKING_P * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX); sonar_induced_slew_rate = constrain(fabs(THR_SURFACE_TRACKING_P * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);

60
ArduCopter/GCS_Mavlink.pde
View File

@ -198,7 +198,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
uint8_t battery_remaining = -1; uint8_t battery_remaining = -1;
if (current_total1 != 0 && g.pack_capacity != 0) { if (current_total1 != 0 && g.pack_capacity != 0) {
battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity); battery_remaining = (100.0f * (g.pack_capacity - current_total1) / g.pack_capacity);
} }
if (current_total1 != 0) { if (current_total1 != 0) {
battery_current = current_amps1 * 100; battery_current = current_amps1 * 100;
@ -264,12 +264,12 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{ {
mavlink_msg_nav_controller_output_send( mavlink_msg_nav_controller_output_send(
chan, chan,
nav_roll / 1.0e2, nav_roll / 1.0e2f,
nav_pitch / 1.0e2, nav_pitch / 1.0e2f,
wp_bearing / 1.0e2, wp_bearing / 1.0e2f,
wp_bearing / 1.0e2, wp_bearing / 1.0e2f,
wp_distance / 1.0e2, wp_distance / 1.0e2f,
altitude_error / 1.0e2, altitude_error / 1.0e2f,
0, 0,
crosstrack_error); // was 0 crosstrack_error); // was 0
} }
@ -434,12 +434,12 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{ {
mavlink_msg_vfr_hud_send( mavlink_msg_vfr_hud_send(
chan, chan,
(float)g_gps->ground_speed / 100.0, (float)g_gps->ground_speed / 100.0f,
(float)g_gps->ground_speed / 100.0, (float)g_gps->ground_speed / 100.0f,
(ahrs.yaw_sensor / 100) % 360, (ahrs.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10, g.rc_3.servo_out/10,
current_loc.alt / 100.0, current_loc.alt / 100.0f,
climb_rate / 100.0); climb_rate / 100.0f);
} }
static void NOINLINE send_raw_imu1(mavlink_channel_t chan) static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
@ -449,12 +449,12 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
mavlink_msg_raw_imu_send( mavlink_msg_raw_imu_send(
chan, chan,
micros(), micros(),
accel.x * 1000.0 / GRAVITY_MSS, accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0 / GRAVITY_MSS, accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0 / GRAVITY_MSS, accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0, gyro.x * 1000.0f,
gyro.y * 1000.0, gyro.y * 1000.0f,
gyro.z * 1000.0, gyro.z * 1000.0f,
compass.mag_x, compass.mag_x,
compass.mag_y, compass.mag_y,
compass.mag_z); compass.mag_z);
@ -465,8 +465,8 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
mavlink_msg_scaled_pressure_send( mavlink_msg_scaled_pressure_send(
chan, chan,
millis(), millis(),
(float)barometer.get_pressure()/100.0, (float)barometer.get_pressure()/100.0f,
(float)(barometer.get_pressure() - barometer.get_ground_pressure())/100.0, (float)(barometer.get_pressure() - barometer.get_ground_pressure())/100.0f,
(int)(barometer.get_temperature()*10)); (int)(barometer.get_temperature()*10));
} }
@ -1344,10 +1344,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (tell_command.id < MAV_CMD_NAV_LAST) { if (tell_command.id < MAV_CMD_NAV_LAST) {
// command needs scaling // command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude) x = tell_command.lat/1.0e7f; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude) y = tell_command.lng/1.0e7f; // local (y), global (longitude)
// ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM
z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) z = tell_command.alt/1.0e2f; // local (z), global/relative (altitude)
} }
// Switch to map APM command fields into MAVLink command fields // Switch to map APM command fields into MAVLink command fields
@ -1594,7 +1594,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
* case MAV_FRAME_LOCAL: // local (relative to home position) * case MAV_FRAME_LOCAL: // local (relative to home position)
* { * {
* tell_command.lat = 1.0e7*ToDeg(packet.x/ * tell_command.lat = 1.0e7*ToDeg(packet.x/
* (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; * (radius_of_earth*cosf(ToRad(home.lat/1.0e7)))) + home.lat;
* tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; * tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
* tell_command.alt = packet.z*1.0e2; * tell_command.alt = packet.z*1.0e2;
* tell_command.options = MASK_OPTIONS_RELATIVE_ALT; * tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
@ -1613,9 +1613,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
*/ */
// we only are supporting Abs position, relative Alt // we only are supporting Abs position, relative Alt
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2; tell_command.alt = packet.z * 1.0e2f;
tell_command.options = 1; // store altitude relative to home alt!! Always!! tell_command.options = 1; // store altitude relative to home alt!! Always!!
switch (tell_command.id) { // Switch to map APM command fields into MAVLink command fields switch (tell_command.id) { // Switch to map APM command fields into MAVLink command fields
@ -1865,7 +1865,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_hil_state_decode(msg, &packet); mavlink_msg_hil_state_decode(msg, &packet);
float vel = pythagorous2(packet.vx, packet.vy); float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100); float cog = wrap_360(ToDeg(atan2f(packet.vx, packet.vy)) * 100);
// set gps hil sensor // set gps hil sensor
g_gps->setHIL(packet.time_usec/1000, g_gps->setHIL(packet.time_usec/1000,
@ -2050,8 +2050,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
send_text_P(SEVERITY_LOW,PSTR("bad fence point")); send_text_P(SEVERITY_LOW,PSTR("bad fence point"));
} else { } else {
Vector2l point; Vector2l point;
point.x = packet.lat*1.0e7; point.x = packet.lat*1.0e7f;
point.y = packet.lng*1.0e7; point.y = packet.lng*1.0e7f;
geofence_limit.set_fence_point_with_index(point, packet.idx); geofence_limit.set_fence_point_with_index(point, packet.idx);
} }
break; break;
@ -2067,7 +2067,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else { } else {
Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx); Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx);
mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(), mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(),
point.x*1.0e-7, point.y*1.0e-7); point.x*1.0e-7f, point.y*1.0e-7f);
} }
break; break;
} }

9
ArduCopter/Log.pde
View File

@ -312,8 +312,8 @@ static void Log_Write_Current()
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
throttle_in : g.rc_3.control_in, throttle_in : g.rc_3.control_in,
throttle_integrator : throttle_integrator, throttle_integrator : throttle_integrator,
battery_voltage : (int16_t) (battery_voltage1 * 100.0), battery_voltage : (int16_t) (battery_voltage1 * 100.0f),
current_amps : (int16_t) (current_amps1 * 100.0), current_amps : (int16_t) (current_amps1 * 100.0f),
current_total : (int16_t) current_total1 current_total : (int16_t) current_total1
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -329,8 +329,8 @@ static void Log_Read_Current()
cliSerial->printf_P(PSTR("CURR, %d, %lu, %4.4f, %4.4f, %d\n"), cliSerial->printf_P(PSTR("CURR, %d, %lu, %4.4f, %4.4f, %d\n"),
(int)pkt.throttle_in, (int)pkt.throttle_in,
(unsigned long)pkt.throttle_integrator, (unsigned long)pkt.throttle_integrator,
(float)pkt.battery_voltage/100.f, (float)pkt.battery_voltage/100.0f,
(float)pkt.current_amps/100.f, (float)pkt.current_amps/100.0f,
(int)pkt.current_total); (int)pkt.current_total);
} }
@ -1159,7 +1159,6 @@ static void Log_Read_Camera()
{ {
struct log_Camera pkt; struct log_Camera pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt)); DataFlash.ReadPacket(&pkt, sizeof(pkt));
// 1 // 1
cliSerial->printf_P(PSTR("CAMERA, %lu, "),(unsigned long)pkt.gps_time); // 1 time cliSerial->printf_P(PSTR("CAMERA, %lu, "),(unsigned long)pkt.gps_time); // 1 time
print_latlon(cliSerial, pkt.latitude); // 2 lat print_latlon(cliSerial, pkt.latitude); // 2 lat

6
ArduCopter/commands.pde
View File

@ -188,9 +188,9 @@ static void init_home()
Log_Write_Cmd(0, &home); Log_Write_Cmd(0, &home);
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles // update navigation scalers. used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; float rads = (fabsf((float)next_WP.lat)/t7) * 0.0174532925f;
scaleLongDown = cos(rads); scaleLongDown = cosf(rads);
scaleLongUp = 1.0f/cos(rads); scaleLongUp = 1.0f/cosf(rads);
// Save prev loc this makes the calcs look better before commands are loaded // Save prev loc this makes the calcs look better before commands are loaded
prev_WP = home; prev_WP = home;

4
ArduCopter/commands_logic.pde
View File

@ -824,7 +824,7 @@ static void do_repeat_servo()
event_timer = 0; event_timer = 0;
event_value = command_cond_queue.alt; event_value = command_cond_queue.alt;
event_repeat = command_cond_queue.lat * 2; event_repeat = command_cond_queue.lat * 2;
event_delay = command_cond_queue.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_delay = command_cond_queue.lng * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds)
switch(command_cond_queue.p1) { switch(command_cond_queue.p1) {
case CH_5: case CH_5:
@ -848,7 +848,7 @@ static void do_repeat_relay()
{ {
event_id = RELAY_TOGGLE; event_id = RELAY_TOGGLE;
event_timer = 0; event_timer = 0;
event_delay = command_cond_queue.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_delay = command_cond_queue.lat * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = command_cond_queue.alt * 2; event_repeat = command_cond_queue.alt * 2;
update_events(); update_events();
} }

130
ArduCopter/config.h
View File

@ -114,7 +114,7 @@
# define RC_FAST_SPEED 125 # define RC_FAST_SPEED 125
# define RTL_YAW YAW_LOOK_AT_HOME # define RTL_YAW YAW_LOOK_AT_HOME
# define TILT_COMPENSATION 5 # define TILT_COMPENSATION 5
# define RATE_INTEGRATOR_LEAK_RATE 0.02 # define RATE_INTEGRATOR_LEAK_RATE 0.02f
# define RATE_ROLL_D 0 # define RATE_ROLL_D 0
# define RATE_PITCH_D 0 # define RATE_PITCH_D 0
# define HELI_PITCH_FF 0 # define HELI_PITCH_FF 0
@ -377,17 +377,17 @@
// Battery monitoring // Battery monitoring
// //
#ifndef LOW_VOLTAGE #ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 9.6 # define LOW_VOLTAGE 9.6f
#endif #endif
#ifndef VOLT_DIV_RATIO #ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56 # define VOLT_DIV_RATIO 3.56f
#endif #endif
#ifndef CURR_AMP_PER_VOLT #ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32 # define CURR_AMP_PER_VOLT 27.32f
#endif #endif
#ifndef CURR_AMPS_OFFSET #ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0 # define CURR_AMPS_OFFSET 0.0f
#endif #endif
#ifndef HIGH_DISCHARGE #ifndef HIGH_DISCHARGE
# define HIGH_DISCHARGE 1760 # define HIGH_DISCHARGE 1760
@ -404,7 +404,7 @@
// INPUT_VOLTAGE // INPUT_VOLTAGE
// //
#ifndef INPUT_VOLTAGE #ifndef INPUT_VOLTAGE
# define INPUT_VOLTAGE 5.0 # define INPUT_VOLTAGE 5.0f
#endif #endif
@ -437,22 +437,22 @@
#endif #endif
// optical flow based loiter PI values // optical flow based loiter PI values
#ifndef OPTFLOW_ROLL_P #ifndef OPTFLOW_ROLL_P
#define OPTFLOW_ROLL_P 2.5 #define OPTFLOW_ROLL_P 2.5f
#endif #endif
#ifndef OPTFLOW_ROLL_I #ifndef OPTFLOW_ROLL_I
#define OPTFLOW_ROLL_I 0.5 #define OPTFLOW_ROLL_I 0.5f
#endif #endif
#ifndef OPTFLOW_ROLL_D #ifndef OPTFLOW_ROLL_D
#define OPTFLOW_ROLL_D 0.12 #define OPTFLOW_ROLL_D 0.12f
#endif #endif
#ifndef OPTFLOW_PITCH_P #ifndef OPTFLOW_PITCH_P
#define OPTFLOW_PITCH_P 2.5 #define OPTFLOW_PITCH_P 2.5f
#endif #endif
#ifndef OPTFLOW_PITCH_I #ifndef OPTFLOW_PITCH_I
#define OPTFLOW_PITCH_I 0.5 #define OPTFLOW_PITCH_I 0.5f
#endif #endif
#ifndef OPTFLOW_PITCH_D #ifndef OPTFLOW_PITCH_D
#define OPTFLOW_PITCH_D 0.12 #define OPTFLOW_PITCH_D 0.12f
#endif #endif
#ifndef OPTFLOW_IMAX #ifndef OPTFLOW_IMAX
#define OPTFLOW_IMAX 1 #define OPTFLOW_IMAX 1
@ -543,7 +543,7 @@
// Y6 Support // Y6 Support
#ifndef TOP_BOTTOM_RATIO #ifndef TOP_BOTTOM_RATIO
# define TOP_BOTTOM_RATIO 1.00 # define TOP_BOTTOM_RATIO 1.00f
#endif #endif
@ -738,26 +738,26 @@
// Extra motor values that are changed from time to time by jani @ jDrones as software // Extra motor values that are changed from time to time by jani @ jDrones as software
// and charachteristics changes. // and charachteristics changes.
#ifdef MOTORS_JD880 #ifdef MOTORS_JD880
# define STABILIZE_ROLL_P 3.7 # define STABILIZE_ROLL_P 3.7f
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.0f
# define STABILIZE_ROLL_IMAX 8.0 // degrees # define STABILIZE_ROLL_IMAX 8.0f // degrees
# define STABILIZE_PITCH_P 3.7 # define STABILIZE_PITCH_P 3.7f
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.0f
# define STABILIZE_PITCH_IMAX 8.0 // degrees # define STABILIZE_PITCH_IMAX 8.0f // degrees
#endif #endif
#ifdef MOTORS_JD850 #ifdef MOTORS_JD850
# define STABILIZE_ROLL_P 4.2 # define STABILIZE_ROLL_P 4.2f
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.0f
# define STABILIZE_ROLL_IMAX 8.0 // degrees # define STABILIZE_ROLL_IMAX 8.0f // degrees
# define STABILIZE_PITCH_P 4.2 # define STABILIZE_PITCH_P 4.2f
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.0f
# define STABILIZE_PITCH_IMAX 8.0 // degrees # define STABILIZE_PITCH_IMAX 8.0f // degrees
#endif #endif
#ifndef ACRO_P #ifndef ACRO_P
# define ACRO_P 4.5 # define ACRO_P 4.5f
#endif #endif
#ifndef AXIS_LOCK_ENABLED #ifndef AXIS_LOCK_ENABLED
@ -766,33 +766,33 @@
// Good for smaller payload motors. // Good for smaller payload motors.
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5 # define STABILIZE_ROLL_P 4.5f
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.0f
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 8.0 // degrees # define STABILIZE_ROLL_IMAX 8.0f // degrees
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.5 # define STABILIZE_PITCH_P 4.5f
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.0f
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 8.0 // degrees # define STABILIZE_PITCH_IMAX 8.0f // degrees
#endif #endif
#ifndef STABILIZE_YAW_P #ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 4.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy # define STABILIZE_YAW_P 4.5f // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif #endif
#ifndef STABILIZE_YAW_I #ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.0 # define STABILIZE_YAW_I 0.0f
#endif #endif
#ifndef STABILIZE_YAW_IMAX #ifndef STABILIZE_YAW_IMAX
# define STABILIZE_YAW_IMAX 8.0 // degrees * 100 # define STABILIZE_YAW_IMAX 8.0f // degrees * 100
#endif #endif
#ifndef YAW_LOOK_AHEAD_MIN_SPEED #ifndef YAW_LOOK_AHEAD_MIN_SPEED
@ -811,42 +811,42 @@
# define MAX_INPUT_PITCH_ANGLE 4500 # define MAX_INPUT_PITCH_ANGLE 4500
#endif #endif
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.150 # define RATE_ROLL_P 0.150f
#endif #endif
#ifndef RATE_ROLL_I #ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.100 # define RATE_ROLL_I 0.100f
#endif #endif
#ifndef RATE_ROLL_D #ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.004 # define RATE_ROLL_D 0.004f
#endif #endif
#ifndef RATE_ROLL_IMAX #ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 5.0 // degrees # define RATE_ROLL_IMAX 5.0f // degrees
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.150 # define RATE_PITCH_P 0.150f
#endif #endif
#ifndef RATE_PITCH_I #ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.100 # define RATE_PITCH_I 0.100f
#endif #endif
#ifndef RATE_PITCH_D #ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.004 # define RATE_PITCH_D 0.004f
#endif #endif
#ifndef RATE_PITCH_IMAX #ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 5.0 // degrees # define RATE_PITCH_IMAX 5.0f // degrees
#endif #endif
#ifndef RATE_YAW_P #ifndef RATE_YAW_P
# define RATE_YAW_P 0.25 # define RATE_YAW_P 0.25f
#endif #endif
#ifndef RATE_YAW_I #ifndef RATE_YAW_I
# define RATE_YAW_I 0.015 # define RATE_YAW_I 0.015f
#endif #endif
#ifndef RATE_YAW_D #ifndef RATE_YAW_D
# define RATE_YAW_D 0.000 # define RATE_YAW_D 0.000f
#endif #endif
#ifndef RATE_YAW_IMAX #ifndef RATE_YAW_IMAX
# define RATE_YAW_IMAX 8.0 // degrees # define RATE_YAW_IMAX 8.0f // degrees
#endif #endif
@ -882,10 +882,10 @@
// Loiter control gains // Loiter control gains
// //
#ifndef LOITER_P #ifndef LOITER_P
# define LOITER_P .20 # define LOITER_P .20f
#endif #endif
#ifndef LOITER_I #ifndef LOITER_I
# define LOITER_I 0.0 # define LOITER_I 0.0f
#endif #endif
#ifndef LOITER_IMAX #ifndef LOITER_IMAX
# define LOITER_IMAX 30 // degrees # define LOITER_IMAX 30 // degrees
@ -896,7 +896,7 @@
# define LOITER_REPOSITIONING DISABLED # define LOITER_REPOSITIONING DISABLED
#endif #endif
#ifndef LOITER_REPOSITION_RATE #ifndef LOITER_REPOSITION_RATE
# define LOITER_REPOSITION_RATE 500.0 // cm/s # define LOITER_REPOSITION_RATE 500.0f // cm/s
#endif #endif
@ -904,13 +904,13 @@
// Loiter Navigation control gains // Loiter Navigation control gains
// //
#ifndef LOITER_RATE_P #ifndef LOITER_RATE_P
# define LOITER_RATE_P 5.0 // # define LOITER_RATE_P 5.0f //
#endif #endif
#ifndef LOITER_RATE_I #ifndef LOITER_RATE_I
# define LOITER_RATE_I 0.04 // Wind control # define LOITER_RATE_I 0.04f // Wind control
#endif #endif
#ifndef LOITER_RATE_D #ifndef LOITER_RATE_D
# define LOITER_RATE_D 0.40 // try 2 or 3 for LOITER_RATE 1 # define LOITER_RATE_D 0.40f // try 2 or 3 for LOITER_RATE 1
#endif #endif
#ifndef LOITER_RATE_IMAX #ifndef LOITER_RATE_IMAX
# define LOITER_RATE_IMAX 30 // degrees # define LOITER_RATE_IMAX 30 // degrees
@ -920,13 +920,13 @@
// WP Navigation control gains // WP Navigation control gains
// //
#ifndef NAV_P #ifndef NAV_P
# define NAV_P 2.4 // # define NAV_P 2.4f //
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0.17 // Wind control # define NAV_I 0.17f // Wind control
#endif #endif
#ifndef NAV_D #ifndef NAV_D
# define NAV_D 0.00 // .95 # define NAV_D 0.00f // .95
#endif #endif
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 18 // degrees # define NAV_IMAX 18 // degrees
@ -963,10 +963,10 @@
#endif #endif
#ifndef ALT_HOLD_P #ifndef ALT_HOLD_P
# define ALT_HOLD_P 2.0 # define ALT_HOLD_P 2.0f
#endif #endif
#ifndef ALT_HOLD_I #ifndef ALT_HOLD_I
# define ALT_HOLD_I 0.0 # define ALT_HOLD_I 0.0f
#endif #endif
#ifndef ALT_HOLD_IMAX #ifndef ALT_HOLD_IMAX
# define ALT_HOLD_IMAX 300 # define ALT_HOLD_IMAX 300
@ -974,13 +974,13 @@
// RATE control // RATE control
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 6.0 # define THROTTLE_P 6.0f
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.0 # define THROTTLE_I 0.0f
#endif #endif
#ifndef THROTTLE_D #ifndef THROTTLE_D
# define THROTTLE_D 0.2 # define THROTTLE_D 0.2f
#endif #endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
@ -1004,13 +1004,13 @@
// Throttle Accel control // Throttle Accel control
#ifndef THROTTLE_ACCEL_P #ifndef THROTTLE_ACCEL_P
# define THROTTLE_ACCEL_P 0.75 # define THROTTLE_ACCEL_P 0.75f
#endif #endif
#ifndef THROTTLE_ACCEL_I #ifndef THROTTLE_ACCEL_I
# define THROTTLE_ACCEL_I 1.50 # define THROTTLE_ACCEL_I 1.50f
#endif #endif
#ifndef THROTTLE_ACCEL_D #ifndef THROTTLE_ACCEL_D
# define THROTTLE_ACCEL_D 0.0 # define THROTTLE_ACCEL_D 0.0f
#endif #endif
#ifndef THROTTLE_ACCEL_IMAX #ifndef THROTTLE_ACCEL_IMAX
# define THROTTLE_ACCEL_IMAX 500 # define THROTTLE_ACCEL_IMAX 500
@ -1021,7 +1021,7 @@
// Crosstrack compensation // Crosstrack compensation
// //
#ifndef CROSSTRACK_GAIN #ifndef CROSSTRACK_GAIN
# define CROSSTRACK_GAIN .2 # define CROSSTRACK_GAIN .2f
#endif #endif
#ifndef CROSSTRACK_MIN_DISTANCE #ifndef CROSSTRACK_MIN_DISTANCE
# define CROSSTRACK_MIN_DISTANCE 15 # define CROSSTRACK_MIN_DISTANCE 15

8
ArduCopter/control_modes.pde
View File

@ -176,8 +176,8 @@ static void read_trim_switch()
static void save_trim() static void save_trim()
{ {
// save roll and pitch trim // save roll and pitch trim
float roll_trim = ToRad((float)g.rc_1.control_in/100.0); float roll_trim = ToRad((float)g.rc_1.control_in/100.0f);
float pitch_trim = ToRad((float)g.rc_2.control_in/100.0); float pitch_trim = ToRad((float)g.rc_2.control_in/100.0f);
ahrs.add_trim(roll_trim, pitch_trim); ahrs.add_trim(roll_trim, pitch_trim);
} }
@ -192,10 +192,10 @@ static void auto_trim()
led_mode = SAVE_TRIM_LEDS; led_mode = SAVE_TRIM_LEDS;
// calculate roll trim adjustment // calculate roll trim adjustment
float roll_trim_adjustment = ToRad((float)-g.rc_1.control_in / 4000.0); float roll_trim_adjustment = ToRad((float)-g.rc_1.control_in / 4000.0f);
// calculate pitch trim adjustment // calculate pitch trim adjustment
float pitch_trim_adjustment = ToRad((float)g.rc_2.control_in / 4000.0); float pitch_trim_adjustment = ToRad((float)g.rc_2.control_in / 4000.0f);
// make sure accelerometer values impact attitude quickly // make sure accelerometer values impact attitude quickly
ahrs.set_fast_gains(true); ahrs.set_fast_gains(true);

8
ArduCopter/defines.h
View File

@ -346,8 +346,8 @@ enum gcs_severity {
#define DATA_RTL_REACHED_ALT 31 #define DATA_RTL_REACHED_ALT 31
// battery monitoring macros // battery monitoring macros
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio #define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0f))*g.volt_div_ratio
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt #define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0f))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
/* ************************************************************** */ /* ************************************************************** */
/* Expansion PIN's that people can use for various things. */ /* Expansion PIN's that people can use for various things. */
@ -394,8 +394,8 @@ enum gcs_severity {
#define PIEZO_PIN AN5 //Last pin on the back ADC connector #define PIEZO_PIN AN5 //Last pin on the back ADC connector
// RADIANS // RADIANS
#define RADX100 0.000174532925 #define RADX100 0.000174532925f
#define DEGX100 5729.57795 #define DEGX100 5729.57795f
// EEPROM addresses // EEPROM addresses

24
ArduCopter/navigation.pde
View File

@ -13,11 +13,11 @@ static void update_navigation()
// used to calculate speed in X and Y, iterms // used to calculate speed in X and Y, iterms
// ------------------------------------------ // ------------------------------------------
dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0; dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0f;
nav_last_gps_update = millis(); nav_last_gps_update = millis();
// prevent runup from bad GPS // prevent runup from bad GPS
dTnav = min(dTnav, 1.0); dTnav = min(dTnav, 1.0f);
// save GPS time // save GPS time
nav_last_gps_time = g_gps->time; nav_last_gps_time = g_gps->time;
@ -34,7 +34,7 @@ static void update_navigation()
// check for inertial nav updates // check for inertial nav updates
if( inertial_nav.position_ok() ) { if( inertial_nav.position_ok() ) {
// 50hz // 50hz
dTnav = 0.02; // To-Do: calculate the time from the mainloop or INS readings? dTnav = 0.02f; // To-Do: calculate the time from the mainloop or INS readings?
// signal to run nav controllers // signal to run nav controllers
pos_updated = true; pos_updated = true;
@ -108,7 +108,7 @@ static void calc_velocity_and_position(){
} }
// this speed is ~ in cm because we are using 10^7 numbers from GPS // this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0/dTnav; float tmp = 1.0f/dTnav;
#if INERTIAL_NAV_XY == ENABLED #if INERTIAL_NAV_XY == ENABLED
if( inertial_nav.position_ok() ) { if( inertial_nav.position_ok() ) {
@ -338,11 +338,11 @@ static void update_nav_wp()
//1 degree = 0.0174532925 radians //1 degree = 0.0174532925 radians
// wrap // wrap
if (circle_angle > 6.28318531) if (circle_angle > 6.28318531f)
circle_angle -= 6.28318531; circle_angle -= 6.28318531f;
next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp);
next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sin(1.57 - circle_angle)); next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle));
// use error as the desired rate towards the target // use error as the desired rate towards the target
// nav_lon, nav_lat is calculated // nav_lon, nav_lat is calculated
@ -563,8 +563,8 @@ static void calc_nav_rate(int16_t max_speed)
// rotate by 90 to deal with trig functions // rotate by 90 to deal with trig functions
temp = (9000l - wp_bearing) * RADX100; temp = (9000l - wp_bearing) * RADX100;
temp_x = cos(temp); temp_x = cosf(temp);
temp_y = sin(temp); temp_y = sinf(temp);
// rotate desired spped vector: // rotate desired spped vector:
int32_t x_target_speed = max_speed * temp_x - cross_speed * temp_y; int32_t x_target_speed = max_speed * temp_x - cross_speed * temp_y;
@ -633,7 +633,7 @@ static int16_t get_desired_speed(int16_t max_speed)
int32_t s_min = WAYPOINT_SPEED_MIN; int32_t s_min = WAYPOINT_SPEED_MIN;
temp += s_min * s_min; temp += s_min * s_min;
if( temp < 0 ) temp = 0; // check to ensure we don't try to take the sqrt of a negative number if( temp < 0 ) temp = 0; // check to ensure we don't try to take the sqrt of a negative number
max_speed = sqrt((float)temp); max_speed = sqrtf((float)temp);
max_speed = min(max_speed, g.waypoint_speed_max); max_speed = min(max_speed, g.waypoint_speed_max);
} }
} }
@ -657,7 +657,7 @@ static void update_crosstrack(void)
abs(wrap_180(wp_bearing - original_wp_bearing)) < 4500) { abs(wrap_180(wp_bearing - original_wp_bearing)) < 4500) {
float temp = (wp_bearing - original_wp_bearing) * RADX100; float temp = (wp_bearing - original_wp_bearing) * RADX100;
crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line crosstrack_error = sinf(temp) * wp_distance; // Meters we are off track line
}else{ }else{
// fade out crosstrack // fade out crosstrack
crosstrack_error >>= 1; crosstrack_error >>= 1;

12
ArduCopter/sensors.pde
View File

@ -10,9 +10,9 @@ static void ReadSCP1000(void) {
static void init_sonar(void) static void init_sonar(void)
{ {
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
sonar->calculate_scaler(g.sonar_type, 3.3); sonar->calculate_scaler(g.sonar_type, 3.3f);
#else #else
sonar->calculate_scaler(g.sonar_type, 5.0); sonar->calculate_scaler(g.sonar_type, 5.0f);
#endif #endif
} }
#endif #endif
@ -28,7 +28,7 @@ static void init_barometer(void)
static int32_t read_barometer(void) static int32_t read_barometer(void)
{ {
barometer.read(); barometer.read();
return baro_filter.apply(barometer.get_altitude() * 100.0); return baro_filter.apply(barometer.get_altitude() * 100.0f);
} }
// return sonar altitude in centimeters // return sonar altitude in centimeters
@ -37,7 +37,7 @@ static int16_t read_sonar(void)
#if CONFIG_SONAR == ENABLED #if CONFIG_SONAR == ENABLED
int16_t temp_alt = sonar->read(); int16_t temp_alt = sonar->read();
if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70) { if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70f) {
if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) { if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
sonar_alt_health++; sonar_alt_health++;
} }
@ -48,7 +48,7 @@ static int16_t read_sonar(void)
#if SONAR_TILT_CORRECTION == 1 #if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar // correct alt for angle of the sonar
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = max(temp, 0.707); temp = max(temp, 0.707f);
temp_alt = (float)temp_alt * temp; temp_alt = (float)temp_alt * temp;
#endif #endif
@ -117,7 +117,7 @@ static void read_battery(void)
if(g.battery_monitoring == 4) { if(g.battery_monitoring == 4) {
batt_curr_analog_source->set_pin(g.battery_curr_pin); batt_curr_analog_source->set_pin(g.battery_curr_pin);
current_amps1 = CURRENT_AMPS(batt_curr_analog_source->read_average()); current_amps1 = CURRENT_AMPS(batt_curr_analog_source->read_average());
current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) current_total1 += current_amps1 * 0.02778f; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
} }
// check for low voltage or current if the low voltage check hasn't already been triggered // check for low voltage or current if the low voltage check hasn't already been triggered

6
ArduCopter/test.pde
View File

@ -201,7 +201,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
for(int16_t i = 0; i < 200; i++){ for(int16_t i = 0; i < 200; i++){
int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100); int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100);
max_speed = sqrt((float)temp); max_speed = sqrtf((float)temp);
max_speed = min(max_speed, g.waypoint_speed_max); max_speed = min(max_speed, g.waypoint_speed_max);
cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance); cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance);
wp_distance += 100; wp_distance += 100;
@ -561,8 +561,8 @@ test_gps(uint8_t argc, const Menu::arg *argv)
* temp.c.x, temp.c.y, temp.c.z); * temp.c.x, temp.c.y, temp.c.z);
* *
* int16_t _pitch = degrees(-asin(temp.c.x)); * int16_t _pitch = degrees(-asin(temp.c.x));
* int16_t _roll = degrees(atan2(temp.c.y, temp.c.z)); * int16_t _roll = degrees(atan2f(temp.c.y, temp.c.z));
* int16_t _yaw = degrees(atan2(temp.b.x, temp.a.x)); * int16_t _yaw = degrees(atan2f(temp.b.x, temp.a.x));
* cliSerial->printf_P(PSTR( "angles\n" * cliSerial->printf_P(PSTR( "angles\n"
* "%d \t %d \t %d\n\n"), * "%d \t %d \t %d\n\n"),
* _pitch, * _pitch,

2
ArduCopter/toy.pde
View File

@ -116,7 +116,7 @@ void roll_pitch_toy()
get_acro_yaw(0); get_acro_yaw(0);
yaw_timer--; yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) { if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) {
ap_system.yaw_stopped = true; ap_system.yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
} }

12
ArduPlane/Attitude.pde
View File

@ -94,7 +94,7 @@ static void stabilize_pitch(float speed_scaler)
{ {
#if APM_CONTROL == DISABLED #if APM_CONTROL == DISABLED
int32_t tempcalc = nav_pitch_cd + int32_t tempcalc = nav_pitch_cd +
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) + fabsf(ahrs.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(ahrs.pitch_sensor - g.pitch_trim_cd); (ahrs.pitch_sensor - g.pitch_trim_cd);
if (inverted_flight) { if (inverted_flight) {
@ -123,12 +123,12 @@ static void stabilize_stick_mixing()
float ch2_inf; float ch2_inf;
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf); ch1_inf = fabsf(ch1_inf);
ch1_inf = min(ch1_inf, 400.0); ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0); ch1_inf = ((400.0 - ch1_inf) /400.0);
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
ch2_inf = fabs(ch2_inf); ch2_inf = fabsf(ch2_inf);
ch2_inf = min(ch2_inf, 400.0); ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0); ch2_inf = ((400.0 - ch2_inf) /400.0);
@ -156,7 +156,7 @@ static void stabilize_yaw(float speed_scaler)
// important for steering on the ground during landing // important for steering on the ground during landing
// ----------------------------------------------- // -----------------------------------------------
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf); ch4_inf = fabsf(ch4_inf);
ch4_inf = min(ch4_inf, 400.0); ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0); ch4_inf = ((400.0 - ch4_inf) /400.0);
} }
@ -328,7 +328,7 @@ static void calc_nav_roll()
} }
// Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity. // Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
nav_roll = ToDeg(atan(speed*turn_rate/GRAVITY_MSS)*100); nav_roll = ToDeg(atanf(speed*turn_rate/GRAVITY_MSS)*100);
#else #else
// this is the old nav_roll calculation. We will use this for 2.50 // this is the old nav_roll calculation. We will use this for 2.50
@ -362,7 +362,7 @@ static void throttle_slew_limit(int16_t last_throttle)
// if slew limit rate is set to zero then do not slew limit // if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) { if (g.throttle_slewrate) {
// limit throttle change by the given percentage per second // limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01 * fabs(g.channel_throttle.radio_max - g.channel_throttle.radio_min); float temp = g.throttle_slewrate * G_Dt * 0.01 * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min);
// allow a minimum change of 1 PWM per cycle // allow a minimum change of 1 PWM per cycle
if (temp < 1) { if (temp < 1) {
temp = 1; temp = 1;

32
ArduPlane/GCS_Mavlink.pde
View File

@ -479,7 +479,7 @@ static void NOINLINE send_wind(mavlink_channel_t chan)
Vector3f wind = ahrs.wind_estimate(); Vector3f wind = ahrs.wind_estimate();
mavlink_msg_wind_send( mavlink_msg_wind_send(
chan, chan,
degrees(atan2(-wind.y, -wind.x)), // use negative, to give degrees(atan2f(-wind.y, -wind.x)), // use negative, to give
// direction wind is coming from // direction wind is coming from
wind.length(), wind.length(),
wind.z); wind.z);
@ -1467,9 +1467,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_MISSION: case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL:
{ {
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm tell_command.alt = packet.z*1.0e2f; // in as m converted to cm
tell_command.options = 0; // absolute altitude tell_command.options = 0; // absolute altitude
break; break;
} }
@ -1477,10 +1477,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#ifdef MAV_FRAME_LOCAL_NED #ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position) case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{ {
tell_command.lat = 1.0e7*ToDeg(packet.x/ tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = -packet.z*1.0e2; tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break; break;
} }
@ -1489,10 +1489,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#ifdef MAV_FRAME_LOCAL #ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position) case MAV_FRAME_LOCAL: // local (relative to home position)
{ {
tell_command.lat = 1.0e7*ToDeg(packet.x/ tell_command.lat = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2; tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break; break;
} }
@ -1500,9 +1500,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{ {
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2; tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break; break;
} }
@ -1800,7 +1800,7 @@ mission_failed:
mavlink_msg_hil_state_decode(msg, &packet); mavlink_msg_hil_state_decode(msg, &packet);
float vel = pythagorous2(packet.vx, packet.vy); float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360_cd(ToDeg(atan2(packet.vy, packet.vx)) * 100); float cog = wrap_360_cd(ToDeg(atan2f(packet.vy, packet.vx)) * 100);
if (g_gps != NULL) { if (g_gps != NULL) {
// set gps hil sensor // set gps hil sensor

10
ArduPlane/navigation.pde
View File

@ -53,7 +53,7 @@ static void navigate()
// Disabled for now // Disabled for now
void calc_distance_error() void calc_distance_error()
{ {
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error_cd * .01)); distance_estimate += (float)g_gps->ground_speed * .0002f * cosf(radians(bearing_error_cd * .01f));
distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
wp_distance = max(distance_estimate,10); wp_distance = max(distance_estimate,10);
} }
@ -165,7 +165,7 @@ static void update_loiter()
* if (wp_distance < g.loiter_radius){ * if (wp_distance < g.loiter_radius){
* nav_bearing += 9000; * nav_bearing += 9000;
* }else{ * }else{
* nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); * nav_bearing -= 100 * M_PI / 180 * asinf(g.loiter_radius / wp_distance);
* } * }
* *
* update_crosstrack(); * update_crosstrack();
@ -182,9 +182,9 @@ static void update_crosstrack(void)
Vector2f wind2d = Vector2f(wind.x, wind.y); Vector2f wind2d = Vector2f(wind.x, wind.y);
float speed; float speed;
if (ahrs.airspeed_estimate(&speed)) { if (ahrs.airspeed_estimate(&speed)) {
Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed; Vector2f nav_vector = Vector2f(cosf(radians(nav_bearing_cd*0.01)), sinf(radians(nav_bearing_cd*0.01))) * speed;
Vector2f nav_adjusted = nav_vector - wind2d; Vector2f nav_adjusted = nav_vector - wind2d;
nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100; nav_bearing_cd = degrees(atan2f(nav_adjusted.y, nav_adjusted.x)) * 100;
} }
} }
@ -194,7 +194,7 @@ static void update_crosstrack(void)
if (wp_totalDistance >= g.crosstrack_min_distance && if (wp_totalDistance >= g.crosstrack_min_distance &&
abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
// Meters we are off track line // Meters we are off track line
crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;
nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing_cd = wrap_360_cd(nav_bearing_cd); nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
} }

4
Tools/ArduTracker/ArduTracker.pde
View File

@ -742,8 +742,8 @@ void update_GPS(void)
// Calculate new climb rate // Calculate new climb rate
add_altitude_data(millis()/100, gps.altitude/10); add_altitude_data(millis()/100, gps.altitude/10);
COGX = cos(ToRad(gps.ground_course/100.0)); COGX = cosf(ToRad(gps.ground_course/100.0));
COGY = sin(ToRad(gps.ground_course/100.0)); COGY = sinf(ToRad(gps.ground_course/100.0));
} }
} }

38
Tools/ArduTracker/Attitude.pde
View File

@ -6,21 +6,21 @@
void stabilize() void stabilize()
{ {
float ch1_inf = 1.0; float ch1_inf = 1.0f;
float ch2_inf = 1.0; float ch2_inf = 1.0f;
float ch4_inf = 1.0; float ch4_inf = 1.0f;
#if AIRSPEED_SENSOR == ENABLED #if AIRSPEED_SENSOR == ENABLED
float speed_scaler = STANDARD_SPEED_SQUARED / (airspeed * airspeed); float speed_scaler = STANDARD_SPEED_SQUARED / (airspeed * airspeed);
speed_scaler = constrain(speed_scaler, 0.11, 9.0); speed_scaler = constrain(speed_scaler, 0.11f, 9.0f);
#endif #endif
#if AIRSPEED_SENSOR == DISABLED #if AIRSPEED_SENSOR == DISABLED
float speed_scaler; float speed_scaler;
if (servo_out[CH_THROTTLE] > 0) if (servo_out[CH_THROTTLE] > 0)
speed_scaler = 0.5 + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0); // First order taylor expansion of square root speed_scaler = 0.5f + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0f); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't goint to implement that... // Should maybe be to the 2/7 power, but we aren't goint to implement that...
else else
speed_scaler = 1.67; speed_scaler = 1.67f;
speed_scaler = constrain(speed_scaler, 0.6, 1.67); // This case is constrained tighter as we don't have real speed info speed_scaler = constrain(speed_scaler, 0.6f, 1.67f); // This case is constrained tighter as we don't have real speed info
#endif #endif
@ -36,7 +36,7 @@ void stabilize()
// Calculate dersired servo output for the roll // Calculate dersired servo output for the roll
// --------------------------------------------- // ---------------------------------------------
servo_out[CH_ROLL] = pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), deltaMiliSeconds, speed_scaler); servo_out[CH_ROLL] = pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), deltaMiliSeconds, speed_scaler);
servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabs(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler); servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler);
//Serial.print(" servo_out[CH_ROLL] "); //Serial.print(" servo_out[CH_ROLL] ");
//Serial.print(servo_out[CH_ROLL],DEC); //Serial.print(servo_out[CH_ROLL],DEC);
@ -45,12 +45,12 @@ void stabilize()
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) {
ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim(CH_ROLL); ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim(CH_ROLL);
ch1_inf = fabs(ch1_inf); ch1_inf = fabsf(ch1_inf);
ch1_inf = min(ch1_inf, 400.0); ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0); ch1_inf = ((400.0 - ch1_inf) /400.0);
ch2_inf = (float)radio_in[CH_PITCH] - radio_trim(CH_PITCH); ch2_inf = (float)radio_in[CH_PITCH] - radio_trim(CH_PITCH);
ch2_inf = fabs(ch2_inf); ch2_inf = fabsf(ch2_inf);
ch2_inf = min(ch2_inf, 400.0); ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0); ch2_inf = ((400.0 - ch2_inf) /400.0);
@ -73,7 +73,7 @@ void stabilize()
// ----------------------------------------------- // -----------------------------------------------
if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) {
ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim(CH_RUDDER); ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim(CH_RUDDER);
ch4_inf = fabs(ch4_inf); ch4_inf = fabsf(ch4_inf);
ch4_inf = min(ch4_inf, 400.0); ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0); ch4_inf = ((400.0 - ch4_inf) /400.0);
} }
@ -249,19 +249,19 @@ void set_servos_4(void)
float x1,x2,y1,y2,x,y,r,z; float x1,x2,y1,y2,x,y,r,z;
y1 = 110600*current_loc.lat/t7; y1 = 110600*current_loc.lat/t7;
x1 = (PI/180)*6378137*(cos(atan(0.99664719*tan(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7); x1 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7);
y2 = 110600*trackVehicle_loc.lat/t7; y2 = 110600*trackVehicle_loc.lat/t7;
x2 = (PI/180)*6378137*(cos(atan(0.99664719*tan(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7); x2 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7);
x = abs(x2 - x1); x = abs(x2 - x1);
y = abs(y2 - y1); y = abs(y2 - y1);
r = sqrt(x*x+y*y); r = sqrtf(x*x+y*y);
z = trackVehicle_loc.alt/100.0 - current_loc.alt; z = trackVehicle_loc.alt/100.0f - current_loc.alt;
phi = (atan(z/r)*180/PI); phi = (atanf(z/r)*180/PI);
theta = (atan(x/y)*180/PI); theta = (atanf(x/y)*180/PI);
// Check to see which quadrant of the angle // Check to see which quadrant of the angle
if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng) if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng)
@ -294,8 +294,8 @@ void set_servos_4(void)
Serial.print("theta: "); Serial.println(theta); Serial.print("theta: "); Serial.println(theta);
// Outputing to the servos // Outputing to the servos
servo_out[CH_ROLL] = 10000*phi/90.0; servo_out[CH_ROLL] = 10000*phi/90.0f;
servo_out[CH_PITCH] = 10000*theta/360.0; servo_out[CH_PITCH] = 10000*theta/360.0f;
servo_out[CH_RUDDER] = 0; servo_out[CH_RUDDER] = 0;
servo_out[CH_THROTTLE] = 0; servo_out[CH_THROTTLE] = 0;

12
Tools/ArduTracker/GCS_DebugTerminal.pde
View File

@ -781,8 +781,8 @@ GCS_DEBUGTERMINAL::run_debugt_command(char *buf)
port->printf_P(PSTR("nav_pitch (%.2f) - pitch_sensor (%.2f) + pitch_comp (%.2f) = %.2f\n"), port->printf_P(PSTR("nav_pitch (%.2f) - pitch_sensor (%.2f) + pitch_comp (%.2f) = %.2f\n"),
(float)nav_pitch / 100, (float)nav_pitch / 100,
(float)dcm.pitch_sensor / 100, (float)dcm.pitch_sensor / 100,
fabs(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) / 100, fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) / 100,
(float)(nav_pitch-dcm.pitch_sensor + fabs(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP))) / 100); (float)(nav_pitch-dcm.pitch_sensor + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP))) / 100);
port->printf_P(PSTR("servo_out[CH_PITCH] (%.2f) = PID[nav_pitch + pitch_comp - pitch_sensor]"), port->printf_P(PSTR("servo_out[CH_PITCH] (%.2f) = PID[nav_pitch + pitch_comp - pitch_sensor]"),
(float)servo_out[CH_PITCH] / 100); (float)servo_out[CH_PITCH] / 100);
@ -1275,12 +1275,12 @@ void
GCS_DEBUGTERMINAL::print_rlocation(Location *wp) GCS_DEBUGTERMINAL::print_rlocation(Location *wp)
{ {
//float x, y; //float x, y;
//y = (float)(wp->lat - current_loc.lat) * 0.0111194927; //y = (float)(wp->lat - current_loc.lat) * 0.0111194927f;
//x = (float)(wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927; //x = (float)(wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f;
BetterStream *port = _port; BetterStream *port = _port;
int x, y; int x, y;
y = (wp->lat - current_loc.lat) * 0.0111194927; y = (wp->lat - current_loc.lat) * 0.0111194927f;
x = (wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927; x = (wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f;
port->printf_P(PSTR("dP = <%d%c, %d%c, %.1f> meters\n"), port->printf_P(PSTR("dP = <%d%c, %d%c, %.1f> meters\n"),
abs(y), (y >= 0 ? 'N' : 'S'), abs(y), (y >= 0 ? 'N' : 'S'),
abs(x), (x >= 0 ? 'E' : 'W'), abs(x), (x >= 0 ? 'E' : 'W'),

14
Tools/ArduTracker/GCS_Mavlink.pde
View File

@ -490,18 +490,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{ {
case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL:
{ {
tell_command.lng = 1.0e7*packet.x; tell_command.lng = 1.0e7f*packet.x;
tell_command.lat = 1.0e7*packet.y; tell_command.lat = 1.0e7f*packet.y;
tell_command.alt = packet.z*1.0e2; tell_command.alt = packet.z*1.0e2f;
break; break;
} }
case MAV_FRAME_LOCAL: // local (relative to home position) case MAV_FRAME_LOCAL: // local (relative to home position)
{ {
tell_command.lng = 1.0e7*ToDeg(packet.x/ tell_command.lng = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng; (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lng;
tell_command.lat = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat; tell_command.lat = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lat;
tell_command.alt = -packet.z*1.0e2 + home.alt; tell_command.alt = -packet.z*1.0e2f + home.alt;
break; break;
} }
} }

22
Tools/ArduTracker/Mavlink_Common.h
View File

@ -120,26 +120,26 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
{ {
float gamma = dcm.pitch; // neglecting angle of attack for now float gamma = dcm.pitch; // neglecting angle of attack for now
float yaw = dcm.yaw; float yaw = dcm.yaw;
mavlink_msg_global_position_send(chan,timeStamp,current_loc.lat/1.0e7, mavlink_msg_global_position_send(chan,timeStamp,current_loc.lat/1.0e7f,
current_loc.lng/1.0e7,current_loc.alt/1.0e2,gps.ground_speed/1.0e2*cos(gamma)*cos(yaw), current_loc.lng/1.0e7f,current_loc.alt/1.0e2f,gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw),
gps.ground_speed/1.0e2*cos(gamma)*sin(yaw),gps.ground_speed/1.0e2*sin(gamma)); gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma));
break; break;
} }
case MSG_LOCAL_LOCATION: case MSG_LOCAL_LOCATION:
{ {
float gamma = dcm.pitch; // neglecting angle of attack for now float gamma = dcm.pitch; // neglecting angle of attack for now
float yaw = dcm.yaw; float yaw = dcm.yaw;
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth, mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7f)*radius_of_earth,
ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)), ToRad((current_loc.lng-home.lng)/1.0e7f)*radius_of_earth*cosf(ToRad(home.lat/1.0e7f)),
(current_loc.alt-home.alt)/1.0e2, gps.ground_speed/1.0e2*cos(gamma)*cos(yaw), (current_loc.alt-home.alt)/1.0e2f, gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw),
gps.ground_speed/1.0e2*cos(gamma)*sin(yaw),gps.ground_speed/1.0e2*sin(gamma)); gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma));
break; break;
} }
case MSG_GPS_RAW: case MSG_GPS_RAW:
{ {
mavlink_msg_gps_raw_send(chan,timeStamp,gps.status(), mavlink_msg_gps_raw_send(chan,timeStamp,gps.status(),
gps.latitude/1.0e7,gps.longitude/1.0e7,gps.altitude/100.0, gps.latitude/1.0e7f,gps.longitude/1.0e7f,gps.altitude/100.0f,
2.0,10.0,gps.ground_speed/100.0,gps.ground_course/100.0); 2.0f,10.0f,gps.ground_speed/100.0f,gps.ground_course/100.0f);
break; break;
} }
case MSG_AIRSPEED: case MSG_AIRSPEED:
@ -179,8 +179,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(chan,timeStamp, mavlink_msg_raw_imu_send(chan,timeStamp,
accel.x*1000.0/gravity,accel.y*1000.0/gravity,accel.z*1000.0/gravity, accel.x*1000.0f/gravity,accel.y*1000.0f/gravity,accel.z*1000.0f/gravity,
gyro.x*1000.0,gyro.y*1000.0,gyro.z*1000.0, gyro.x*1000.0f,gyro.y*1000.0f,gyro.z*1000.0f,
compass.mag_x,compass.mag_y,compass.mag_z); compass.mag_x,compass.mag_y,compass.mag_z);
mavlink_msg_raw_pressure_send(chan,timeStamp, mavlink_msg_raw_pressure_send(chan,timeStamp,
adc.Ch(AIRSPEED_CH),pitot.RawPress,0); adc.Ch(AIRSPEED_CH),pitot.RawPress,0);

6
Tools/ArduTracker/commands.pde
View File

@ -178,10 +178,10 @@ void set_next_WP(struct Location *wp)
loiter_sum = 0; loiter_sum = 0;
loiter_total = 0; loiter_total = 0;
float rads = (abs(next_WP.lat)/t7) * 0.0174532925; float rads = (abs(next_WP.lat)/t7) * 0.0174532925f;
//377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943 //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943
scaleLongDown = cos(rads); scaleLongDown = cosf(rads);
scaleLongUp = 1.0f/cos(rads); scaleLongUp = 1.0f/cosf(rads);
// this is handy for the groundstation // this is handy for the groundstation
wp_totalDistance = getDistance(&current_loc, &next_WP); wp_totalDistance = getDistance(&current_loc, &next_WP);

10
Tools/ArduTracker/navigation.pde
View File

@ -91,7 +91,7 @@ void update_navigation()
Disabled for now Disabled for now
void calc_distance_error() void calc_distance_error()
{ {
//distance_estimate += (float)gps.ground_speed * .0002 * cos(radians(bearing_error * .01)); //distance_estimate += (float)gps.ground_speed * .0002f * cosf(radians(bearing_error * .01f));
//distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
//wp_distance = max(distance_estimate,10); //wp_distance = max(distance_estimate,10);
} }
@ -134,7 +134,7 @@ void calc_altitude_error()
// special thanks to Ryan Beall for this one // special thanks to Ryan Beall for this one
float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°)
pitch_angle = constrain(pitch_angle, -2000, 2000); pitch_angle = constrain(pitch_angle, -2000, 2000);
float scale = sin(radians(pitch_angle * .01)); float scale = sinf(radians(pitch_angle * .01));
altitude_estimate += (float)airspeed * .0002 * scale; altitude_estimate += (float)airspeed * .0002 * scale;
altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt);
@ -195,7 +195,7 @@ void update_crosstrack(void)
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * get(PARAM_XTRACK_GAIN), -get(PARAM_XTRACK_ANGLE), get(PARAM_XTRACK_ANGLE)); nav_bearing += constrain(crosstrack_error * get(PARAM_XTRACK_GAIN), -get(PARAM_XTRACK_ANGLE), get(PARAM_XTRACK_ANGLE));
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
} }
@ -223,7 +223,7 @@ long getDistance(struct Location *loc1, struct Location *loc2)
return -1; return -1;
float dlat = (float)(loc2->lat - loc1->lat); float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195; return sqrtf(sq(dlat) + sq(dlong)) * .01113195f;
} }
long get_alt_distance(struct Location *loc1, struct Location *loc2) long get_alt_distance(struct Location *loc1, struct Location *loc2)
@ -235,7 +235,7 @@ long get_bearing(struct Location *loc1, struct Location *loc2)
{ {
long off_x = loc2->lng - loc1->lng; long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp; long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; long bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f;
if (bearing < 0) bearing += 36000; if (bearing < 0) bearing += 36000;
return bearing; return bearing;
} }

2
Tools/ArduTracker/sensors.pde
View File

@ -28,7 +28,7 @@ void read_airspeed(void)
airpressure_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75); airpressure_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75);
airpressure = (int)airpressure_raw - airpressure_offset; airpressure = (int)airpressure_raw - airpressure_offset;
airpressure = max(airpressure, 0); airpressure = max(airpressure, 0);
airspeed = sqrt((float)airpressure * get(PARAM_ARSPD_RATIO)) * 100; airspeed = sqrtf((float)airpressure * get(PARAM_ARSPD_RATIO)) * 100;
#endif #endif
calc_airspeed_errors(); calc_airspeed_errors();

14
Tools/CPUInfo/CPUInfo.pde
View File

@ -100,13 +100,13 @@ static void show_timings(void)
TIMEIT("dmul", v_out_d *= v_d, 100); TIMEIT("dmul", v_out_d *= v_d, 100);
TIMEIT("ddiv", v_out_d /= v_d, 100); TIMEIT("ddiv", v_out_d /= v_d, 100);
TIMEIT("sin()", v_out = sin(v_f), 20); TIMEIT("sin()", v_out = sinf(v_f), 20);
TIMEIT("cos()", v_out = cos(v_f), 20); TIMEIT("cos()", v_out = cosf(v_f), 20);
TIMEIT("tan()", v_out = tan(v_f), 20); TIMEIT("tan()", v_out = tanf(v_f), 20);
TIMEIT("acos()", v_out = acos(v_f * 0.2), 20); TIMEIT("acos()", v_out = acosf(v_f * 0.2), 20);
TIMEIT("asin()", v_out = asin(v_f * 0.2), 20); TIMEIT("asin()", v_out = asinf(v_f * 0.2), 20);
TIMEIT("atan2()", v_out = atan2(v_f * 0.2, v_f * 0.3), 20); TIMEIT("atan2()", v_out = atan2f(v_f * 0.2, v_f * 0.3), 20);
TIMEIT("sqrt()",v_out = sqrt(v_f), 20); TIMEIT("sqrt()",v_out = sqrtf(v_f), 20);
TIMEIT("iadd8", v_out_8 += v_8, 100); TIMEIT("iadd8", v_out_8 += v_8, 100);
TIMEIT("isub8", v_out_8 -= v_8, 100); TIMEIT("isub8", v_out_8 -= v_8, 100);

10
libraries/APM_Control/AP_PitchController.cpp
View File

@ -37,7 +37,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
_last_t = tnow; _last_t = tnow;
if(_ahrs == NULL) return 0; if(_ahrs == NULL) return 0;
float delta_time = (float)dt / 1000.0; float delta_time = (float)dt / 1000.0f;
int8_t inv = 1; int8_t inv = 1;
if(abs(_ahrs->roll_sensor)>9000) inv = -1; if(abs(_ahrs->roll_sensor)>9000) inv = -1;
@ -47,12 +47,12 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
float rate = _ahrs->get_pitch_rate_earth(); float rate = _ahrs->get_pitch_rate_earth();
float RC = 1/(2*M_PI*_fCut); float RC = 1/(2*PI*_fCut);
rate = _last_rate + rate = _last_rate +
(delta_time / (RC + delta_time)) * (rate - _last_rate); (delta_time / (RC + delta_time)) * (rate - _last_rate);
_last_rate = rate; _last_rate = rate;
float roll_scaler = 1/constrain(cos(_ahrs->roll),.33,1); float roll_scaler = 1/constrain(cosf(_ahrs->roll),0.33f,1);
int32_t desired_rate = angle_err * _kp_angle; int32_t desired_rate = angle_err * _kp_angle;
@ -70,7 +70,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
int32_t rate_error = desired_rate - ToDeg(rate)*100; int32_t rate_error = desired_rate - ToDeg(rate)*100;
float roll_ff = _roll_ff * 1000 * (roll_scaler-1.0); float roll_ff = _roll_ff * 1000 * (roll_scaler-1.0f);
if(roll_ff > 4500) if(roll_ff > 4500)
roll_ff = 4500; roll_ff = 4500;
else if(roll_ff < 0) else if(roll_ff < 0)
@ -80,7 +80,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
//rate integrator //rate integrator
if (!stabilize) { if (!stabilize) {
if ((fabs(_ki_rate) > 0) && (dt > 0)) if ((fabsf(_ki_rate) > 0) && (dt > 0))
{ {
_integrator += (rate_error * _ki_rate) * scaler * delta_time; _integrator += (rate_error * _ki_rate) * scaler * delta_time;
if (_integrator < -4500-out) _integrator = -4500-out; if (_integrator < -4500-out) _integrator = -4500-out;

6
libraries/APM_Control/AP_RollController.cpp
View File

@ -34,13 +34,13 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
_last_t = tnow; _last_t = tnow;
if(_ahrs == NULL) return 0; // can't control without a reference if(_ahrs == NULL) return 0; // can't control without a reference
float delta_time = (float)dt / 1000.0; float delta_time = (float)dt / 1000.0f;
int32_t angle_err = angle - _ahrs->roll_sensor; int32_t angle_err = angle - _ahrs->roll_sensor;
float rate = _ahrs->get_roll_rate_earth(); float rate = _ahrs->get_roll_rate_earth();
float RC = 1/(2*M_PI*_fCut); float RC = 1/(2*PI*_fCut);
rate = _last_rate + rate = _last_rate +
(delta_time / (RC + delta_time)) * (rate - _last_rate); (delta_time / (RC + delta_time)) * (rate - _last_rate);
_last_rate = rate; _last_rate = rate;
@ -60,7 +60,7 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
//rate integrator //rate integrator
if (!stabilize) { if (!stabilize) {
if ((fabs(_ki_rate) > 0) && (dt > 0)) if ((fabsf(_ki_rate) > 0) && (dt > 0))
{ {
_integrator += (rate_error * _ki_rate) * scaler * delta_time; _integrator += (rate_error * _ki_rate) * scaler * delta_time;
if (_integrator < -4500-out) _integrator = -4500-out; if (_integrator < -4500-out) _integrator = -4500-out;

12
libraries/APM_Control/AP_YawController.cpp
View File

@ -22,8 +22,8 @@ const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = {
// Low pass filter cut frequency for derivative calculation. // Low pass filter cut frequency for derivative calculation.
// FCUT macro computes a frequency cut based on an acceptable delay. // FCUT macro computes a frequency cut based on an acceptable delay.
#define FCUT(d) (1 / ( 2 * 3.14 * (d) ) ) #define FCUT(d) (1 / ( 2 * 3.14f * (d) ) )
const float AP_YawController::_fCut = FCUT(.5); const float AP_YawController::_fCut = FCUT(0.5f);
int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement) int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
{ {
@ -38,7 +38,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
return 0; return 0;
} }
float delta_time = (float) dt / 1000.0; float delta_time = (float) dt / 1000.0f;
if(stick_movement) { if(stick_movement) {
if(!_stick_movement) { if(!_stick_movement) {
@ -54,20 +54,20 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
Vector3f accels = _ins->get_accel(); Vector3f accels = _ins->get_accel();
// I didn't pull 512 out of a hat - it is a (very) loose approximation of // I didn't pull 512 out of a hat - it is a (very) loose approximation of
// 100*ToDeg(asin(-accels.y/9.81)) // 100*ToDeg(asinf(-accels.y/9.81f))
// which, with a P of 1.0, would mean that your rudder angle would be // which, with a P of 1.0, would mean that your rudder angle would be
// equal to your roll angle when // equal to your roll angle when
// the plane is still. Thus we have an (approximate) unit to go by. // the plane is still. Thus we have an (approximate) unit to go by.
float error = 512 * -accels.y; float error = 512 * -accels.y;
// strongly filter the error // strongly filter the error
float RC = 1/(2*M_PI*_fCut); float RC = 1/(2*PI*_fCut);
error = _last_error + error = _last_error +
(delta_time / (RC + delta_time)) * (error - _last_error); (delta_time / (RC + delta_time)) * (error - _last_error);
_last_error = error; _last_error = error;
// integrator // integrator
if(_freeze_start_time < (tnow - 2000)) { if(_freeze_start_time < (tnow - 2000)) {
if ((fabs(_ki) > 0) && (dt > 0)) { if ((fabsf(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time; _integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) _integrator = -_imax; if (_integrator < -_imax) _integrator = -_imax;
else if (_integrator > _imax) _integrator = _imax; else if (_integrator > _imax) _integrator = _imax;

2
libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde
View File

@ -82,7 +82,7 @@ static void show_data()
for (i=0; i<6; i++) { for (i=0; i<6; i++) {
if (result[i] < min[i]) min[i] = result[i]; if (result[i] < min[i]) min[i] = result[i];
if (result[i] > max[i]) max[i] = result[i]; if (result[i] > max[i]) max[i] = result[i];
if (fabs(result[i]) > 0x8000) { if (fabsf(result[i]) > 0x8000) {
hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]); hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]);
} }
} }

20
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -19,7 +19,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Description: This controls how how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0. // @Description: This controls how how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0.
// @Range: 0.0 1.0 // @Range: 0.0 1.0
// @Increment: .01 // @Increment: .01
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0), AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f),
// @Param: GPS_USE // @Param: GPS_USE
// @DisplayName: AHRS use GPS for navigation // @DisplayName: AHRS use GPS for navigation
@ -32,14 +32,14 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly. // @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly.
// @Range: 0.1 0.4 // @Range: 0.1 0.4
// @Increment: .01 // @Increment: .01
AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.4), AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.4f),
// @Param: RP_P // @Param: RP_P
// @DisplayName: AHRS RP_P // @DisplayName: AHRS RP_P
// @Description: This controls how fast the accelerometers correct the attitude // @Description: This controls how fast the accelerometers correct the attitude
// @Range: 0.1 0.4 // @Range: 0.1 0.4
// @Increment: .01 // @Increment: .01
AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.4), AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.4f),
// @Param: WIND_MAX // @Param: WIND_MAX
// @DisplayName: Maximum wind // @DisplayName: Maximum wind
@ -47,7 +47,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Range: 0 127 // @Range: 0 127
// @Units: m/s // @Units: m/s
// @Increment: 1 // @Increment: 1
AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0), AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f),
// @Param: BARO_USE // @Param: BARO_USE
// @DisplayName: AHRS Use Barometer // @DisplayName: AHRS Use Barometer
@ -89,13 +89,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
float AP_AHRS::get_pitch_rate_earth(void) float AP_AHRS::get_pitch_rate_earth(void)
{ {
Vector3f omega = get_gyro(); Vector3f omega = get_gyro();
return cos(roll) * omega.y - sin(roll) * omega.z; return cosf(roll) * omega.y - sinf(roll) * omega.z;
} }
// get roll rate in earth frame, in radians/s // get roll rate in earth frame, in radians/s
float AP_AHRS::get_roll_rate_earth(void) { float AP_AHRS::get_roll_rate_earth(void) {
Vector3f omega = get_gyro(); Vector3f omega = get_gyro();
return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll)); return omega.x + tanf(pitch)*(omega.y*sinf(roll) + omega.z*cosf(roll));
} }
// return airspeed estimate if available // return airspeed estimate if available
@ -107,8 +107,8 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
// constrain the airspeed by the ground speed // constrain the airspeed by the ground speed
// and AHRS_WIND_MAX // and AHRS_WIND_MAX
*airspeed_ret = constrain(*airspeed_ret, *airspeed_ret = constrain(*airspeed_ret,
_gps->ground_speed*0.01 - _wind_max, _gps->ground_speed*0.01f - _wind_max,
_gps->ground_speed*0.01 + _wind_max); _gps->ground_speed*0.01f + _wind_max);
} }
return true; return true;
} }
@ -121,8 +121,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_
Vector3f trim = _trim.get(); Vector3f trim = _trim.get();
// add new trim // add new trim
trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0)); trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0f), ToRad(10.0f));
trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0)); trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0f), ToRad(10.0f));
// set new trim values // set new trim values
_trim.set(trim); _trim.set(trim);

70
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -48,7 +48,7 @@ AP_AHRS_DCM::update(void)
// if the update call took more than 0.2 seconds then discard it, // if the update call took more than 0.2 seconds then discard it,
// otherwise we may move too far. This happens when arming motors // otherwise we may move too far. This happens when arming motors
// in ArduCopter // in ArduCopter
if (delta_t > 0.2) { if (delta_t > 0.2f) {
_ra_sum.zero(); _ra_sum.zero();
_ra_deltat = 0; _ra_deltat = 0;
return; return;
@ -129,14 +129,14 @@ AP_AHRS_DCM::check_matrix(void)
// the pitch calculation via asin(). These NaN values can // the pitch calculation via asin(). These NaN values can
// feed back into the rest of the DCM matrix via the // feed back into the rest of the DCM matrix via the
// error_course value. // error_course value.
if (!(_dcm_matrix.c.x < 1.0 && if (!(_dcm_matrix.c.x < 1.0f &&
_dcm_matrix.c.x > -1.0)) { _dcm_matrix.c.x > -1.0f)) {
// We have an invalid matrix. Force a normalisation. // We have an invalid matrix. Force a normalisation.
renorm_range_count++; renorm_range_count++;
normalize(); normalize();
if (_dcm_matrix.is_nan() || if (_dcm_matrix.is_nan() ||
fabs(_dcm_matrix.c.x) > 10) { fabsf(_dcm_matrix.c.x) > 10) {
// normalisation didn't fix the problem! We're // normalisation didn't fix the problem! We're
// in real trouble. All we can do is reset // in real trouble. All we can do is reset
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
@ -172,16 +172,16 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
// we don't want to compound the error by making DCM less // we don't want to compound the error by making DCM less
// accurate. // accurate.
renorm_val = 1.0 / a.length(); renorm_val = 1.0f / a.length();
// keep the average for reporting // keep the average for reporting
_renorm_val_sum += renorm_val; _renorm_val_sum += renorm_val;
_renorm_val_count++; _renorm_val_count++;
if (!(renorm_val < 2.0 && renorm_val > 0.5)) { if (!(renorm_val < 2.0f && renorm_val > 0.5f)) {
// this is larger than it should get - log it as a warning // this is larger than it should get - log it as a warning
renorm_range_count++; renorm_range_count++;
if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) { if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) {
// we are getting values which are way out of // we are getting values which are way out of
// range, we will reset the matrix and hope we // range, we will reset the matrix and hope we
// can recover our attitude using drift // can recover our attitude using drift
@ -245,8 +245,8 @@ AP_AHRS_DCM::yaw_error_compass(void)
} }
// get the earths magnetic field (only X and Y components needed) // get the earths magnetic field (only X and Y components needed)
Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), Vector3f mag_earth = Vector3f(cosf(_compass->get_declination()),
sin(_compass->get_declination()), 0); sinf(_compass->get_declination()), 0);
// calculate the error term in earth frame // calculate the error term in earth frame
Vector3f error = rb % mag_earth; Vector3f error = rb % mag_earth;
@ -259,7 +259,7 @@ AP_AHRS_DCM::yaw_error_compass(void)
float float
AP_AHRS_DCM::yaw_error_gps(void) AP_AHRS_DCM::yaw_error_gps(void)
{ {
return sin(ToRad(_gps->ground_course * 0.01) - yaw); return sinf(ToRad(_gps->ground_course * 0.01f) - yaw);
} }
@ -270,10 +270,10 @@ float
AP_AHRS_DCM::_P_gain(float spin_rate) AP_AHRS_DCM::_P_gain(float spin_rate)
{ {
if (spin_rate < ToDeg(50)) { if (spin_rate < ToDeg(50)) {
return 1.0; return 1.0f;
} }
if (spin_rate > ToDeg(500)) { if (spin_rate > ToDeg(500)) {
return 10.0; return 10.0f;
} }
return spin_rate/ToDeg(50); return spin_rate/ToDeg(50);
} }
@ -299,7 +299,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
if (_compass && _compass->use_for_yaw()) { if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) { if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6; yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f;
_compass_last_update = _compass->last_update; _compass_last_update = _compass->last_update;
// we force an additional compass read() // we force an additional compass read()
// here. This has the effect of throwing away // here. This has the effect of throwing away
@ -316,10 +316,10 @@ AP_AHRS_DCM::drift_correction_yaw(void)
} else if (_fly_forward && have_gps()) { } else if (_fly_forward && have_gps()) {
if (_gps->last_fix_time != _gps_last_update && if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) { _gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3; yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps->last_fix_time; _gps_last_update = _gps->last_fix_time;
if (!_have_initial_yaw) { if (!_have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01)); _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01f));
_omega_yaw_P.zero(); _omega_yaw_P.zero();
_have_initial_yaw = true; _have_initial_yaw = true;
} }
@ -332,7 +332,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// we don't have any new yaw information // we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss // slowly decay _omega_yaw_P to cope with loss
// of our yaw source // of our yaw source
_omega_yaw_P *= 0.97; _omega_yaw_P *= 0.97f;
return; return;
} }
@ -358,12 +358,12 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// don't update the drift term if we lost the yaw reference // don't update the drift term if we lost the yaw reference
// for more than 2 seconds // for more than 2 seconds
if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) { if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term // also add to the I term
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat; _omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
} }
_error_yaw_sum += fabs(yaw_error); _error_yaw_sum += fabsf(yaw_error);
_error_yaw_count++; _error_yaw_count++;
} }
@ -411,7 +411,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// As a fallback we use the fixed wing acceleration correction // As a fallback we use the fixed wing acceleration correction
// if we have an airspeed estimate (which we only have if // if we have an airspeed estimate (which we only have if
// _fly_forward is set), otherwise no correction // _fly_forward is set), otherwise no correction
if (_ra_deltat < 0.2) { if (_ra_deltat < 0.2f) {
// not enough time has accumulated // not enough time has accumulated
return; return;
} }
@ -489,8 +489,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
Vector3f vdelta = (velocity - _last_velocity) * v_scale; Vector3f vdelta = (velocity - _last_velocity) * v_scale;
// limit vertical acceleration correction to 0.5 gravities. The // limit vertical acceleration correction to 0.5 gravities. The
// barometer sometimes gives crazy acceleration changes. // barometer sometimes gives crazy acceleration changes.
vdelta.z = constrain(vdelta.z, -0.5, 0.5); vdelta.z = constrain(vdelta.z, -0.5f, 0.5f);
GA_e = Vector3f(0, 0, -1.0) + vdelta; GA_e = Vector3f(0, 0, -1.0f) + vdelta;
GA_e.normalize(); GA_e.normalize();
if (GA_e.is_inf()) { if (GA_e.is_inf()) {
// wait for some non-zero acceleration information // wait for some non-zero acceleration information
@ -500,7 +500,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// calculate the error term in earth frame. // calculate the error term in earth frame.
Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS); Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS);
float length = GA_b.length(); float length = GA_b.length();
if (length > 1.0) { if (length > 1.0f) {
GA_b /= length; GA_b /= length;
if (GA_b.is_inf()) { if (GA_b.is_inf()) {
// wait for some non-zero acceleration information // wait for some non-zero acceleration information
@ -518,10 +518,10 @@ AP_AHRS_DCM::drift_correction(float deltat)
float tilt = pythagorous2(GA_e.x, GA_e.y); float tilt = pythagorous2(GA_e.x, GA_e.y);
// equation 11 // equation 11
float theta = atan2(GA_b.y, GA_b.x); float theta = atan2f(GA_b.y, GA_b.x);
// equation 12 // equation 12
Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z); Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
// step 6 // step 6
error = GA_b % GA_e2; error = GA_b % GA_e2;
@ -533,8 +533,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
// flat, but still allow for yaw correction using the // flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS // accelerometers at high roll angles as long as we have a GPS
if (_compass && _compass->use_for_yaw()) { if (_compass && _compass->use_for_yaw()) {
if (have_gps() && gps_gain == 1.0) { if (have_gps() && gps_gain == 1.0f) {
error.z *= sin(fabs(roll)); error.z *= sinf(fabsf(roll));
} else { } else {
error.z = 0; error.z = 0;
} }
@ -617,7 +617,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
} }
float diff_length = fuselageDirectionDiff.length(); float diff_length = fuselageDirectionDiff.length();
if (diff_length > 0.2) { if (diff_length > 0.2f) {
// when turning, use the attitude response to estimate // when turning, use the attitude response to estimate
// wind speed // wind speed
float V; float V;
@ -632,18 +632,18 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse; Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
Vector3f velocitySum = velocity + _last_vel; Vector3f velocitySum = velocity + _last_vel;
float theta = atan2(velocityDiff.y, velocityDiff.x) - atan2(fuselageDirectionDiff.y, fuselageDirectionDiff.x); float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
float sintheta = sin(theta); float sintheta = sinf(theta);
float costheta = cos(theta); float costheta = cosf(theta);
Vector3f wind = Vector3f(); Vector3f wind = Vector3f();
wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y); wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y);
wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y); wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y);
wind.z = velocitySum.z - V * fuselageDirectionSum.z; wind.z = velocitySum.z - V * fuselageDirectionSum.z;
wind *= 0.5; wind *= 0.5f;
if (wind.length() < _wind.length() + 20) { if (wind.length() < _wind.length() + 20) {
_wind = _wind * 0.95 + wind * 0.05; _wind = _wind * 0.95f + wind * 0.05f;
} }
_last_wind_time = now; _last_wind_time = now;
@ -651,7 +651,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
// when flying straight use airspeed to get wind estimate if available // when flying straight use airspeed to get wind estimate if available
Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed(); Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
Vector3f wind = velocity - airspeed; Vector3f wind = velocity - airspeed;
_wind = _wind * 0.92 + wind * 0.08; _wind = _wind * 0.92f + wind * 0.08f;
} }
} }
@ -734,8 +734,8 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
// constrain the airspeed by the ground speed // constrain the airspeed by the ground speed
// and AHRS_WIND_MAX // and AHRS_WIND_MAX
*airspeed_ret = constrain(*airspeed_ret, *airspeed_ret = constrain(*airspeed_ret,
_gps->ground_speed*0.01 - _wind_max, _gps->ground_speed*0.01f - _wind_max,
_gps->ground_speed*0.01 + _wind_max); _gps->ground_speed*0.01f + _wind_max);
} }
return ret; return ret;
} }

22
libraries/AP_AHRS/AP_AHRS_MPU6000.cpp
View File

@ -79,11 +79,11 @@ AP_AHRS_MPU6000::update(void)
// TO-DO: should remove and replace with more standard functions // TO-DO: should remove and replace with more standard functions
float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians) float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
{ {
if( angle_in_radians > M_PI ) { if( angle_in_radians > PI ) {
return(angle_in_radians - 2*M_PI); return(angle_in_radians - 2*PI);
} }
else if( angle_in_radians < -M_PI ) { else if( angle_in_radians < -PI ) {
return(angle_in_radians + 2*M_PI); return(angle_in_radians + 2*PI);
} }
else{ else{
return(angle_in_radians); return(angle_in_radians);
@ -141,13 +141,13 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
// TO-DO: fix this. Currently it makes the roll and pitch drift more! // TO-DO: fix this. Currently it makes the roll and pitch drift more!
// If bias values are greater than 1 LSB we update the hardware offset // If bias values are greater than 1 LSB we update the hardware offset
// registers // registers
if( fabs(_gyro_bias[0])>1.0 ) { if( fabsf(_gyro_bias[0])>1.0f ) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0); //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0);
//_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0); //_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0);
//_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that //_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that
// we have already corrected on registers... // we have already corrected on registers...
} }
if (fabs(_gyro_bias[1])>1.0) { if (fabsf(_gyro_bias[1])>1.0f) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0); //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0);
//_gyro_bias[1] -= (int)_gyro_bias[1]; //_gyro_bias[1] -= (int)_gyro_bias[1];
} }
@ -199,9 +199,9 @@ void
AP_AHRS_MPU6000::push_gains_to_dmp() AP_AHRS_MPU6000::push_gains_to_dmp()
{ {
uint8_t gain; uint8_t gain;
if( _kp.get() >= 1.0 ) { if( _kp.get() >= 1.0f ) {
gain = 0xFF; gain = 0xFF;
}else if( _kp.get() <= 0.0 ) { }else if( _kp.get() <= 0.0f ) {
gain = 0x00; gain = 0x00;
}else{ }else{
gain = (uint8_t)((float)0xFF * _kp.get()); gain = (uint8_t)((float)0xFF * _kp.get());
@ -226,8 +226,8 @@ AP_AHRS_MPU6000::yaw_error_compass(void)
} }
// get the earths magnetic field (only X and Y components needed) // get the earths magnetic field (only X and Y components needed)
Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), Vector3f mag_earth = Vector3f(cosf(_compass->get_declination()),
sin(_compass->get_declination()), 0); sinf(_compass->get_declination()), 0);
// calculate the error term in earth frame // calculate the error term in earth frame
Vector3f error = rb % mag_earth; Vector3f error = rb % mag_earth;
@ -290,7 +290,7 @@ AP_AHRS_MPU6000::drift_correction_yaw(void)
yaw_error = wrap_PI(heading - yaw_corrected); yaw_error = wrap_PI(heading - yaw_corrected);
// shift the corrected yaw towards the compass heading a bit // shift the corrected yaw towards the compass heading a bit
yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get() * 0.1); yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get() * 0.1f);
// rebuild the dcm matrix yet again // rebuild the dcm matrix yet again
_dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected); _dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected);

6
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -40,7 +40,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = {
// @DisplayName: Airspeed ratio // @DisplayName: Airspeed ratio
// @Description: Airspeed calibration ratio // @Description: Airspeed calibration ratio
// @Increment: 0.1 // @Increment: 0.1
AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio, 1.9936), AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio, 1.9936f),
AP_GROUPEND AP_GROUPEND
}; };
@ -73,6 +73,6 @@ void AP_Airspeed::read(void)
} }
_airspeed_raw = _source->read_average(); _airspeed_raw = _source->read_average();
airspeed_pressure = max(_airspeed_raw - _offset, 0); airspeed_pressure = max(_airspeed_raw - _offset, 0);
_airspeed = 0.7 * _airspeed + _airspeed = 0.7f * _airspeed +
0.3 * sqrt(airspeed_pressure * _ratio); 0.3f * sqrtf(airspeed_pressure * _ratio);
} }

10
libraries/AP_Baro/AP_Baro.cpp
View File

@ -83,9 +83,9 @@ void AP_Baro::calibrate()
"for more than 500ms in AP_Baro::calibrate [3]\r\n")); "for more than 500ms in AP_Baro::calibrate [3]\r\n"));
} }
} while (!healthy); } while (!healthy);
ground_pressure = (ground_pressure * 0.8) + (get_pressure() * 0.2); ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f);
ground_temperature = (ground_temperature * 0.8) + ground_temperature = (ground_temperature * 0.8f) +
(get_temperature() * 0.2); (get_temperature() * 0.2f);
hal.scheduler->delay(100); hal.scheduler->delay(100);
} }
@ -111,7 +111,7 @@ float AP_Baro::get_altitude(void)
// unsmoothed values // unsmoothed values
scaling = (float)_ground_pressure / (float)get_pressure(); scaling = (float)_ground_pressure / (float)get_pressure();
temp = ((float)_ground_temperature) + 273.15f; temp = ((float)_ground_temperature) + 273.15f;
_altitude = log(scaling) * temp * 29.271267f; _altitude = logf(scaling) * temp * 29.271267f;
_last_altitude_t = _last_update; _last_altitude_t = _last_update;
@ -128,6 +128,6 @@ float AP_Baro::get_climb_rate(void)
{ {
// we use a 7 point derivative filter on the climb rate. This seems // we use a 7 point derivative filter on the climb rate. This seems
// to produce somewhat reasonable results on real hardware // to produce somewhat reasonable results on real hardware
return _climb_rate_filter.slope() * 1.0e3; return _climb_rate_filter.slope() * 1.0e3f;
} }

8
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -379,15 +379,15 @@ void AP_Baro_MS5611::_calculate()
// multiple samples, giving us more precision // multiple samples, giving us more precision
dT = D2-(((uint32_t)C5)<<8); dT = D2-(((uint32_t)C5)<<8);
TEMP = (dT * C6)/8388608; TEMP = (dT * C6)/8388608;
OFF = C2 * 65536.0 + (C4 * dT) / 128; OFF = C2 * 65536.0f + (C4 * dT) / 128;
SENS = C1 * 32768.0 + (C3 * dT) / 256; SENS = C1 * 32768.0f + (C3 * dT) / 256;
if (TEMP < 0) { if (TEMP < 0) {
// second order temperature compensation when under 20 degrees C // second order temperature compensation when under 20 degrees C
float T2 = (dT*dT) / 0x80000000; float T2 = (dT*dT) / 0x80000000;
float Aux = TEMP*TEMP; float Aux = TEMP*TEMP;
float OFF2 = 2.5*Aux; float OFF2 = 2.5f*Aux;
float SENS2 = 1.25*Aux; float SENS2 = 1.25f*Aux;
TEMP = TEMP - T2; TEMP = TEMP - T2;
OFF = OFF - OFF2; OFF = OFF - OFF2;
SENS = SENS - SENS2; SENS = SENS - SENS2;

12
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -221,13 +221,13 @@ AP_Compass_HMC5843::init()
float cal[3]; float cal[3];
cal[0] = fabs(expected_x / (float)_mag_x); cal[0] = fabsf(expected_x / (float)_mag_x);
cal[1] = fabs(expected_yz / (float)_mag_y); cal[1] = fabsf(expected_yz / (float)_mag_y);
cal[2] = fabs(expected_yz / (float)_mag_z); cal[2] = fabsf(expected_yz / (float)_mag_z);
if (cal[0] > 0.7 && cal[0] < 1.3 && if (cal[0] > 0.7f && cal[0] < 1.3f &&
cal[1] > 0.7 && cal[1] < 1.3 && cal[1] > 0.7f && cal[1] < 1.3f &&
cal[2] > 0.7 && cal[2] < 1.3) { cal[2] > 0.7f && cal[2] < 1.3f) {
good_count++; good_count++;
calibration[0] += cal[0]; calibration[0] += cal[0];
calibration[1] += cal[1]; calibration[1] += cal[1];

44
libraries/AP_Compass/Compass.cpp
View File

@ -144,26 +144,26 @@ Compass::calculate_heading(float roll, float pitch)
float sin_pitch; float sin_pitch;
float heading; float heading;
cos_roll = cos(roll); cos_roll = cosf(roll);
sin_roll = sin(roll); sin_roll = sinf(roll);
cos_pitch = cos(pitch); cos_pitch = cosf(pitch);
sin_pitch = sin(pitch); sin_pitch = sinf(pitch);
// Tilt compensated magnetic field X component: // Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch; headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch;
// Tilt compensated magnetic field Y component: // Tilt compensated magnetic field Y component:
headY = mag_y*cos_roll - mag_z*sin_roll; headY = mag_y*cos_roll - mag_z*sin_roll;
// magnetic heading // magnetic heading
heading = atan2(-headY,headX); heading = atan2f(-headY,headX);
// Declination correction (if supplied) // Declination correction (if supplied)
if( fabs(_declination) > 0.0 ) if( fabsf(_declination) > 0.0f )
{ {
heading = heading + _declination; heading = heading + _declination;
if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) if (heading > PI) // Angle normalization (-180 deg, 180 deg)
heading -= (2.0 * M_PI); heading -= (2.0f * PI);
else if (heading < -M_PI) else if (heading < -PI)
heading += (2.0 * M_PI); heading += (2.0f * PI);
} }
return heading; return heading;
@ -178,11 +178,11 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix)
float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x)); float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
float heading; float heading;
// sin(pitch) = - dcm_matrix(3,1) // sinf(pitch) = - dcm_matrix(3,1)
// cos(pitch)*sin(roll) = - dcm_matrix(3,2) // cosf(pitch)*sinf(roll) = - dcm_matrix(3,2)
// cos(pitch)*cos(roll) = - dcm_matrix(3,3) // cosf(pitch)*cosf(roll) = - dcm_matrix(3,3)
if (cos_pitch == 0.0) { if (cos_pitch == 0.0f) {
// we are pointing straight up or down so don't update our // we are pointing straight up or down so don't update our
// heading using the compass. Wait for the next iteration when // heading using the compass. Wait for the next iteration when
// we hopefully will have valid values again. // we hopefully will have valid values again.
@ -195,16 +195,16 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix)
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch; headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
// magnetic heading // magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S. // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
heading = constrain(atan2(-headY,headX), -3.15, 3.15); heading = constrain(atan2f(-headY,headX), -3.15f, 3.15f);
// Declination correction (if supplied) // Declination correction (if supplied)
if( fabs(_declination) > 0.0 ) if( fabsf(_declination) > 0.0f )
{ {
heading = heading + _declination; heading = heading + _declination;
if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) if (heading > PI) // Angle normalization (-180 deg, 180 deg)
heading -= (2.0 * M_PI); heading -= (2.0f * PI);
else if (heading < -M_PI) else if (heading < -PI)
heading += (2.0 * M_PI); heading += (2.0f * PI);
} }
return heading; return heading;
@ -254,7 +254,7 @@ Compass::null_offsets(void)
for (uint8_t i=0; i<_mag_history_size; i++) { for (uint8_t i=0; i<_mag_history_size; i++) {
// fill the history buffer with the current mag vector, // fill the history buffer with the current mag vector,
// with the offset removed // with the offset removed
_mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); _mag_history[i] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z);
} }
_mag_history_index = 0; _mag_history_index = 0;
return; return;
@ -289,7 +289,7 @@ Compass::null_offsets(void)
} }
// put the vector in the history // put the vector in the history
_mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); _mag_history[_mag_history_index] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z);
_mag_history_index = (_mag_history_index + 1) % _mag_history_size; _mag_history_index = (_mag_history_index + 1) % _mag_history_size;
// equation 6 of Bills paper // equation 6 of Bills paper

4
libraries/AP_Declination/AP_Declination.cpp
View File

@ -118,8 +118,8 @@ AP_Declination::get_declination(float lat, float lon)
lat = constrain(lat, -90, 90); lat = constrain(lat, -90, 90);
lon = constrain(lon, -180, 180); lon = constrain(lon, -180, 180);
latmin = floor(lat/5)*5; latmin = floorf(lat/5)*5;
lonmin = floor(lon/5)*5; lonmin = floorf(lon/5)*5;
latmin_index= (90+latmin)/5; latmin_index= (90+latmin)/5;
lonmin_index= (180+lonmin)/5; lonmin_index= (180+lonmin)/5;

12
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -34,12 +34,12 @@ void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{ {
time = _time; time = _time;
latitude = _latitude*1.0e7; latitude = _latitude*1.0e7f;
longitude = _longitude*1.0e7; longitude = _longitude*1.0e7f;
altitude = _altitude*1.0e2; altitude = _altitude*1.0e2f;
ground_speed = _ground_speed*1.0e2; ground_speed = _ground_speed*1.0e2f;
ground_course = _ground_course*1.0e2; ground_course = _ground_course*1.0e2f;
speed_3d = _speed_3d*1.0e2; speed_3d = _speed_3d*1.0e2f;
num_sats = _num_sats; num_sats = _num_sats;
fix = true; fix = true;
new_data = true; new_data = true;

14
libraries/AP_GPS/GPS.cpp
View File

@ -54,16 +54,16 @@ GPS::update(void)
if (_have_raw_velocity) { if (_have_raw_velocity) {
// the GPS is able to give us velocity numbers directly // the GPS is able to give us velocity numbers directly
_velocity_north = _vel_north * 0.01; _velocity_north = _vel_north * 0.01f;
_velocity_east = _vel_east * 0.01; _velocity_east = _vel_east * 0.01f;
_velocity_down = _vel_down * 0.01; _velocity_down = _vel_down * 0.01f;
} else { } else {
float gps_heading = ToRad(ground_course * 0.01); float gps_heading = ToRad(ground_course * 0.01f);
float gps_speed = ground_speed * 0.01; float gps_speed = ground_speed * 0.01f;
float sin_heading, cos_heading; float sin_heading, cos_heading;
cos_heading = cos(gps_heading); cos_heading = cosf(gps_heading);
sin_heading = sin(gps_heading); sin_heading = sinf(gps_heading);
_velocity_north = gps_speed * cos_heading; _velocity_north = gps_speed * cos_heading;
_velocity_east = gps_speed * sin_heading; _velocity_east = gps_speed * sin_heading;

6
libraries/AP_HAL/utility/Print.cpp
View File

@ -197,7 +197,7 @@ size_t Print::printFloat(float number, uint8_t digits)
size_t n = 0; size_t n = 0;
// Handle negative numbers // Handle negative numbers
if (number < 0.0) if (number < 0.0f)
{ {
n += print('-'); n += print('-');
number = -number; number = -number;
@ -206,7 +206,7 @@ size_t Print::printFloat(float number, uint8_t digits)
// Round correctly so that print(1.999, 2) prints as "2.00" // Round correctly so that print(1.999, 2) prints as "2.00"
float rounding = 0.5; float rounding = 0.5;
for (uint8_t i=0; i<digits; ++i) for (uint8_t i=0; i<digits; ++i)
rounding /= 10.0; rounding /= 10.0f;
number += rounding; number += rounding;
@ -223,7 +223,7 @@ size_t Print::printFloat(float number, uint8_t digits)
// Extract digits from the remainder one at a time // Extract digits from the remainder one at a time
while (digits-- > 0) while (digits-- > 0)
{ {
remainder *= 10.0; remainder *= 10.0f;
int toPrint = int(remainder); int toPrint = int(remainder);
n += print(toPrint); n += print(toPrint);
remainder -= toPrint; remainder -= toPrint;

2
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -226,7 +226,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
velned.ned_down = 0; velned.ned_down = 0;
velned.speed_2d = pythagorous2(d.speedN, d.speedE) * 100; velned.speed_2d = pythagorous2(d.speedN, d.speedE) * 100;
velned.speed_3d = velned.speed_2d; velned.speed_3d = velned.speed_2d;
velned.heading_2d = ToDeg(atan2(d.speedE, d.speedN)) * 100000.0; velned.heading_2d = ToDeg(atan2f(d.speedE, d.speedN)) * 100000.0;
if (velned.heading_2d < 0.0) { if (velned.heading_2d < 0.0) {
velned.heading_2d += 360.0 * 100000.0; velned.heading_2d += 360.0 * 100000.0;
} }

12
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -64,7 +64,7 @@ void AP_InertialNav::update(float dt)
Vector3f velocity_increase; Vector3f velocity_increase;
// discard samples where dt is too large // discard samples where dt is too large
if( dt > 0.1 ) { if( dt > 0.1f ) {
return; return;
} }
@ -149,7 +149,7 @@ void AP_InertialNav::check_gps()
// calculate time since last gps reading // calculate time since last gps reading
now = hal.scheduler->millis(); now = hal.scheduler->millis();
float dt = (float)(now - _gps_last_update) / 1000.0; float dt = (float)(now - _gps_last_update) / 1000.0f;
// call position correction method // call position correction method
correct_with_gps((*_gps_ptr)->longitude, (*_gps_ptr)->latitude, dt); correct_with_gps((*_gps_ptr)->longitude, (*_gps_ptr)->latitude, dt);
@ -167,7 +167,7 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
float hist_position_base_x, hist_position_base_y; float hist_position_base_x, hist_position_base_y;
// discard samples where dt is too large // discard samples where dt is too large
if( dt > 1.0 || dt == 0 || !_xy_enabled) { if( dt > 1.0f || dt == 0 || !_xy_enabled) {
return; return;
} }
@ -240,7 +240,7 @@ void AP_InertialNav::set_current_position(int32_t lon, int32_t lat)
// set longitude->meters scaling // set longitude->meters scaling
// this is used to offset the shrinking longitude as we go towards the poles // this is used to offset the shrinking longitude as we go towards the poles
_lon_to_m_scaling = cos((fabs((float)lat)/10000000.0) * 0.0174532925); _lon_to_m_scaling = cosf((fabsf((float)lat)/10000000.0f) * 0.0174532925f);
// reset corrections to base position to zero // reset corrections to base position to zero
_position_base.x = 0; _position_base.x = 0;
@ -332,7 +332,7 @@ void AP_InertialNav::check_baro()
// calculate time since last baro reading // calculate time since last baro reading
baro_update_time = _baro->get_last_update(); baro_update_time = _baro->get_last_update();
if( baro_update_time != _baro_last_update ) { if( baro_update_time != _baro_last_update ) {
float dt = (float)(baro_update_time - _baro_last_update) / 1000.0; float dt = (float)(baro_update_time - _baro_last_update) / 1000.0f;
// call correction method // call correction method
correct_with_baro(_baro->get_altitude()*100, dt); correct_with_baro(_baro->get_altitude()*100, dt);
_baro_last_update = baro_update_time; _baro_last_update = baro_update_time;
@ -348,7 +348,7 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
float accel_ef_z_correction; float accel_ef_z_correction;
// discard samples where dt is too large // discard samples where dt is too large
if( dt > 0.5 ) { if( dt > 0.5f ) {
return; return;
} }

8
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -8,9 +8,9 @@
#include <AP_Baro.h> // ArduPilot Mega Barometer Library #include <AP_Baro.h> // ArduPilot Mega Barometer Library
#include <AP_Buffer.h> // FIFO buffer library #include <AP_Buffer.h> // FIFO buffer library
#define AP_INTERTIALNAV_GRAVITY 9.80665 #define AP_INTERTIALNAV_GRAVITY 9.80665f
#define AP_INTERTIALNAV_TC_XY 3.0 // default time constant for complementary filter's X & Y axis #define AP_INTERTIALNAV_TC_XY 3.0f // default time constant for complementary filter's X & Y axis
#define AP_INTERTIALNAV_TC_Z 7.0 // default time constant for complementary filter's Z axis #define AP_INTERTIALNAV_TC_Z 7.0f // default time constant for complementary filter's Z axis
#define AP_INTERTIALNAV_ACCEL_CORR_MAX 100.0 // max allowed accelerometer offset correction #define AP_INTERTIALNAV_ACCEL_CORR_MAX 100.0 // max allowed accelerometer offset correction
@ -19,7 +19,7 @@
#define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10 #define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10
#define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y #define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y
#define AP_INERTIALNAV_LATLON_TO_CM 1.1113175 #define AP_INERTIALNAV_LATLON_TO_CM 1.1113175f
/* /*
* AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold * AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold

22
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -289,7 +289,7 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
// TO-DO: replace with gravity #define form location.cpp // TO-DO: replace with gravity #define form location.cpp
accel_offset.z += GRAVITY; accel_offset.z += GRAVITY;
total_change = fabs(prev.x - accel_offset.x) + fabs(prev.y - accel_offset.y) + fabs(prev.z - accel_offset.z); total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z);
max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y; max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y;
max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z; max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z;
@ -416,7 +416,7 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6],
// reset // reset
beta[0] = beta[1] = beta[2] = 0; beta[0] = beta[1] = beta[2] = 0;
beta[3] = beta[4] = beta[5] = 1.0/GRAVITY; beta[3] = beta[4] = beta[5] = 1.0f/GRAVITY;
while( num_iterations < 20 && change > eps ) { while( num_iterations < 20 && change > eps ) {
num_iterations++; num_iterations++;
@ -454,11 +454,11 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6],
accel_offsets.z = beta[2] * accel_scale.z; accel_offsets.z = beta[2] * accel_scale.z;
// sanity check scale // sanity check scale
if( accel_scale.is_nan() || fabs(accel_scale.x-1.0) > 0.1 || fabs(accel_scale.y-1.0) > 0.1 || fabs(accel_scale.z-1.0) > 0.1 ) { if( accel_scale.is_nan() || fabsf(accel_scale.x-1.0f) > 0.1f || fabsf(accel_scale.y-1.0f) > 0.1f || fabsf(accel_scale.z-1.0f) > 0.1f ) {
success = false; success = false;
} }
// sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G) // sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G)
if( accel_offsets.is_nan() || fabs(accel_offsets.x) > 2.0 || fabs(accel_offsets.y) > 2.0 || fabs(accel_offsets.z) > 3.0 ) { if( accel_offsets.is_nan() || fabsf(accel_offsets.x) > 2.0f || fabsf(accel_offsets.y) > 2.0f || fabsf(accel_offsets.z) > 3.0f ) {
success = false; success = false;
} }
@ -478,8 +478,8 @@ void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6],
b = beta[3+j]; b = beta[3+j];
dx = (float)data[j] - beta[j]; dx = (float)data[j] - beta[j];
residual -= b*b*dx*dx; residual -= b*b*dx*dx;
jacobian[j] = 2.0*b*b*dx; jacobian[j] = 2.0f*b*b*dx;
jacobian[3+j] = -2.0*b*dx*dx; jacobian[3+j] = -2.0f*b*dx*dx;
} }
for( j=0; j<6; j++ ) { for( j=0; j<6; j++ ) {
@ -496,9 +496,9 @@ void AP_InertialSensor::_calibrate_reset_matrices(float dS[6], float JS[6][6])
{ {
int16_t j,k; int16_t j,k;
for( j=0; j<6; j++ ) { for( j=0; j<6; j++ ) {
dS[j] = 0.0; dS[j] = 0.0f;
for( k=0; k<6; k++ ) { for( k=0; k<6; k++ ) {
JS[j][k] = 0.0; JS[j][k] = 0.0f;
} }
} }
} }
@ -515,7 +515,7 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float
//eliminate all nonzero entries below JS[i][i] //eliminate all nonzero entries below JS[i][i]
for( j=i+1; j<6; j++ ) { for( j=i+1; j<6; j++ ) {
mu = JS[i][j]/JS[i][i]; mu = JS[i][j]/JS[i][i];
if( mu != 0.0 ) { if( mu != 0.0f ) {
dS[j] -= mu*dS[i]; dS[j] -= mu*dS[i];
for( k=j; k<6; k++ ) { for( k=j; k<6; k++ ) {
JS[k][j] -= mu*JS[k][i]; JS[k][j] -= mu*JS[k][i];
@ -527,12 +527,12 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float
//back-substitute //back-substitute
for( i=5; i>=0; i-- ) { for( i=5; i>=0; i-- ) {
dS[i] /= JS[i][i]; dS[i] /= JS[i][i];
JS[i][i] = 1.0; JS[i][i] = 1.0f;
for( j=0; j<i; j++ ) { for( j=0; j<i; j++ ) {
mu = JS[i][j]; mu = JS[i][j];
dS[j] -= mu*dS[i]; dS[j] -= mu*dS[i];
JS[i][j] = 0.0; JS[i][j] = 0.0f;
} }
} }

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -3,10 +3,10 @@
#ifndef __AP_INERTIAL_SENSOR_H__ #ifndef __AP_INERTIAL_SENSOR_H__
#define __AP_INERTIAL_SENSOR_H__ #define __AP_INERTIAL_SENSOR_H__
#define GRAVITY 9.80665 #define GRAVITY 9.80665f
// Gyro and Accelerometer calibration criteria // Gyro and Accelerometer calibration criteria
#define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0 #define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f
#define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0 #define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f
#include <stdint.h> #include <stdint.h>
#include <AP_HAL.h> #include <AP_HAL.h>

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -6,7 +6,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// MPU6000 accelerometer scaling // MPU6000 accelerometer scaling
#define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0) #define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0f)
// MPU 6000 registers // MPU 6000 registers
#define MPUREG_XG_OFFS_TC 0x00 #define MPUREG_XG_OFFS_TC 0x00
@ -318,7 +318,7 @@ bool AP_InertialSensor_MPU6000::update( void )
} }
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
count_scale = 1.0 / _num_samples; count_scale = 1.0f / _num_samples;
_gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]], _gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]],
_gyro_data_sign[1] * sum[_gyro_data_index[1]], _gyro_data_sign[1] * sum[_gyro_data_index[1]],

12
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -24,17 +24,17 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
// Tested value : 418 // Tested value : 418
// Oilpan accelerometer scaling & offsets // Oilpan accelerometer scaling & offsets
#define OILPAN_ACCEL_SCALE_1G (GRAVITY * 2.0 / (2465.0 - 1617.0)) #define OILPAN_ACCEL_SCALE_1G (GRAVITY * 2.0f / (2465.0f - 1617.0f))
#define OILPAN_RAW_ACCEL_OFFSET ((2465.0 + 1617.0) * 0.5) #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f)
#define OILPAN_RAW_GYRO_OFFSET 1658.0 #define OILPAN_RAW_GYRO_OFFSET 1658.0f
#define ToRad(x) radians(x) // *pi/180 #define ToRad(x) radians(x) // *pi/180
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s,
// 0.8mV/ADC step => 0.8/3.33 = 0.4 // 0.8mV/ADC step => 0.8/3.33 = 0.4
// Tested values : 0.4026, ?, 0.4192 // Tested values : 0.4026, ?, 0.4192
const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4); const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f);
const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41); const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f);
const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41); const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f);
/* ------ Public functions -------------------------------------------*/ /* ------ Public functions -------------------------------------------*/

14
libraries/AP_Math/AP_Math.cpp
View File

@ -8,13 +8,13 @@ float safe_asin(float v)
if (isnan(v)) { if (isnan(v)) {
return 0.0; return 0.0;
} }
if (v >= 1.0) { if (v >= 1.0f) {
return PI/2; return PI/2;
} }
if (v <= -1.0) { if (v <= -1.0f) {
return -PI/2; return -PI/2;
} }
return asin(v); return asinf(v);
} }
// a varient of sqrt() that checks the input ranges and ensures a // a varient of sqrt() that checks the input ranges and ensures a
@ -24,7 +24,7 @@ float safe_asin(float v)
// real input should have been zero // real input should have been zero
float safe_sqrt(float v) float safe_sqrt(float v)
{ {
float ret = sqrt(v); float ret = sqrtf(v);
if (isnan(ret)) { if (isnan(ret)) {
return 0; return 0;
} }
@ -51,7 +51,7 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou
tv2(1,2,3); tv2(1,2,3);
tv2.rotate(r); tv2.rotate(r);
diff = tv1 - tv2; diff = tv1 - tv2;
if (diff.length() < 1.0e-6) { if (diff.length() < 1.0e-6f) {
// we found a match // we found a match
if (found) { if (found) {
*found = true; *found = true;
@ -101,10 +101,10 @@ float sq(float v) {
// 2D vector length // 2D vector length
float pythagorous2(float a, float b) { float pythagorous2(float a, float b) {
return sqrt(sq(a)+sq(b)); return sqrtf(sq(a)+sq(b));
} }
// 3D vector length // 3D vector length
float pythagorous3(float a, float b, float c) { float pythagorous3(float a, float b, float c) {
return sqrt(sq(a)+sq(b)+sq(c)); return sqrtf(sq(a)+sq(b)+sq(c));
} }

8
libraries/AP_Math/AP_Math.h
View File

@ -17,13 +17,13 @@
#include "polygon.h" #include "polygon.h"
#ifndef PI #ifndef PI
#define PI 3.141592653589793 #define PI 3.141592653589793f
#endif #endif
#define DEG_TO_RAD 0.017453292519943295769236907684886 #define DEG_TO_RAD 0.017453292519943295769236907684886f
#define RAD_TO_DEG 57.295779513082320876798154814105 #define RAD_TO_DEG 57.295779513082320876798154814105f
// acceleration due to gravity in m/s/s // acceleration due to gravity in m/s/s
#define GRAVITY_MSS 9.80665 #define GRAVITY_MSS 9.80665f
#define ROTATION_COMBINATION_SUPPORT 0 #define ROTATION_COMBINATION_SUPPORT 0

2
libraries/AP_Math/examples/eulers/eulers.pde
View File

@ -22,7 +22,7 @@ static float rad_diff(float rad1, float rad2)
if (diff < -PI) { if (diff < -PI) {
diff += 2*PI; diff += 2*PI;
} }
return fabs(diff); return fabsf(diff);
} }
static void check_result(float roll, float pitch, float yaw, static void check_result(float roll, float pitch, float yaw,

2
libraries/AP_Math/examples/location/location.pde
View File

@ -85,7 +85,7 @@ static void test_one_offset(struct Location &loc,
brg_error += 360; brg_error += 360;
} }
if (fabs(dist - dist2) > 1.0 || if (fabsf(dist - dist2) > 1.0 ||
brg_error > 1.0) { brg_error > 1.0) {
hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n", hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n",
brg_error, dist-dist2); brg_error, dist-dist2);

26
libraries/AP_Math/location.cpp
View File

@ -36,7 +36,7 @@ static float longitude_scale(const struct Location *loc)
// the same scale factor. // the same scale factor.
return scale; return scale;
} }
scale = cos((fabs((float)loc->lat)/1.0e7) * 0.0174532925); scale = cosf((fabsf((float)loc->lat)/1.0e7f) * 0.0174532925f);
last_lat = loc->lat; last_lat = loc->lat;
return scale; return scale;
} }
@ -53,7 +53,7 @@ float get_distance(const struct Location *loc1, const struct Location *loc2)
return -1; return -1;
float dlat = (float)(loc2->lat - loc1->lat); float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2); float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2);
return pythagorous2(dlat, dlong) * 0.01113195; return pythagorous2(dlat, dlong) * 0.01113195f;
} }
// return distance in centimeters to between two locations, or -1 if // return distance in centimeters to between two locations, or -1 if
@ -68,7 +68,7 @@ int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2)
{ {
int32_t off_x = loc2->lng - loc1->lng; int32_t off_x = loc2->lng - loc1->lng;
int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2); int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2);
int32_t bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; int32_t bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f;
if (bearing < 0) bearing += 36000; if (bearing < 0) bearing += 36000;
return bearing; return bearing;
} }
@ -120,17 +120,17 @@ bool location_passed_point(struct Location &location,
*/ */
void location_update(struct Location *loc, float bearing, float distance) void location_update(struct Location *loc, float bearing, float distance)
{ {
float lat1 = radians(loc->lat*1.0e-7); float lat1 = radians(loc->lat*1.0e-7f);
float lon1 = radians(loc->lng*1.0e-7); float lon1 = radians(loc->lng*1.0e-7f);
float brng = radians(bearing); float brng = radians(bearing);
float dr = distance/RADIUS_OF_EARTH; float dr = distance/RADIUS_OF_EARTH;
float lat2 = asin(sin(lat1)*cos(dr) + float lat2 = asinf(sinf(lat1)*cosf(dr) +
cos(lat1)*sin(dr)*cos(brng)); cosf(lat1)*sinf(dr)*cosf(brng));
float lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1), float lon2 = lon1 + atan2f(sinf(brng)*sinf(dr)*cosf(lat1),
cos(dr)-sin(lat1)*sin(lat2)); cosf(dr)-sinf(lat1)*sinf(lat2));
loc->lat = degrees(lat2)*1.0e7; loc->lat = degrees(lat2)*1.0e7f;
loc->lng = degrees(lon2)*1.0e7; loc->lng = degrees(lon2)*1.0e7f;
} }
/* /*
@ -140,8 +140,8 @@ void location_update(struct Location *loc, float bearing, float distance)
void location_offset(struct Location *loc, float ofs_north, float ofs_east) void location_offset(struct Location *loc, float ofs_north, float ofs_east)
{ {
if (ofs_north != 0 || ofs_east != 0) { if (ofs_north != 0 || ofs_east != 0) {
float dlat = ofs_north * 89.831520982; float dlat = ofs_north * 89.831520982f;
float dlng = (ofs_east * 89.831520982) / longitude_scale(loc); float dlng = (ofs_east * 89.831520982f) / longitude_scale(loc);
loc->lat += dlat; loc->lat += dlat;
loc->lng += dlng; loc->lng += dlng;
} }

16
libraries/AP_Math/matrix3.cpp
View File

@ -140,12 +140,12 @@ void Matrix3<T>::rotation(enum Rotation r)
template <typename T> template <typename T>
void Matrix3<T>::from_euler(float roll, float pitch, float yaw) void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
{ {
float cp = cos(pitch); float cp = cosf(pitch);
float sp = sin(pitch); float sp = sinf(pitch);
float sr = sin(roll); float sr = sinf(roll);
float cr = cos(roll); float cr = cosf(roll);
float sy = sin(yaw); float sy = sinf(yaw);
float cy = cos(yaw); float cy = cosf(yaw);
a.x = cp * cy; a.x = cp * cy;
a.y = (sr * sp * cy) - (cr * sy); a.y = (sr * sp * cy) - (cr * sy);
@ -167,10 +167,10 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
*pitch = -safe_asin(c.x); *pitch = -safe_asin(c.x);
} }
if (roll != NULL) { if (roll != NULL) {
*roll = atan2(c.y, c.z); *roll = atan2f(c.y, c.z);
} }
if (yaw != NULL) { if (yaw != NULL) {
*yaw = atan2(b.x, a.x); *yaw = atan2f(b.x, a.x);
} }
} }

22
libraries/AP_Math/quaternion.cpp
View File

@ -58,12 +58,12 @@ void Quaternion::earth_to_body(Vector3f &v)
// create a quaternion from Euler angles // create a quaternion from Euler angles
void Quaternion::from_euler(float roll, float pitch, float yaw) void Quaternion::from_euler(float roll, float pitch, float yaw)
{ {
float cr2 = cos(roll*0.5); float cr2 = cosf(roll*0.5f);
float cp2 = cos(pitch*0.5); float cp2 = cosf(pitch*0.5f);
float cy2 = cos(yaw*0.5); float cy2 = cosf(yaw*0.5f);
float sr2 = sin(roll*0.5); float sr2 = sinf(roll*0.5f);
float sp2 = sin(pitch*0.5); float sp2 = sinf(pitch*0.5f);
float sy2 = sin(yaw*0.5); float sy2 = sinf(yaw*0.5f);
q1 = cr2*cp2*cy2 + sr2*sp2*sy2; q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
q2 = sr2*cp2*cy2 - cr2*sp2*sy2; q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
@ -75,15 +75,15 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
void Quaternion::to_euler(float *roll, float *pitch, float *yaw) void Quaternion::to_euler(float *roll, float *pitch, float *yaw)
{ {
if (roll) { if (roll) {
*roll = (atan2(2.0*(q1*q2 + q3*q4), *roll = (atan2f(2.0f*(q1*q2 + q3*q4),
1 - 2.0*(q2*q2 + q3*q3))); 1 - 2.0f*(q2*q2 + q3*q3)));
} }
if (pitch) { if (pitch) {
// we let safe_asin() handle the singularities near 90/-90 in pitch // we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = safe_asin(2.0*(q1*q3 - q4*q2)); *pitch = safe_asin(2.0f*(q1*q3 - q4*q2));
} }
if (yaw) { if (yaw) {
*yaw = atan2(2.0*(q1*q4 + q2*q3), *yaw = atan2f(2.0f*(q1*q4 + q2*q3),
1 - 2.0*(q3*q3 + q4*q4)); 1 - 2.0f*(q3*q3 + q4*q4));
} }
} }

6
libraries/AP_Math/vector2.h
View File

@ -167,19 +167,19 @@ struct Vector2
// computes the angle between 2 arbitrary vectors // computes the angle between 2 arbitrary vectors
T angle(const Vector2<T> &v1, const Vector2<T> &v2) T angle(const Vector2<T> &v1, const Vector2<T> &v2)
{ {
return (T)acos((v1*v2) / (v1.length()*v2.length())); return (T)acosf((v1*v2) / (v1.length()*v2.length()));
} }
// computes the angle between this vector and another vector // computes the angle between this vector and another vector
T angle(const Vector2<T> &v2) T angle(const Vector2<T> &v2)
{ {
return (T)acos(((*this)*v2) / (this->length()*v2.length())); return (T)acosf(((*this)*v2) / (this->length()*v2.length()));
} }
// computes the angle between 2 normalized arbitrary vectors // computes the angle between 2 normalized arbitrary vectors
T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2) T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
{ {
return (T)acos(v1*v2); return (T)acosf(v1*v2);
} }
}; };

2
libraries/AP_Math/vector3.cpp
View File

@ -19,7 +19,7 @@
#include "AP_Math.h" #include "AP_Math.h"
#define HALF_SQRT_2 0.70710678118654757 #define HALF_SQRT_2 0.70710678118654757f
// rotate a vector by a standard rotation, attempting // rotate a vector by a standard rotation, attempting
// to use the minimum number of floating point operations // to use the minimum number of floating point operations

6
libraries/AP_Math/vector3.h
View File

@ -193,19 +193,19 @@ public:
// computes the angle between 2 arbitrary vectors // computes the angle between 2 arbitrary vectors
T angle(const Vector3<T> &v1, const Vector3<T> &v2) T angle(const Vector3<T> &v1, const Vector3<T> &v2)
{ {
return (T)acos((v1*v2) / (v1.length()*v2.length())); return (T)acosf((v1*v2) / (v1.length()*v2.length()));
} }
// computes the angle between this vector and another vector // computes the angle between this vector and another vector
T angle(const Vector3<T> &v2) T angle(const Vector3<T> &v2)
{ {
return (T)acos(((*this)*v2) / (this->length()*v2.length())); return (T)acosf(((*this)*v2) / (this->length()*v2.length()));
} }
// computes the angle between 2 arbitrary normalized vectors // computes the angle between 2 arbitrary normalized vectors
T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2) T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
{ {
return (T)acos(v1*v2); return (T)acosf(v1*v2);
} }
// check if any elements are NAN // check if any elements are NAN

42
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -330,14 +330,14 @@ void AP_MotorsHeli::reset_swash()
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform servo control mixing if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform servo control mixing
// roll factors // roll factors
_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle)); _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle)); _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle)); _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle));
// pitch factors // pitch factors
_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle)); _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle)); _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle)); _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle));
// collective factors // collective factors
_collectiveFactor[CH_1] = 1; _collectiveFactor[CH_1] = 1;
@ -363,10 +363,10 @@ void AP_MotorsHeli::reset_swash()
} }
// set roll, pitch and throttle scaling // set roll, pitch and throttle scaling
_roll_scaler = 1.0; _roll_scaler = 1.0f;
_pitch_scaler = 1.0; _pitch_scaler = 1.0f;
_collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0; _collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0f;
_stab_throttle_scalar = 1.0; _stab_throttle_scalar = 1.0f;
// we must be in set-up mode so mark swash as uninitialised // we must be in set-up mode so mark swash as uninitialised
_swash_initialised = false; _swash_initialised = false;
@ -391,25 +391,25 @@ void AP_MotorsHeli::init_swash()
collective_mid = constrain(collective_mid, collective_min, collective_max); collective_mid = constrain(collective_mid, collective_min, collective_max);
// calculate throttle mid point // calculate throttle mid point
throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0; throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0f;
// determine roll, pitch and throttle scaling // determine roll, pitch and throttle scaling
_roll_scaler = (float)roll_max/4500.0; _roll_scaler = (float)roll_max/4500.0f;
_pitch_scaler = (float)pitch_max/4500.0; _pitch_scaler = (float)pitch_max/4500.0f;
_collective_scalar = ((float)(collective_max-collective_min))/1000.0; _collective_scalar = ((float)(collective_max-collective_min))/1000.0f;
_stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0; _stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0f;
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing
// roll factors // roll factors
_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle)); _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle)); _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle)); _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle));
// pitch factors // pitch factors
_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle)); _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle)); _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle)); _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle));
// collective factors // collective factors
_collectiveFactor[CH_1] = 1; _collectiveFactor[CH_1] = 1;

4
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -353,8 +353,8 @@ void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t di
// call raw motor set-up method // call raw motor set-up method
add_motor_raw( add_motor_raw(
motor_num, motor_num,
cos(radians(angle_degrees + 90)), // roll factor cosf(radians(angle_degrees + 90)), // roll factor
cos(radians(angle_degrees)), // pitch factor cosf(radians(angle_degrees)), // pitch factor
(float)direction, // yaw factor (float)direction, // yaw factor
testing_order); testing_order);

2
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -80,7 +80,7 @@ void AP_MotorsTri::output_armed()
_rc_throttle->calc_pwm(); _rc_throttle->calc_pwm();
_rc_yaw->calc_pwm(); _rc_yaw->calc_pwm();
int roll_out = (float)_rc_roll->pwm_out * .866; int roll_out = (float)_rc_roll->pwm_out * 0.866f;
int pitch_out = _rc_pitch->pwm_out / 2; int pitch_out = _rc_pitch->pwm_out / 2;
//left front //left front

4
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -97,8 +97,8 @@ bool AP_Motors::setup_throttle_curve()
int16_t min_pwm = _rc_throttle->radio_min; int16_t min_pwm = _rc_throttle->radio_min;
int16_t max_pwm = _rc_throttle->radio_max; int16_t max_pwm = _rc_throttle->radio_max;
int16_t mid_throttle_pwm = (max_pwm + min_pwm) / 2; int16_t mid_throttle_pwm = (max_pwm + min_pwm) / 2;
int16_t mid_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_mid/100.0); int16_t mid_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_mid/100.0f);
int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0); int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0f);
bool retval = true; bool retval = true;
// some basic checks that the curve is valid // some basic checks that the curve is valid

42
libraries/AP_Mount/AP_Mount.cpp
View File

@ -346,21 +346,21 @@ void AP_Mount::update_mount_position()
// allow pilot speed position input to come directly from an RC_Channel // allow pilot speed position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
//_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed; //_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed;
_roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; _roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001f * _joystick_speed;
if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01); if (_roll_control_angle < radians(_roll_angle_min*0.01f)) _roll_control_angle = radians(_roll_angle_min*0.01f);
if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01); if (_roll_control_angle > radians(_roll_angle_max*0.01f)) _roll_control_angle = radians(_roll_angle_max*0.01f);
} }
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) { if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
//_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed; //_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed;
_tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; _tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001f * _joystick_speed;
if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01); if (_tilt_control_angle < radians(_tilt_angle_min*0.01f)) _tilt_control_angle = radians(_tilt_angle_min*0.01f);
if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01); if (_tilt_control_angle > radians(_tilt_angle_max*0.01f)) _tilt_control_angle = radians(_tilt_angle_max*0.01f);
} }
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
//_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed; //_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed;
_pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; _pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001f * _joystick_speed;
if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01); if (_pan_control_angle < radians(_pan_angle_min*0.01f)) _pan_control_angle = radians(_pan_angle_min*0.01f);
if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01); if (_pan_control_angle > radians(_pan_angle_max*0.01f)) _pan_control_angle = radians(_pan_angle_max*0.01f);
} }
} else { } else {
#endif #endif
@ -408,9 +408,9 @@ void AP_Mount::update_mount_position()
#endif #endif
// write the results to the servos // write the results to the servos
move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1f, _roll_angle_max*0.1f);
move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1); move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1f, _tilt_angle_max*0.1f);
move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1); move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1f, _pan_angle_max*0.1f);
} }
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
@ -449,7 +449,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
{ {
#if MNT_RETRACT_OPTION == ENABLED #if MNT_RETRACT_OPTION == ENABLED
case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); set_retract_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
if (packet.save_position) if (packet.save_position)
{ {
_retract_angles.save(); _retract_angles.save();
@ -458,7 +458,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
#endif #endif
case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); set_neutral_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
if (packet.save_position) if (packet.save_position)
{ {
_neutral_angles.save(); _neutral_angles.save();
@ -466,7 +466,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
break; break;
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
set_control_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); set_control_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
break; break;
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
@ -570,19 +570,19 @@ AP_Mount::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
float float
AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{ {
return radians(angle_input(rc, angle_min, angle_max)*0.01); return radians(angle_input(rc, angle_min, angle_max)*0.01f);
} }
void void
AP_Mount::calc_GPS_target_angle(struct Location *target) 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_x = (target->lng-_current_loc->lng)*cosf(ToRad((_current_loc->lat+target->lat)*0.00000005f))*0.01113195f;
float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195; float GPS_vector_y = (target->lat-_current_loc->lat)*0.01113195f;
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 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*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. float target_distance = 100.0f*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; _roll_control_angle = 0;
_tilt_control_angle = atan2(GPS_vector_z, target_distance); _tilt_control_angle = atan2f(GPS_vector_z, target_distance);
_pan_control_angle = atan2(GPS_vector_x, GPS_vector_y); _pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
} }
/// Stabilizes mount relative to the Earth's frame /// Stabilizes mount relative to the Earth's frame

12
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -10,7 +10,7 @@
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
#define FORTYFIVE_DEGREES 0.78539816 #define FORTYFIVE_DEGREES 0.78539816f
// pointer to the last instantiated optical flow sensor. Will be turned into // pointer to the last instantiated optical flow sensor. Will be turned into
// a table if we ever add support for more than one sensor // a table if we ever add support for more than one sensor
@ -81,8 +81,8 @@ void AP_OpticalFlow::update_conversion_factors()
{ {
// multiply this number by altitude and pixel change to get horizontal // multiply this number by altitude and pixel change to get horizontal
// move (in same units as altitude) // move (in same units as altitude)
conv_factor = ((1.0 / (float)(num_pixels * scaler)) conv_factor = ((1.0f / (float)(num_pixels * scaler))
* 2.0 * tan(field_of_view / 2.0)); * 2.0f * tanf(field_of_view / 2.0f));
// 0.00615 // 0.00615
radians_to_pixels = (num_pixels * scaler) / field_of_view; radians_to_pixels = (num_pixels * scaler) / field_of_view;
// 162.99 // 162.99
@ -97,8 +97,8 @@ void AP_OpticalFlow::update_position(float roll, float pitch,
// only update position if surface quality is good and angle is not // only update position if surface quality is good and angle is not
// over 45 degrees // over 45 degrees
if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES if( surface_quality >= 10 && fabsf(roll) <= FORTYFIVE_DEGREES
&& fabs(pitch) <= FORTYFIVE_DEGREES ) { && fabsf(pitch) <= FORTYFIVE_DEGREES ) {
altitude = max(altitude, 0); altitude = max(altitude, 0);
// calculate expected x,y diff due to roll and pitch change // calculate expected x,y diff due to roll and pitch change
exp_change_x = diff_roll * radians_to_pixels; exp_change_x = diff_roll * radians_to_pixels;
@ -108,7 +108,7 @@ void AP_OpticalFlow::update_position(float roll, float pitch,
change_x = dx - exp_change_x; change_x = dx - exp_change_x;
change_y = dy - exp_change_y; change_y = dy - exp_change_y;
float avg_altitude = (altitude + _last_altitude)*0.5; float avg_altitude = (altitude + _last_altitude)*0.5f;
// convert raw change to horizontal movement in cm // convert raw change to horizontal movement in cm
// perhaps this altitude should actually be the distance to the // perhaps this altitude should actually be the distance to the

2
libraries/AP_Param/AP_Param.cpp
View File

@ -696,7 +696,7 @@ bool AP_Param::save(void)
return true; return true;
} }
if (phdr.type != AP_PARAM_INT32 && if (phdr.type != AP_PARAM_INT32 &&
(fabs(v1-v2) < 0.0001*fabs(v1))) { (fabsf(v1-v2) < 0.0001f*fabsf(v1))) {
// for other than 32 bit integers, we accept values within // for other than 32 bit integers, we accept values within
// 0.01 percent of the current value as being the same // 0.01 percent of the current value as being the same
return true; return true;

4
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -40,7 +40,7 @@ AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_HAL::AnalogSource *sourc
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage)
{ {
float type_scaler = 1.0; float type_scaler = 1.0f;
switch(sonar_type) { switch(sonar_type) {
case AP_RANGEFINDER_MAXSONARXL: case AP_RANGEFINDER_MAXSONARXL:
type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
@ -63,6 +63,6 @@ float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refe
max_distance = AP_RANGEFINDER_MAXSONARHRLV_MAX_DISTANCE; max_distance = AP_RANGEFINDER_MAXSONARHRLV_MAX_DISTANCE;
break; break;
} }
_scaler = type_scaler * adc_refence_voltage / 5.0; _scaler = type_scaler * adc_refence_voltage / 5.0f;
return _scaler; return _scaler;
} }

2
libraries/Filter/LowPassFilter.h
View File

@ -78,7 +78,7 @@ template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq) void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq)
{ {
// calculate alpha // calculate alpha
float rc = 1/(2*(float)M_PI*cutoff_freq); float rc = 1/(2*PI*cutoff_freq);
_alpha = time_step / (time_step + rc); _alpha = time_step / (time_step + rc);
} }

8
libraries/Filter/examples/Derivative/Derivative.pde
View File

@ -21,7 +21,7 @@ void setup(){}
static float noise(void) static float noise(void)
{ {
return ((random() % 100)-50) * 0.001; return ((random() % 100)-50) * 0.001f;
} }
//Main loop where the action takes place //Main loop where the action takes place
@ -29,12 +29,12 @@ void loop()
{ {
hal.scheduler->delay(50); hal.scheduler->delay(50);
float t = hal.scheduler->millis()*1.0e-3; float t = hal.scheduler->millis()*1.0e-3;
float s = sin(t); float s = sinf(t);
//s += noise(); //s += noise();
uint32_t t1 = hal.scheduler->micros(); uint32_t t1 = hal.scheduler->micros();
derivative.update(s, t1); derivative.update(s, t1);
float output = derivative.slope() * 1.0e6; float output = derivative.slope() * 1.0e6f;
hal.console->printf("%f %f %f %f\n", t, output, s, cos(t)); hal.console->printf("%f %f %f %f\n", t, output, s, cosf(t));
} }
AP_HAL_MAIN(); AP_HAL_MAIN();

6
libraries/Filter/examples/LowPassFilter/LowPassFilter.pde
View File

@ -24,8 +24,8 @@ void setup()
hal.console->printf("ArduPilot LowPassFilter test ver 1.0\n\n"); hal.console->printf("ArduPilot LowPassFilter test ver 1.0\n\n");
// set-up filter // set-up filter
low_pass_filter.set_time_constant(0.02, 0.015); low_pass_filter.set_time_constant(0.02f, 0.015f);
//low_pass_filter.set_cutoff_frequency(0.02, 1.0); //low_pass_filter.set_cutoff_frequency(0.02f, 1.0f);
// Wait for the serial connection // Wait for the serial connection
hal.scheduler->delay(500); hal.scheduler->delay(500);
@ -44,7 +44,7 @@ void loop()
for( i=0; i<300; i++ ) { for( i=0; i<300; i++ ) {
// new data value // new data value
new_value = sin((float)i*2*M_PI*5/50.0); // 5hz new_value = sinf((float)i*2*PI*5/50.0f); // 5hz
// output to user // output to user
hal.console->printf("applying: %6.4f", new_value); hal.console->printf("applying: %6.4f", new_value);

9
libraries/PID/PID.cpp
View File

@ -7,6 +7,7 @@
#include "PID.h" #include "PID.h"
#include <AP_HAL.h> #include <AP_HAL.h>
#include <AP_Math.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -36,13 +37,13 @@ int32_t PID::get_pid(int32_t error, float scaler)
} }
_last_t = tnow; _last_t = tnow;
delta_time = (float)dt / 1000.0; delta_time = (float)dt / 1000.0f;
// Compute proportional component // Compute proportional component
output += error * _kp; output += error * _kp;
// Compute derivative component if time has elapsed // Compute derivative component if time has elapsed
if ((fabs(_kd) > 0) && (dt > 0)) { if ((fabsf(_kd) > 0) && (dt > 0)) {
float derivative; float derivative;
if (isnan(_last_derivative)) { if (isnan(_last_derivative)) {
@ -57,7 +58,7 @@ int32_t PID::get_pid(int32_t error, float scaler)
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
float RC = 1/(2*M_PI*_fCut); float RC = 1/(2*PI*_fCut);
derivative = _last_derivative + derivative = _last_derivative +
((delta_time / (RC + delta_time)) * ((delta_time / (RC + delta_time)) *
(derivative - _last_derivative)); (derivative - _last_derivative));
@ -74,7 +75,7 @@ int32_t PID::get_pid(int32_t error, float scaler)
output *= scaler; output *= scaler;
// Compute integral component if time has elapsed // Compute integral component if time has elapsed
if ((fabs(_ki) > 0) && (dt > 0)) { if ((fabsf(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time; _integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) { if (_integrator < -_imax) {
_integrator = -_imax; _integrator = -_imax;

2
libraries/RC_Channel/RC_Channel.cpp
View File

@ -180,7 +180,7 @@ RC_Channel::calc_pwm(void)
radio_out = (_reverse >= 0) ? (radio_min + pwm_out) : (radio_max - pwm_out); radio_out = (_reverse >= 0) ? (radio_min + pwm_out) : (radio_max - pwm_out);
}else if(_type == RC_CHANNEL_TYPE_ANGLE_RAW) { }else if(_type == RC_CHANNEL_TYPE_ANGLE_RAW) {
pwm_out = (float)servo_out * .1; pwm_out = (float)servo_out * 0.1f;
radio_out = (pwm_out * _reverse) + radio_trim; radio_out = (pwm_out * _reverse) + radio_trim;
}else{ // RC_CHANNEL_TYPE_ANGLE }else{ // RC_CHANNEL_TYPE_ANGLE