From 3c97fad2ec0e298618fd8c3b2e685325159cac72 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 10 Jul 2013 14:03:03 +1000 Subject: [PATCH] Plane: changes for GPS field changes --- ArduPlane/ArduPlane.pde | 10 ++++++---- ArduPlane/Attitude.pde | 6 +++--- ArduPlane/GCS_Mavlink.pde | 10 +++++----- ArduPlane/commands.pde | 2 +- ArduPlane/commands_logic.pde | 4 ++-- ArduPlane/test.pde | 2 +- 6 files changed, 18 insertions(+), 16 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f7686393c1..da9728ef16 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -957,7 +957,7 @@ static void update_GPS(void) if(ground_start_count > 1) { ground_start_count--; - ground_start_avg += g_gps->ground_speed; + ground_start_avg += g_gps->ground_speed_cm; } else if (ground_start_count == 1) { // We countdown N number of good GPS fixes @@ -1021,7 +1021,7 @@ static void update_flight_mode(void) if (nav_pitch_cd < takeoff_pitch_cd) nav_pitch_cd = takeoff_pitch_cd; } else { - nav_pitch_cd = (g_gps->ground_speed / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd; + nav_pitch_cd = (g_gps->ground_speed_cm / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd; nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd); } @@ -1235,10 +1235,12 @@ static void update_alt() //altitude_sensor = BARO; if (barometer.healthy) { - current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) + // alt_MSL centimeters (centimeters) + current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude_cm; current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); } else if (g_gps->status() >= GPS::GPS_OK_FIX_3D) { - current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100) + // alt_MSL centimeters (centimeters) + current_loc.alt = g_gps->altitude_cm; } geofence_check(true); diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 7d17f9bf58..d92669bfff 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -259,7 +259,7 @@ static void stabilize() if (channel_throttle->control_in == 0 && relative_altitude_abs_cm() < 500 && fabs(barometer.get_climb_rate()) < 0.5f && - g_gps->ground_speed < 300) { + g_gps->ground_speed_cm < 300) { // we are low, with no climb rate, and zero throttle, and very // low ground speed. Zero the attitude controller // integrators. This prevents integrator buildup pre-takeoff. @@ -405,7 +405,7 @@ static bool auto_takeoff_check(void) // no auto takeoff without GPS lock return false; } - if (g_gps->ground_speed < g.takeoff_throttle_min_speed*100.0f) { + if (g_gps->ground_speed_cm < g.takeoff_throttle_min_speed*100.0f) { // we haven't reached the minimum ground speed return false; } @@ -519,7 +519,7 @@ static bool suppress_throttle(void) if (g_gps != NULL && g_gps->status() >= GPS::GPS_OK_FIX_2D && - g_gps->ground_speed >= 500) { + g_gps->ground_speed_cm >= 500) { // if we have an airspeed sensor, then check it too, and // require 5m/s. This prevents throttle up due to spiky GPS // groundspeed with bad GPS reception diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 7b89a5884f..70f6f92bd4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -264,7 +264,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) fix_time, current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - g_gps->altitude * 10, // millimeters above sea level + g_gps->altitude_cm * 10, // millimeters above sea level relative_altitude() * 1.0e3, // millimeters above ground g_gps->velocity_north() * 100, // X speed cm/s (+ve North) g_gps->velocity_east() * 100, // Y speed cm/s (+ve East) @@ -294,11 +294,11 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) g_gps->status(), g_gps->latitude, // in 1E7 degrees g_gps->longitude, // in 1E7 degrees - g_gps->altitude * 10, // in mm + g_gps->altitude_cm * 10, // in mm g_gps->hdop, 65535, - g_gps->ground_speed, // cm/s - g_gps->ground_course, // 1/100 degrees, + g_gps->ground_speed_cm, // cm/s + g_gps->ground_course_cd, // 1/100 degrees, g_gps->num_sats); } @@ -386,7 +386,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) mavlink_msg_vfr_hud_send( chan, aspeed, - (float)g_gps->ground_speed * 0.01, + (float)g_gps->ground_speed_cm * 0.01f, (ahrs.yaw_sensor / 100) % 360, throttle, current_loc.alt / 100.0, diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index d44a661fbd..957e483ecd 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -236,7 +236,7 @@ void init_home() home.id = MAV_CMD_NAV_WAYPOINT; home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 - home.alt = max(g_gps->altitude, 0); + home.alt = max(g_gps->altitude_cm, 0); home_is_set = true; gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 2a5534f4d9..adfa605cbd 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -345,7 +345,7 @@ static bool verify_land() // Set land_complete if we are within 2 seconds distance or within // 3 meters altitude of the landing point - if ((wp_distance <= (g.land_flare_sec*g_gps->ground_speed*0.01)) + if ((wp_distance <= (g.land_flare_sec*g_gps->ground_speed_cm*0.01f)) || (adjusted_altitude_cm() <= next_WP.alt + g.land_flare_alt*100)) { land_complete = true; @@ -364,7 +364,7 @@ static bool verify_land() gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course_cd); } - if (g_gps->ground_speed*0.01 < 3.0) { + if (g_gps->ground_speed_cm*0.01f < 3.0) { // reload any airspeed or groundspeed parameters that may have // been set for landing. We don't do this till ground // speed drops below 3.0 m/s as otherwise we will change diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 0d1b7b0a98..d7765d8e6d 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -453,7 +453,7 @@ test_gps(uint8_t argc, const Menu::arg *argv) cliSerial->printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), (long)g_gps->latitude, (long)g_gps->longitude, - (long)g_gps->altitude/100, + (long)g_gps->altitude_cm/100, (int)g_gps->num_sats); }else{ cliSerial->printf_P(PSTR("."));