diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 2f6c11d24e..a50bcdc460 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -183,7 +183,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _AUTO_CONFIG // @DisplayName: Automatic GPS configuration // @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings - // @Values: 0:Disables automatic configuration,1:Enable automatic configuration + // @Values: 0:Disables automatic configuration,1:Enable automatic configuration,2:Enable automatic configuration for UAVCAN as well // @User: Advanced AP_GROUPINFO("_AUTO_CONFIG", 13, AP_GPS, _auto_config, 1), @@ -300,7 +300,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options - // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200 + // @Bitmask: 0:Use UART2/2nd CAN for moving baseline on ublox/uavcan,,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200 // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), #endif @@ -642,7 +642,7 @@ void AP_GPS::detect_instance(uint8_t instance) _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; - if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { + if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { if (_type[instance] == GPS_TYPE_HEMI) { send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || @@ -663,7 +663,7 @@ void AP_GPS::detect_instance(uint8_t instance) } } - if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { + if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { send_blob_update(instance); } @@ -807,7 +807,7 @@ void AP_GPS::update_instance(uint8_t instance) return; } - if (_auto_config == GPS_AUTO_CONFIG_ENABLE) { + if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) { send_blob_update(instance); } @@ -1915,7 +1915,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const happens with the RTCMv3 data */ const uint8_t delay_threshold = 2; - const float delay_avg_max = _type[instance] == (GPS_TYPE_UBLOX_RTK_ROVER || GPS_TYPE_UAVCAN_RTK_ROVER)?245:215; + const float delay_avg_max = ((_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER))?245:215; const GPS_timing &t = timing[instance]; bool delay_ok = (t.delayed_count < delay_threshold) && t.average_delta_ms < delay_avg_max && diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 110c6efae0..d69ae1e233 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -694,7 +694,8 @@ private: // Auto configure types enum GPS_AUTO_CONFIG { GPS_AUTO_CONFIG_DISABLE = 0, - GPS_AUTO_CONFIG_ENABLE = 1 + GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY = 1, + GPS_AUTO_CONFIG_ENABLE_ALL = 2, }; enum class GPSAutoSwitch { diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 596418cb42..edcad23644 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -39,6 +39,21 @@ extern const AP_HAL::HAL& hal; +#define GPS_UAVCAN_DEBUGGING 0 + +#if GPS_UAVCAN_DEBUGGING +#if defined(HAL_BUILD_AP_PERIPH) + extern "C" { + void can_printf(const char *fmt, ...); + } + # define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0) +#else + # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) +#endif +#else + # define Debug(fmt, args ...) +#endif + #define LOG_TAG "GPS" UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix); @@ -58,7 +73,11 @@ HAL_Semaphore AP_GPS_UAVCAN::_sem_registry; AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) : AP_GPS_Backend(_gps, _state, nullptr), role(_role) -{} +{ + param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_int, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); + param_float_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_float, bool, AP_UAVCAN*, const uint8_t, const char*, float &); + param_save_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_save_response, void, AP_UAVCAN*, const uint8_t, bool); +} AP_GPS_UAVCAN::~AP_GPS_UAVCAN() { @@ -185,7 +204,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) return NULL; } // initialise the backend based on the UAVCAN Moving baseline selection - switch (_gps.get_type(found_match)) { + switch (_gps.get_type(_state.instance)) { case AP_GPS::GPS_TYPE_UAVCAN: backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL); break; @@ -738,9 +757,49 @@ void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, ui } #endif +bool AP_GPS_UAVCAN::do_config() +{ + AP_UAVCAN *ap_uavcan = _detected_modules[_detected_module].ap_uavcan; + if (ap_uavcan == nullptr) { + return false; + } + uint8_t node_id = _detected_modules[_detected_module].node_id; + + switch(cfg_step) { + case STEP_SET_TYPE: + ap_uavcan->get_parameter_on_node(node_id, "GPS_TYPE", ¶m_int_cb); + break; + case STEP_SET_MB_CAN_TX: + if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) { + ap_uavcan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", ¶m_int_cb); + } else { + cfg_step++; + } + break; + case STEP_SAVE_AND_REBOOT: + if (requires_save_and_reboot) { + ap_uavcan->save_parameters_on_node(node_id, ¶m_save_cb); + } else { + cfg_step++; + } + break; + case STEP_FINISHED: + return true; + default: + break; + } + return false; +} + // Consume new data and mark it received bool AP_GPS_UAVCAN::read(void) { + if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_ALL) { + if (!do_config()) { + return false; + } + } + WITH_SEMAPHORE(sem); if (_new_data) { _new_data = false; @@ -805,4 +864,67 @@ void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len) } } +/* + handle param get/set response +*/ +bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, int32_t &value) +{ + Debug("AP_GPS_UAVCAN: param set/get response from %d %s %ld\n", node_id, name, value); + if (strcmp(name, "GPS_TYPE") == 0 && cfg_step == STEP_SET_TYPE) { + if (role == AP_GPS::GPS_ROLE_MB_BASE && value != AP_GPS::GPS_TYPE_UBLOX_RTK_BASE) { + value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_BASE; + requires_save_and_reboot = true; + return true; + } else if (role == AP_GPS::GPS_ROLE_MB_ROVER && value != AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER) { + value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER; + requires_save_and_reboot = true; + return true; + } else { + cfg_step++; + } + } + + if (strcmp(name, "GPS_MB_ONLY_PORT") == 0 && cfg_step == STEP_SET_MB_CAN_TX) { + if (gps._driver_options == 0 && value != 1) { + // set up so that another CAN port is used for the Moving Baseline Data + // setting this value will allow another CAN port to be used as dedicated + // line for the Moving Baseline Data + value = 1; + requires_save_and_reboot = true; + return true; + } else if (gps._driver_options != 0 && value != 0) { + // set up so that all CAN ports are used for the Moving Baseline Data + value = 0; + requires_save_and_reboot = true; + return true; + } else { + cfg_step++; + } + } + return false; +} + +bool AP_GPS_UAVCAN::handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, float &value) +{ + Debug("AP_GPS_UAVCAN: param set/get response from %d %s %f\n", node_id, name, value); + return false; +} + +void AP_GPS_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success) +{ + Debug("AP_GPS_UAVCAN: param save response from %d %s\n", node_id, success ? "success" : "failure"); + + if (cfg_step != STEP_SAVE_AND_REBOOT) { + return; + } + + if (success) { + cfg_step++; + } + // Also send reboot command + // this is ok as we are sending from UAVCAN thread context + Debug("AP_GPS_UAVCAN: sending reboot command %d\n", node_id); + ap_uavcan->send_reboot_request(node_id); +} + #endif // HAL_ENABLE_LIBUAVCAN_DRIVERS diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index a3a28aeaeb..71f9d8a9b0 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -71,7 +71,22 @@ public: bool get_RTCMV3(const uint8_t *&data, uint16_t &len) override; void clear_RTCMV3() override; #endif + private: + + bool param_configured = true; + enum config_step { + STEP_SET_TYPE = 0, + STEP_SET_MB_CAN_TX, + STEP_SAVE_AND_REBOOT, + STEP_FINISHED + }; + uint8_t cfg_step; + bool requires_save_and_reboot; + + // returns true once configuration has finished + bool do_config(void); + void handle_fix_msg(const FixCb &cb); void handle_fix2_msg(const Fix2Cb &cb); void handle_aux_msg(const AuxCb &cb); @@ -79,8 +94,8 @@ private: void handle_status_msg(const StatusCb &cb); #if GPS_MOVING_BASELINE - void handle_moving_baseline_msg(const MovingBaselineDataCb &cb); - void handle_relposheading_msg(const RelPosHeadingCb &cb); + void handle_moving_baseline_msg(const MovingBaselineDataCb &cb, uint8_t node_id); + void handle_relposheading_msg(const RelPosHeadingCb &cb, uint8_t node_id); #endif static bool take_registry(); @@ -120,4 +135,11 @@ private: // the role set from GPS_TYPE AP_GPS::GPS_Role role; + FUNCTOR_DECLARE(param_int_cb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); + FUNCTOR_DECLARE(param_float_cb, bool, AP_UAVCAN*, const uint8_t, const char*, float &); + FUNCTOR_DECLARE(param_save_cb, void, AP_UAVCAN*, const uint8_t, bool); + + bool handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, int32_t &value); + bool handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, float &value); + void handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success); }; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index f5ad34acf1..420c65fdcb 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1866,7 +1866,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const // always bail out in this case, it's used to indicate we have yet to receive a valid // hardware generation, however the user may have inhibited us detecting the generation // so if we aren't allowed to do configuration, we will accept this as the default delay - return gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_ENABLE; + return gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE; case UBLOX_5: case UBLOX_6: default: