diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index c9f26c982d..5a9534984f 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -30,9 +30,7 @@ #include #include #endif -#ifndef HAL_PERIPH_ENABLE_GPS -#define HAL_PERIPH_ENABLE_GPS 1 -#endif + extern const AP_HAL::HAL &hal; AP_Periph_FW periph; @@ -148,7 +146,8 @@ void AP_Periph_FW::init() #ifdef HAL_PERIPH_ENABLE_GPS if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE && g.gps_port >= 0) { - serial_manager.set_protocol_and_baud(g.gps_port, AP_SerialManager::SerialProtocol_GPS, AP_SERIALMANAGER_GPS_BAUD); + hal.serial(3)->printf("gps port: %d\n", (uint8_t)g.gps_port); + serial_manager.set_protocol_and_baud(g.gps_port, AP_SerialManager::SerialProtocol_GPS, 115200); #if HAL_LOGGING_ENABLED #define MASK_LOG_GPS (1<<2) gps.set_log_gps_bit(MASK_LOG_GPS); @@ -209,9 +208,9 @@ void AP_Periph_FW::update() { static uint32_t last_led_ms; uint32_t now = AP_HAL::native_millis(); - if (now - last_led_ms > 500) { + if (now - last_led_ms > 1000) { last_led_ms = now; - hal.serial(1)->printf("update: %lu\n", now); + hal.serial(3)->printf("update: %lu,status: %u,sat:%u\n\r", now,(unsigned)gps.status(),(unsigned)gps.num_sats()); #ifdef HAL_GPIO_PIN_LED if (!no_iface_finished_dna) { @@ -220,7 +219,7 @@ void AP_Periph_FW::update() #endif #if 1 #ifdef HAL_PERIPH_ENABLE_GPS - hal.serial(1)->printf("GPS status: %u\n", (unsigned)gps.status()); + hal.serial(3)->printf("GPS status: %u\n", (unsigned)gps.status()); #endif hal.scheduler->delay(1); #endif diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 6753b1da14..6b236f6e18 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -53,7 +53,7 @@ def build(bld): 'AP_ROMFS', 'AP_MSP', # 'SRV_Channel', - 'AP_Notify', + # 'AP_Notify', 'AP_SerialLED', 'AP_Filesystem', # 'AP_InertialSensor', diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 7a29a62992..e3e2e5e692 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -751,13 +751,13 @@ void AP_GPS::detect_instance(uint8_t instance) new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); } #endif -#if AP_GPS_ERB_ENABLED +#if 1 else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); } #endif // AP_GPS_ERB_ENABLED -#if AP_GPS_NMEA_ENABLED +#if 1 else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA || _type[instance] == GPS_TYPE_HEMI || _type[instance] == GPS_TYPE_ALLYSTAR) && diff --git a/libraries/AP_HAL_ChibiOS/hwdef/f103-periph/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/f103-periph/hwdef.inc index fbb546c011..d4e636b314 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/f103-periph/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/f103-periph/hwdef.inc @@ -135,4 +135,5 @@ define HAL_CAN_RX_QUEUE_SIZE 32 # disable stack checking to reduce flash cost define CH_DBG_ENABLE_STACK_CHECK FALSE -define HAL_GCS_ENABLED 0 \ No newline at end of file +define HAL_GCS_ENABLED 0 +define HAL_PERIPH_ENABLE_GPS 1 \ No newline at end of file