Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: add support for auto config Moving Baseline for AP_Periph GPS

Browse Source
This commit is contained in:
bugobliterator 2021-07-16 21:51:01 +05:30 committed by Andrew Tridgell
parent aa9a40acf5
commit 85d8becb8c
5 changed files with 157 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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 &&

3
libraries/AP_GPS/AP_GPS.h
View File

@ -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 {

126
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -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", &param_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", &param_int_cb);
} else {
cfg_step++;
}
break;
case STEP_SAVE_AND_REBOOT:
if (requires_save_and_reboot) {
ap_uavcan->save_parameters_on_node(node_id, &param_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

26
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -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);
};

2
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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: