diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 09e6e20471..0c4d646356 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -190,11 +190,8 @@ static int32_t pitch_limit_min_cd; // supply data from the simulation. // -// All GPS access should be through this pointer. -static GPS *g_gps; -#if GPS2_ENABLE -static GPS *g_gps2; -#endif +// GPS driver +static AP_GPS gps; // flight modes convenience array static AP_Int8 *flight_modes = &g.flight_mode1; @@ -227,38 +224,6 @@ static AP_Compass_HIL compass; #error Unrecognized CONFIG_COMPASS setting #endif -// GPS selection -#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO -AP_GPS_Auto g_gps_driver(&g_gps); -#if GPS2_ENABLE -AP_GPS_Auto g_gps2_driver(&g_gps2); -#endif - -#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA -AP_GPS_NMEA g_gps_driver; - -#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF -AP_GPS_SIRF g_gps_driver; - -#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX -AP_GPS_UBLOX g_gps_driver; - -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK -AP_GPS_MTK g_gps_driver; - -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19 -AP_GPS_MTK19 g_gps_driver; - -#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE -AP_GPS_None g_gps_driver; - -#elif GPS_PROTOCOL == GPS_PROTOCOL_HIL -AP_GPS_HIL g_gps_driver; - -#else - #error Unrecognised GPS_PROTOCOL setting. -#endif // GPS PROTOCOL - #if CONFIG_INS_TYPE == CONFIG_INS_OILPAN || CONFIG_HAL_BOARD == HAL_BOARD_APM1 AP_ADC_ADS7844 apm1_adc; #endif @@ -281,9 +246,9 @@ AP_InertialSensor_L3G4200D ins; // Inertial Navigation EKF #if AP_AHRS_NAVEKF_AVAILABLE -AP_AHRS_NavEKF ahrs(ins, barometer, g_gps); +AP_AHRS_NavEKF ahrs(ins, barometer, gps); #else -AP_AHRS_DCM ahrs(ins, barometer, g_gps); +AP_AHRS_DCM ahrs(ins, barometer, gps); #endif static AP_L1_Control L1_controller(ahrs); @@ -690,13 +655,13 @@ static uint16_t mainLoop_count; #if MOUNT == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. // mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? -static AP_Mount camera_mount(¤t_loc, g_gps, ahrs, 0); +static AP_Mount camera_mount(¤t_loc, ahrs, 0); #endif #if MOUNT2 == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. // mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? -static AP_Mount camera_mount2(¤t_loc, g_gps, ahrs, 1); +static AP_Mount camera_mount2(¤t_loc, ahrs, 1); #endif //////////////////////////////////////////////////////////////////////////////// @@ -1003,13 +968,13 @@ static void compass_save() static void airspeed_ratio_update(void) { if (!airspeed.enabled() || - g_gps->status() < GPS::GPS_OK_FIX_3D || - g_gps->ground_speed_cm < 400) { + gps.status() < AP_GPS::GPS_OK_FIX_3D || + gps.ground_speed() < 4) { // don't calibrate when not moving return; } if (airspeed.get_airspeed() < aparm.airspeed_min && - g_gps->ground_speed_cm < (uint32_t)aparm.airspeed_min*100) { + gps.ground_speed() < (uint32_t)aparm.airspeed_min) { // don't calibrate when flying below the minimum airspeed. We // check both airspeed and ground speed to catch cases where // the airspeed ratio is way too low, which could lead to it @@ -1022,7 +987,7 @@ static void airspeed_ratio_update(void) // don't calibrate when going beyond normal flight envelope return; } - Vector3f vg = g_gps->velocity_vector(); + const Vector3f &vg = gps.velocity(); airspeed.update_calibration(vg); gcs_send_airspeed_calibration(vg); } @@ -1034,27 +999,14 @@ static void airspeed_ratio_update(void) static void update_GPS_50Hz(void) { static uint32_t last_gps_reading; - g_gps->update(); + gps.update(); - if (g_gps->last_message_time_ms() != last_gps_reading) { - last_gps_reading = g_gps->last_message_time_ms(); + if (gps.last_message_time_ms() != last_gps_reading) { + last_gps_reading = gps.last_message_time_ms(); if (should_log(MASK_LOG_GPS)) { Log_Write_GPS(); } } - -#if GPS2_ENABLE - static uint32_t last_gps2_reading; - if (g_gps2 != NULL) { - g_gps2->update(); - if (g_gps2->last_message_time_ms() != last_gps2_reading) { - last_gps2_reading = g_gps2->last_message_time_ms(); - if (should_log(MASK_LOG_GPS)) { - DataFlash.Log_Write_GPS2(g_gps2); - } - } - } -#endif } /* @@ -1065,10 +1017,11 @@ static void update_GPS_10Hz(void) // get position from AHRS have_position = ahrs.get_position(current_loc); - if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) { - g_gps->new_data = false; + static uint32_t last_gps_msg_ms; + if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + last_gps_msg_ms = gps.last_message_time_ms(); - if(ground_start_count > 1) { + if (ground_start_count > 1) { ground_start_count--; } else if (ground_start_count == 1) { // We countdown N number of good GPS fixes @@ -1081,11 +1034,12 @@ static void update_GPS_10Hz(void) init_home(); // set system clock for log timestamps - hal.util->set_system_clock(g_gps->time_epoch_usec()); + hal.util->set_system_clock(gps.time_epoch_usec()); if (g.compass_enabled) { // Set compass declination automatically - compass.set_initial_location(g_gps->latitude, g_gps->longitude); + const Location &loc = gps.location(); + compass.set_initial_location(loc.lat, loc.lng); } ground_start_count = 0; } @@ -1143,7 +1097,7 @@ static void handle_auto_mode(void) if (nav_pitch_cd < takeoff_pitch_cd) nav_pitch_cd = takeoff_pitch_cd; } else { - nav_pitch_cd = (g_gps->ground_speed_cm / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd; + nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd; nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd); } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 85c56b75ed..88652bccf5 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -345,7 +345,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_cm < 300) { + gps.ground_speed() < 3) { // 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. @@ -410,7 +410,7 @@ static void calc_nav_yaw_course(void) */ static void calc_nav_yaw_ground(void) { - if (g_gps->ground_speed_cm < 100 && + if (gps.ground_speed() < 1 && channel_throttle->control_in == 0) { // manual rudder control while still steer_state.locked_course = false; @@ -504,7 +504,7 @@ static bool auto_takeoff_check(void) last_check_ms = now; // Check for bad GPS - if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) { + if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { // no auto takeoff without GPS lock return false; } @@ -539,9 +539,9 @@ static bool auto_takeoff_check(void) } // Check ground speed and time delay - if (((g_gps->ground_speed_cm > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0)) && + if (((gps.ground_speed() > g.takeoff_throttle_min_speed || g.takeoff_throttle_min_speed == 0.0)) && ((now - last_tkoff_arm_time) >= min(uint16_t(g.takeoff_throttle_delay)*100,2500))) { - gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed_cm*0.01f); + gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), gps.ground_speed()); launchTimerStarted = false; last_tkoff_arm_time = 0; return true; @@ -565,9 +565,8 @@ no_launch: static bool is_flying(void) { // If we don't have a GPS lock then don't use GPS for this test - bool gpsMovement = (g_gps == NULL || - g_gps->status() < GPS::GPS_OK_FIX_2D || - g_gps->ground_speed_cm >= 500); + bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D || + gps.ground_speed() >= 5); bool airspeedMovement = !airspeed.use() || airspeed.get_airspeed() >= 5; @@ -623,9 +622,8 @@ static bool suppress_throttle(void) return false; } - if (g_gps != NULL && - g_gps->status() >= GPS::GPS_OK_FIX_2D && - g_gps->ground_speed_cm >= 500) { + if (gps.status() >= AP_GPS::GPS_OK_FIX_2D && + gps.ground_speed() >= 5) { // 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 17323e4420..99ecc69cb6 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -146,7 +146,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (airspeed.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; } - if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) { + if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; } if (geofence_present()) { @@ -218,7 +218,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } - if (g_gps != NULL && g_gps->status() >= GPS::GPS_OK_FIX_3D) { + if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; } if (!ins.healthy()) { @@ -255,27 +255,28 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) static void NOINLINE send_location(mavlink_channel_t chan) { - uint32_t fix_time; + uint32_t fix_time_ms; // if we have a GPS fix, take the time as the last fix time. That // allows us to correctly calculate velocities and extrapolate // positions. // If we don't have a GPS fix then we are dead reckoning, and will // use the current boot time as the fix time. - if (g_gps->status() >= GPS::GPS_OK_FIX_2D) { - fix_time = g_gps->last_fix_time; + if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { + fix_time_ms = gps.last_fix_time_ms(); } else { - fix_time = millis(); + fix_time_ms = hal.scheduler->millis(); } + const Vector3f &vel = gps.velocity(); mavlink_msg_global_position_int_send( chan, - fix_time, + fix_time_ms, current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - g_gps->altitude_cm * 10, // millimeters above sea level + gps.location().alt * 10UL, // 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) - g_gps->velocity_down() * -100, // Z speed cm/s (+ve up) + vel.x * 100, // X speed cm/s (+ve North) + vel.y * 100, // Y speed cm/s (+ve East) + vel.z * -100, // Z speed cm/s (+ve up) ahrs.yaw_sensor); } @@ -295,39 +296,45 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) static void NOINLINE send_gps_raw(mavlink_channel_t chan) { - static uint32_t last_send_time; - if (last_send_time != 0 && last_send_time == g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_3D) { - return; + static uint32_t last_send_time_ms; + if (last_send_time_ms == 0 || last_send_time_ms != gps.last_message_time_ms(0)) { + last_send_time_ms = gps.last_message_time_ms(0); + const Location &loc = gps.location(0); + mavlink_msg_gps_raw_int_send( + chan, + gps.last_fix_time_ms(0)*(uint64_t)1000, + gps.status(0), + loc.lat, // in 1E7 degrees + loc.lng, // in 1E7 degrees + loc.alt * 10UL, // in mm + gps.get_hdop(0), + 65535, + gps.ground_speed(0)*100, // cm/s + gps.ground_course_cd(0), // 1/100 degrees, + gps.num_sats(0)); } - last_send_time = g_gps->last_fix_time; - mavlink_msg_gps_raw_int_send( - chan, - g_gps->last_fix_time*(uint64_t)1000, - g_gps->status(), - g_gps->latitude, // in 1E7 degrees - g_gps->longitude, // in 1E7 degrees - g_gps->altitude_cm * 10, // in mm - g_gps->hdop, - 65535, - g_gps->ground_speed_cm, // cm/s - g_gps->ground_course_cd, // 1/100 degrees, - g_gps->num_sats); -#if GPS2_ENABLE - if (g_gps2 != NULL && g_gps2->status() != GPS::NO_GPS) { + +#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 + static uint32_t last_send_time_ms2; + if (gps.num_sensors() > 1 && + gps.status(1) > AP_GPS::NO_GPS && + (last_send_time_ms2 == 0 || last_send_time_ms2 != gps.last_message_time_ms(1))) { int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) { + const Location &loc = gps.location(1); + last_send_time_ms = gps.last_message_time_ms(1); mavlink_msg_gps2_raw_send( chan, - g_gps2->last_fix_time*(uint64_t)1000, - g_gps2->status(), - g_gps2->latitude, // in 1E7 degrees - g_gps2->longitude, // in 1E7 degrees - g_gps2->altitude_cm * 10, // in mm - g_gps2->hdop, + gps.last_fix_time_ms(1)*(uint64_t)1000, + gps.status(1), + loc.lat, + loc.lng, + loc.alt * 10UL, + gps.get_hdop(1), 65535, - g_gps2->ground_speed_cm, // cm/s - g_gps2->ground_course_cd, // 1/100 degrees, - g_gps2->num_sats, + gps.ground_speed(1)*100, // cm/s + gps.ground_course_cd(1), // 1/100 degrees, + gps.num_sats(1), 0, 0); } @@ -339,7 +346,7 @@ static void NOINLINE send_system_time(mavlink_channel_t chan) { mavlink_msg_system_time_send( chan, - g_gps->time_epoch_usec(), + gps.time_epoch_usec(), hal.scheduler->millis()); } @@ -455,7 +462,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) mavlink_msg_vfr_hud_send( chan, aspeed, - (float)g_gps->ground_speed_cm * 0.01f, + gps.ground_speed(), (ahrs.yaw_sensor / 100) % 360, throttle, current_loc.alt / 100.0, @@ -1580,16 +1587,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) last_hil_state = packet; - float vel = pythagorous2(packet.vx, packet.vy); - float cog = wrap_360_cd(ToDeg(atan2f(packet.vy, packet.vx)) * 100); + // set gps hil sensor + Location loc; + loc.lat = packet.lat; + loc.lng = packet.lon; + loc.alt = packet.alt/10; + Vector3f vel(packet.vx, packet.vy, packet.vz); + vel *= 0.01f; - if (g_gps != NULL) { - // set gps hil sensor - g_gps->setHIL(GPS::FIX_3D, - packet.time_usec/1000, - packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, - vel*1.0e-2, cog*1.0e-2, 0, 10); - } + gps.setHIL(AP_GPS::GPS_OK_FIX_3D, + packet.time_usec/1000, + loc, vel, 10); // rad/sec Vector3f gyros; diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index b1939f84df..fd25a71ba1 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -253,8 +253,8 @@ static void Log_Write_Camera() #if CAMERA == ENABLED struct log_Camera pkt = { LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG), - gps_time : g_gps->time_week_ms, - gps_week : g_gps->time_week, + gps_time : gps.time_week_ms(), + gps_week : gps.time_week(), latitude : current_loc.lat, longitude : current_loc.lng, altitude : current_loc.alt, @@ -352,7 +352,7 @@ static void Log_Write_Nav_Tuning() altitude_error_cm : (int16_t)altitude_error_cm, airspeed_cm : (int16_t)airspeed.get_airspeed_cm(), altitude : barometer.get_altitude(), - groundspeed_cm : g_gps->ground_speed_cm + groundspeed_cm : (uint32_t)(gps.ground_speed()*100) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -395,7 +395,7 @@ static void Log_Write_Sonar() distance : sonar.distance_cm(), voltage : sonar.voltage(), baro_alt : barometer.get_altitude(), - groundspeed : (0.01f * g_gps->ground_speed_cm), + groundspeed : gps.ground_speed(), throttle : (uint8_t)(100 * channel_throttle->norm_output()) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -493,7 +493,7 @@ static void Log_Write_Compass() static void Log_Write_GPS(void) { - DataFlash.Log_Write_GPS(g_gps, current_loc.alt - ahrs.get_home().alt); + DataFlash.Log_Write_GPS(gps, current_loc.alt - ahrs.get_home().alt); } static void Log_Write_IMU() diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index eb82fe3069..b3ffa244df 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -104,6 +104,7 @@ public: k_param_rssi_range, k_param_flapin_channel, k_param_flaperon_output, + k_param_gps, // 100: Arming parameters k_param_arming = 100, diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index c79a717d5b..e08719fdd1 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -805,6 +805,11 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_Baro/AP_Baro.cpp GOBJECT(barometer, "GND_", AP_Baro), + // GPS driver + // @Group: GPS_ + // @Path: ../libraries/AP_GPS/AP_GPS.cpp + GOBJECT(gps, "GPS_", AP_GPS), + #if CAMERA == ENABLED // @Group: CAM_ // @Path: ../libraries/AP_Camera/AP_Camera.cpp diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index d7483683fd..7f8b1d046c 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -97,17 +97,7 @@ static void init_home() { gcs_send_text_P(SEVERITY_LOW, PSTR("init home")); - // block until we get a good fix - // ----------------------------- - while (!g_gps->new_data || !g_gps->fix) { - g_gps->update(); -#if HIL_MODE != HIL_MODE_DISABLED - // update hil gps so we have new_data - gcs_update(); -#endif - } - - ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm); + ahrs.set_home(gps.location()); home_is_set = true; gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); @@ -132,6 +122,6 @@ static void init_home() */ static void update_home() { - ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm); + ahrs.set_home(gps.location()); barometer.update_calibration(); } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 70fabe61f7..db0dbeb0df 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -359,7 +359,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_cm*0.01f)) + if ((wp_distance <= (g.land_flare_sec * gps.ground_speed())) || (adjusted_altitude_cm() <= next_WP_loc.alt + g.land_flare_alt*100)) { land_complete = true; @@ -378,7 +378,7 @@ static bool verify_land() gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), steer_state.hold_course_cd); } - if (g_gps->ground_speed_cm*0.01f < 3.0) { + if (gps.ground_speed() < 3) { // 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 @@ -574,10 +574,10 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd) static void do_set_home(const AP_Mission::Mission_Command& cmd) { - if (cmd.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) { + if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { init_home(); } else { - ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, cmd.content.location.alt*100.0f); + ahrs.set_home(cmd.content.location); home_is_set = true; } } diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 39214d733b..20169dd7a8 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -118,12 +118,6 @@ #endif -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 -#define GPS2_ENABLE 1 -#else -#define GPS2_ENABLE 0 -#endif - #ifndef CONFIG_BARO # error "CONFIG_BARO not set" #endif @@ -140,26 +134,12 @@ #endif #if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode - #undef GPS_PROTOCOL - #define GPS_PROTOCOL GPS_PROTOCOL_HIL #undef CONFIG_BARO #define CONFIG_BARO AP_BARO_HIL #undef CONFIG_INS_TYPE #define CONFIG_INS_TYPE CONFIG_INS_HIL #undef CONFIG_COMPASS #define CONFIG_COMPASS AP_COMPASS_HIL - #undef GPS2_ENABLE - #define GPS2_ENABLE 0 -#endif - -////////////////////////////////////////////////////////////////////////////// -// GPS_PROTOCOL -// -// Note that this test must follow the HIL_PROTOCOL block as the HIL -// setup may override the GPS configuration. -// -#ifndef GPS_PROTOCOL - # define GPS_PROTOCOL GPS_PROTOCOL_AUTO #endif #ifndef MAV_SYSTEM_ID diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 33a1c1906f..cca574e9df 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -47,17 +47,6 @@ enum gcs_failsafe { #define T6 1000000 #define T7 10000000 -// GPS type codes - use the names, not the numbers -#define GPS_PROTOCOL_NONE -1 -#define GPS_PROTOCOL_NMEA 0 -#define GPS_PROTOCOL_SIRF 1 -#define GPS_PROTOCOL_UBLOX 2 -#define GPS_PROTOCOL_IMU 3 -#define GPS_PROTOCOL_MTK 4 -#define GPS_PROTOCOL_HIL 5 -#define GPS_PROTOCOL_MTK19 6 -#define GPS_PROTOCOL_AUTO 7 - // HIL enumerations. Note that HIL_MODE_ATTITUDE and HIL_MODE_SENSORS // are now the same thing, and are sensors based. The old define is // kept to allow old APM_Config.h headers to keep working diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 029aa58058..c42dd7777e 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -118,7 +118,7 @@ static void calc_gndspeed_undershoot() { // Use the component of ground speed in the forward direction // This prevents flyaway if wind takes plane backwards - if (g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D) { + if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { Vector2f gndVel = ahrs.groundspeed_vector(); const Matrix3f &rotMat = ahrs.get_dcm_matrix(); Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); @@ -169,8 +169,8 @@ static void update_cruise() if (!cruise_state.locked_heading && channel_roll->control_in == 0 && channel_rudder->control_in == 0 && - g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D && - g_gps->ground_speed_cm >= 300 && + gps.status() >= AP_GPS::GPS_OK_FIX_2D && + gps.ground_speed() >= 3 && cruise_state.lock_timer_ms == 0) { // user wants to lock the heading - start the timer cruise_state.lock_timer_ms = hal.scheduler->millis(); @@ -181,7 +181,7 @@ static void update_cruise() // from user cruise_state.locked_heading = true; cruise_state.lock_timer_ms = 0; - cruise_state.locked_heading_cd = g_gps->ground_course_cd; + cruise_state.locked_heading_cd = gps.ground_course_cd(); prev_WP_loc = current_loc; } if (cruise_state.locked_heading) { diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index b4db6a3699..f0c635fa54 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -77,17 +77,6 @@ static void init_ardupilot() // hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE); - // GPS serial port. - // - // standard gps running - hal.uartB->begin(38400, 256, 16); - -#if GPS2_ENABLE - if (hal.uartE != NULL) { - hal.uartE->begin(38400, 256, 16); - } -#endif - cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n"), hal.util->available_memory()); @@ -178,18 +167,8 @@ static void init_ardupilot() // give AHRS the airspeed sensor ahrs.set_airspeed(&airspeed); - // Do GPS init - g_gps = &g_gps_driver; // GPS Initialization - g_gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_4G, &DataFlash); - -#if GPS2_ENABLE - if (hal.uartE != NULL) { - g_gps2 = &g_gps2_driver; - g_gps2->init(hal.uartE, GPS::GPS_ENGINE_AIRBORNE_4G, &DataFlash); - g_gps2->set_secondary(); - } -#endif + gps.init(&DataFlash); //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id @@ -290,12 +269,6 @@ static void startup_ground(void) hal.uartD->set_blocking_writes(false); } -#if 0 - // leave GPS blocking until we have support for correct handling - // of GPS config in uBlox when non-blocking - hal.uartB->set_blocking_writes(false); -#endif - gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 539b5da357..b3c44c764c 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -20,7 +20,6 @@ static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); static int8_t test_logging(uint8_t argc, const Menu::arg *argv); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 @@ -52,7 +51,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"adc", test_adc}, #endif {"gps", test_gps}, - {"rawgps", test_rawgps}, {"ins", test_ins}, {"airspeed", test_airspeed}, {"airpressure", test_pressure}, @@ -406,18 +404,21 @@ test_gps(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); + uint32_t last_message_time_ms = 0; while(1) { delay(100); - g_gps->update(); + gps.update(); - if (g_gps->new_data) { + if (gps.last_message_time_ms() != last_message_time_ms) { + last_message_time_ms = gps.last_message_time_ms(); + const Location &loc = gps.location(); 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_cm/100, - (int)g_gps->num_sats); - }else{ + (long)loc.lat, + (long)loc.lng, + (long)loc.alt/100, + (int)gps.num_sats()); + } else { cliSerial->printf_P(PSTR(".")); } if(cliSerial->available() > 0) { @@ -609,26 +610,6 @@ test_pressure(uint8_t argc, const Menu::arg *argv) } } -static int8_t -test_rawgps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1) { - // Blink Yellow LED if we are sending data to GPS - if (hal.uartC->available()) { - hal.uartB->write(hal.uartC->read()); - } - // Blink Red LED if we are receiving data from GPS - if (hal.uartB->available()) { - hal.uartC->write(hal.uartB->read()); - } - if(cliSerial->available() > 0) { - return (0); - } - } -} #endif // HIL_MODE == HIL_MODE_DISABLED #endif // CLI_ENABLED