yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

gps add device_id

Browse Source
This commit is contained in:
Daniel Agar
2021-04-01 21:02:48 -04:00
committed by Beat Küng
parent 1f4960d480
commit 39c96a8884
10 changed files with 104 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-hardware
View File

@@ -1030,6 +1030,7 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gps"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"'
@@ -1041,6 +1042,7 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_gps_position"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position"'

2
msg/sensor_gps.msg
View File

@@ -2,6 +2,8 @@
# the field 'timestamp' is for the position & velocity (microseconds)
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
int32 lat # Latitude in 1E-7 degrees
int32 lon # Longitude in 1E-7 degrees
int32 alt # Altitude in 1E-3 meters above MSL, (millimetres)

16
src/drivers/drv_sensor.h
View File

@@ -177,6 +177,22 @@
#define DRV_DIST_DEVTYPE_SIM 0x9a
#define DRV_DIST_DEVTYPE_SRF05 0x9b
#define DRV_GPS_DEVTYPE_ASHTECH 0xA0
#define DRV_GPS_DEVTYPE_EMLID_REACH 0xA1
#define DRV_GPS_DEVTYPE_FEMTOMES 0xA2
#define DRV_GPS_DEVTYPE_MTK 0xA3
#define DRV_GPS_DEVTYPE_SBF 0xA4
#define DRV_GPS_DEVTYPE_UBX 0xA5
#define DRV_GPS_DEVTYPE_UBX_6 0xA6
#define DRV_GPS_DEVTYPE_UBX_7 0xA7
#define DRV_GPS_DEVTYPE_UBX_8 0xA8
#define DRV_GPS_DEVTYPE_UBX_9 0xA9
#define DRV_GPS_DEVTYPE_UBX_F9P 0xAA
#define DRV_GPS_DEVTYPE_SIM 0xAF
#define DRV_DEVTYPE_UNUSED 0xff
#endif /* _DRV_SENSOR_H */

2
src/drivers/gps/devices

Submodule src/drivers/gps/devices updated: f2eb62c2c7...ac1b5ce8e0

60
src/drivers/gps/gps.cpp
View File

@@ -47,6 +47,8 @@
#include <termios.h>
#include <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
@@ -91,7 +93,7 @@ struct GPS_Sat_Info {
static constexpr int TASK_STACK_SIZE = 1760;
class GPS : public ModuleBase<GPS>
class GPS : public ModuleBase<GPS>, public device::Device
{
public:
@@ -260,6 +262,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info,
Instance instance, unsigned configured_baudrate) :
Device(MODULE_NAME),
_configured_baudrate(configured_baudrate),
_mode(mode),
_interface(interface),
@@ -280,6 +283,16 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
}
if (_interface == GPSHelper::Interface::UART) {
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
set_device_bus(atoi(&c));
} else if (_interface == GPSHelper::Interface::SPI) {
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
}
if (_mode == GPS_DRIVER_MODE_NONE) {
// use parameter to select mode if not provided via CLI
char protocol_param_name[16];
@@ -345,7 +358,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
case GPSCallbackType::writeDeviceData:
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
return write(gps->_serial_fd, data1, (size_t)data2);
return ::write(gps->_serial_fd, data1, (size_t)data2);
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
@@ -417,7 +430,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
#ifdef __PX4_NUTTX
int err = 0;
int bytes_available = 0;
err = ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
if (err != 0 || bytes_available < (int)character_count) {
px4_usleep(sleeptime);
@@ -662,14 +675,14 @@ GPS::run()
if (_interface == GPSHelper::Interface::SPI) {
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
return;
}
status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
@@ -743,19 +756,23 @@ GPS::run()
case GPS_DRIVER_MODE_UBX:
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
gps_ubx_dynmodel, heading_offset, ubx_mode);
set_device_type(DRV_GPS_DEVTYPE_UBX);
break;
#ifndef CONSTRAINED_FLASH
case GPS_DRIVER_MODE_MTK:
_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
set_device_type(DRV_GPS_DEVTYPE_MTK);
break;
case GPS_DRIVER_MODE_ASHTECH:
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
set_device_type(DRV_GPS_DEVTYPE_ASHTECH);
break;
case GPS_DRIVER_MODE_EMLIDREACH:
_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
break;
#endif // CONSTRAINED_FLASH
@@ -779,6 +796,37 @@ GPS::run()
/* GPS is obviously detected successfully, reset statistics */
_helper->resetUpdateRates();
// populate specific ublox model
if (get_device_type() == DRV_GPS_DEVTYPE_UBX) {
GPSDriverUBX *driver_ubx = (GPSDriverUBX *)_helper;
switch (driver_ubx->board()) {
case GPSDriverUBX::Board::u_blox6:
set_device_type(DRV_GPS_DEVTYPE_UBX_6);
break;
case GPSDriverUBX::Board::u_blox7:
set_device_type(DRV_GPS_DEVTYPE_UBX_7);
break;
case GPSDriverUBX::Board::u_blox8:
set_device_type(DRV_GPS_DEVTYPE_UBX_8);
break;
case GPSDriverUBX::Board::u_blox9:
set_device_type(DRV_GPS_DEVTYPE_UBX_9);
break;
case GPSDriverUBX::Board::u_blox9_F9P:
set_device_type(DRV_GPS_DEVTYPE_UBX_F9P);
break;
default:
set_device_type(DRV_GPS_DEVTYPE_UBX);
break;
}
}
}
int helper_ret;
@@ -987,6 +1035,8 @@ void
GPS::publish()
{
if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
_report_gps_pos.device_id = get_device_id();
_report_gps_pos_pub.publish(_report_gps_pos);
// Heading/yaw data can be updated at a lower rate than the other navigation data.
// The uORB message definition requires this data to be set to a NAN if no new valid data is available.

3
src/drivers/uavcan/sensors/gnss.cpp
View File

@@ -64,6 +64,8 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
for (uint8_t i = 0; i < _max_channels; i++) {
_channel_using_fix2[i] = false;
}
set_device_type(DRV_GPS_DEVTYPE_UAVCAN);
}
UavcanGnssBridge::~UavcanGnssBridge()
@@ -282,6 +284,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
const bool valid_pos_cov, const bool valid_vel_cov)
{
sensor_gps_s report{};
report.device_id = get_device_id();
/*
* FIXME HACK

1
src/drivers/uavcan/sensors/sensor_bridge.cpp
View File

@@ -114,6 +114,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report)
// update device id as we now know our device node_id
_device_id.devid_s.address = static_cast<uint8_t>(node_id);
_device_id.devid_s.bus_type = DeviceBusType::DeviceBusType_UAVCAN;
// Publish to the appropriate topic, abort on failure
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->instance);

6
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -2232,6 +2232,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
sensor_gps_s gps{};
device::Device::DeviceId device_id{};
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
gps.device_id = device_id.devid;
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;

9
src/modules/sih/sih.cpp
View File

@@ -46,6 +46,7 @@
#include <px4_platform_common/log.h>
#include <drivers/drv_pwm_output.h> // to get PWM flags
#include <lib/drivers/device/Device.hpp>
using namespace math;
using namespace matrix;
@@ -364,6 +365,14 @@ void Sih::send_gps()
gps_no_fix();
}
// device id
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
device_id.devid_s.bus = 0;
device_id.devid_s.address = 0;
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
_sensor_gps.device_id = device_id.devid;
_sensor_gps_pub.publish(_sensor_gps);
}

9
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -44,6 +44,7 @@
#include <drivers/drv_pwm_output.h>
#include <conversion/rotation.h>
#include <mathlib/mathlib.h>
#include <lib/drivers/device/Device.hpp>
#include <arpa/inet.h>
#include <errno.h>
@@ -432,6 +433,14 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
if (_sensor_gps_pubs[i] == nullptr) {
_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
_gps_ids[i] = hil_gps.id;
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
device_id.devid_s.bus = 0;
device_id.devid_s.address = i;
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
gps.device_id = device_id.devid;
_sensor_gps_pubs[i]->publish(gps);
break;
}