mirror of https://github.com/ArduPilot/ardupilot
Copter: convert to new GPS API
This commit is contained in:
parent
f4079f57b2
commit
640b64f5e4
|
@ -23,8 +23,6 @@
|
|||
# define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
|
||||
#endif
|
||||
//#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space
|
||||
//#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX // hard code GPS to Ublox to save 8k of flash
|
||||
//#define GPS_PROTOCOL GPS_PROTOCOL_MTK19 // hard cdoe GPS to Mediatek to save 10k of flash
|
||||
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
|
||||
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
|
||||
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space
|
||||
|
|
|
@ -28,11 +28,6 @@
|
|||
|
||||
#define HIL_PORT 3
|
||||
|
||||
// You can set your gps protocol here for your actual
|
||||
// hardware and leave it without affecting the hardware
|
||||
// in the loop simulation
|
||||
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
|
||||
|
||||
// Sensors
|
||||
// All sensors are supported in all modes.
|
||||
#define MAGNETOMETER ENABLED
|
||||
|
|
|
@ -237,12 +237,9 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
|
|||
// 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
|
||||
static GPS_Glitch gps_glitch(g_gps);
|
||||
static AP_GPS gps;
|
||||
|
||||
static GPS_Glitch gps_glitch(gps);
|
||||
|
||||
// flight modes convenience array
|
||||
static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||
|
@ -295,41 +292,11 @@ static AP_Compass_HMC5843 compass;
|
|||
#endif
|
||||
#endif
|
||||
|
||||
// real 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;
|
||||
|
||||
#else
|
||||
#error Unrecognised GPS_PROTOCOL setting.
|
||||
#endif // GPS PROTOCOL
|
||||
|
||||
#elif HIL_MODE != HIL_MODE_DISABLED
|
||||
// sensor emulators
|
||||
static AP_ADC_HIL adc;
|
||||
static AP_Baro_HIL barometer;
|
||||
static AP_Compass_HIL compass;
|
||||
static AP_GPS_HIL g_gps_driver;
|
||||
static AP_InertialSensor_HIL ins;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
|
@ -343,9 +310,9 @@ static SITL sitl;
|
|||
|
||||
// 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
|
||||
|
||||
// Mission library
|
||||
|
@ -655,9 +622,9 @@ static float G_Dt = 0.02;
|
|||
// Inertial Navigation
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, g_gps, gps_glitch);
|
||||
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
|
||||
#else
|
||||
static AP_InertialNav inertial_nav(ahrs, barometer, g_gps, gps_glitch);
|
||||
static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch);
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -716,14 +683,12 @@ static AP_HAL::AnalogSource* rssi_analog_source;
|
|||
// --------------------------------------
|
||||
#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
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -1203,15 +1168,15 @@ static void update_GPS(void)
|
|||
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location
|
||||
bool report_gps_glitch;
|
||||
|
||||
g_gps->update();
|
||||
gps.update();
|
||||
|
||||
// logging and glitch protection run after every gps message
|
||||
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();
|
||||
|
||||
// log GPS message
|
||||
if (g.log_bitmask & MASK_LOG_GPS) {
|
||||
DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
|
||||
DataFlash.Log_Write_GPS(gps, current_loc.alt);
|
||||
}
|
||||
|
||||
// run glitch protection and update AP_Notify if home has been initialised
|
||||
|
@ -1227,34 +1192,31 @@ static void update_GPS(void)
|
|||
AP_Notify::flags.gps_glitching = report_gps_glitch;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// checks to initialise home and take location based pictures
|
||||
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
|
||||
// clear new data flag
|
||||
g_gps->new_data = false;
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
|
||||
// check if we can initialise home yet
|
||||
if (!ap.home_is_set) {
|
||||
// if we have a 3d lock and valid location
|
||||
if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) {
|
||||
if( ground_start_count > 0 ) {
|
||||
if(gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.location().lat != 0) {
|
||||
if (ground_start_count > 0 ) {
|
||||
ground_start_count--;
|
||||
}else{
|
||||
} else {
|
||||
// after 10 successful reads store home location
|
||||
// ap.home_is_set will be true so this will only happen once
|
||||
ground_start_count = 0;
|
||||
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);
|
||||
compass.set_initial_location(gps.location().lat, gps.location().lng);
|
||||
}
|
||||
}
|
||||
}else{
|
||||
} else {
|
||||
// start again if we lose 3d lock
|
||||
ground_start_count = 10;
|
||||
}
|
||||
|
@ -1266,22 +1228,10 @@ static void update_GPS(void)
|
|||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// check for loss of gps
|
||||
failsafe_gps_check();
|
||||
|
||||
#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 (g.log_bitmask & MASK_LOG_GPS) {
|
||||
DataFlash.Log_Write_GPS2(g_gps2);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
static void
|
||||
|
|
|
@ -67,8 +67,8 @@ static float get_roi_yaw()
|
|||
static float get_look_ahead_yaw()
|
||||
{
|
||||
// Commanded Yaw to automatically look ahead.
|
||||
if (g_gps->fix && g_gps->ground_speed_cm > YAW_LOOK_AHEAD_MIN_SPEED) {
|
||||
yaw_look_ahead_bearing = g_gps->ground_course_cd;
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed_cm() > YAW_LOOK_AHEAD_MIN_SPEED) {
|
||||
yaw_look_ahead_bearing = gps.ground_course_cd();
|
||||
}
|
||||
return yaw_look_ahead_bearing;
|
||||
}
|
||||
|
|
|
@ -151,7 +151,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
|
|||
if (g.compass_enabled) {
|
||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
|
||||
}
|
||||
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 OPTFLOW == ENABLED
|
||||
|
@ -188,7 +188,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::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
|
||||
if (gps.status() > AP_GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
|
||||
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
|
||||
}
|
||||
if (ap.rc_receiver_present && !failsafe.radio) {
|
||||
|
@ -229,21 +229,22 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
|||
// 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 = gps.last_fix_time_ms();
|
||||
} else {
|
||||
fix_time = millis();
|
||||
}
|
||||
const Vector3f &vel = gps.velocity();
|
||||
mavlink_msg_global_position_int_send(
|
||||
chan,
|
||||
fix_time,
|
||||
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
|
||||
(current_loc.alt - home.alt) * 10, // 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.x * -100, // Z speed cm/s (+ve up)
|
||||
ahrs.yaw_sensor); // compass heading in 1/100 degree
|
||||
}
|
||||
|
||||
|
@ -295,35 +296,38 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
|
|||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||
{
|
||||
const Location &loc = gps.location(0);
|
||||
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,
|
||||
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,
|
||||
g_gps->ground_speed_cm, // cm/s
|
||||
g_gps->ground_course_cd, // 1/100 degrees,
|
||||
g_gps->num_sats);
|
||||
gps.ground_speed(0)*100, // cm/s
|
||||
gps.ground_course_cd(0), // 1/100 degrees,
|
||||
gps.num_sats(0));
|
||||
|
||||
#if GPS2_ENABLE
|
||||
if (g_gps2 != NULL && g_gps2->status() != GPS::NO_GPS) {
|
||||
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
|
||||
if (gps.num_sensors() > 1 &&
|
||||
gps.status(1) > AP_GPS::NO_GPS) {
|
||||
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
|
||||
const Location &loc2 = gps.location(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),
|
||||
loc2.lat,
|
||||
loc2.lng,
|
||||
loc2.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);
|
||||
}
|
||||
|
@ -335,7 +339,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());
|
||||
}
|
||||
|
||||
|
@ -484,8 +488,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
|
|||
{
|
||||
mavlink_msg_vfr_hud_send(
|
||||
chan,
|
||||
(float)g_gps->ground_speed_cm / 100.0f,
|
||||
(float)g_gps->ground_speed_cm / 100.0f,
|
||||
gps.ground_speed(),
|
||||
gps.ground_speed(),
|
||||
(ahrs.yaw_sensor / 100) % 360,
|
||||
g.rc_3.servo_out/10,
|
||||
current_loc.alt / 100.0f,
|
||||
|
@ -1339,18 +1343,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
mavlink_hil_state_t packet;
|
||||
mavlink_msg_hil_state_decode(msg, &packet);
|
||||
|
||||
float vel = pythagorous2(packet.vx, packet.vy);
|
||||
float cog = wrap_360_cd(ToDeg(atan2f(packet.vx, packet.vy)) * 100);
|
||||
|
||||
// if we are erasing the dataflash this object doesnt exist yet. as its called from delay_cb
|
||||
if (g_gps == NULL)
|
||||
break;
|
||||
|
||||
// set gps hil sensor
|
||||
g_gps->setHIL(GPS::FIX_3D,
|
||||
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;
|
||||
|
||||
gps.setHIL(AP_GPS::GPS_OK_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);
|
||||
loc, vel, 10);
|
||||
|
||||
if (!ap.home_is_set) {
|
||||
init_home();
|
||||
|
|
|
@ -627,8 +627,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,
|
||||
|
|
|
@ -5,7 +5,12 @@
|
|||
static void init_home()
|
||||
{
|
||||
set_home_is_set(true);
|
||||
ahrs.set_home(g_gps->latitude, g_gps->longitude, 0);
|
||||
|
||||
// copter uses 0 home altitude
|
||||
Location loc = gps.location();
|
||||
loc.alt = 0;
|
||||
|
||||
ahrs.set_home(loc);
|
||||
|
||||
inertial_nav.setup_home_position();
|
||||
|
||||
|
|
|
@ -708,7 +708,9 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
|
|||
if(cmd.p1 == 1) {
|
||||
init_home();
|
||||
} else {
|
||||
ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, 0);
|
||||
Location loc = cmd.content.location;
|
||||
loc.alt = 0;
|
||||
ahrs.set_home(loc);
|
||||
set_home_is_set(true);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -107,14 +107,6 @@
|
|||
# define MAIN_LOOP_MICROS 10000
|
||||
#endif
|
||||
|
||||
// 2nd GPS support
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||||
#define GPS2_ENABLE 1
|
||||
#else
|
||||
#define GPS2_ENABLE 0
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// FRAME_CONFIG
|
||||
//
|
||||
|
@ -269,22 +261,9 @@
|
|||
|
||||
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
|
||||
|
||||
# undef GPS_PROTOCOL
|
||||
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
|
||||
|
||||
#undef CONFIG_SONAR
|
||||
#define CONFIG_SONAR DISABLED
|
||||
|
||||
#undef GPS2_ENABLE
|
||||
#define GPS2_ENABLE 0
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// GPS_PROTOCOL
|
||||
//
|
||||
#ifndef GPS_PROTOCOL
|
||||
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
|
||||
#endif
|
||||
|
||||
|
||||
|
|
|
@ -77,17 +77,6 @@
|
|||
#define ToRad(x) radians(x) // *pi/180
|
||||
#define ToDeg(x) degrees(x) // *180/pi
|
||||
|
||||
// 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
|
||||
#define HIL_MODE_DISABLED 0
|
||||
#define HIL_MODE_SENSORS 1
|
||||
|
|
|
@ -436,7 +436,7 @@ static bool pre_arm_gps_checks(bool display_failure)
|
|||
}
|
||||
|
||||
// warn about hdop separately - to prevent user confusion with no gps lock
|
||||
if (g_gps->hdop > g.gps_hdop_good) {
|
||||
if (gps.get_hdop() > g.gps_hdop_good) {
|
||||
if (display_failure) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
|
||||
}
|
||||
|
|
|
@ -227,17 +227,7 @@ static void init_ardupilot()
|
|||
#endif // CONFIG_ADC
|
||||
|
||||
// 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);
|
||||
|
||||
if(g.compass_enabled)
|
||||
init_compass();
|
||||
|
@ -338,7 +328,8 @@ static void startup_ground(bool force_gyro_cal)
|
|||
// returns true if the GPS is ok and home position is set
|
||||
static bool GPS_ok()
|
||||
{
|
||||
if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D && !gps_glitch.glitching() && !failsafe.gps) {
|
||||
if (ap.home_is_set && gps.status() == AP_GPS::GPS_OK_FIX_3D &&
|
||||
!gps_glitch.glitching() && !failsafe.gps) {
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
|
|
Loading…
Reference in New Issue