aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: add a way to transmit MovingBaseline Data on another port

Browse Source
This commit is contained in:
bugobliterator 2021-07-16 21:46:24 +05:30 committed by Andrew Tridgell
parent c7907bb8fb
commit aa9a40acf5
5 changed files with 120 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/AP_Periph/AP_Periph.cpp
View File

@ -147,8 +147,6 @@ void AP_Periph_FW::init()
gps.set_log_gps_bit(MASK_LOG_GPS);
#endif
gps.init(serial_manager);
gps.set_MBL_data_cb(FUNCTOR_BIND_MEMBER(&AP_Periph_FW::send_moving_baseline_msg, void, const uint8_t*&, uint16_t));
gps.set_RelPosHeading_cb(FUNCTOR_BIND_MEMBER(&AP_Periph_FW::send_relposheading_msg, void, uint32_t, float, float, float, float));
}
#endif

9
Tools/AP_Periph/AP_Periph.h
View File

@ -85,10 +85,8 @@ public:
void can_update();
void can_mag_update();
void can_gps_update();
void send_moving_baseline_msg(const uint8_t *&data, uint16_t len);
void send_relposheading_msg(uint32_t timestamp, float reported_heading,
float relative_distance, float relative_down_pos,
float reported_heading_acc);
void send_moving_baseline_msg();
void send_relposheading_msg();
void can_baro_update();
void can_airspeed_update();
void can_rangefinder_update();
@ -111,6 +109,9 @@ public:
#ifdef HAL_PERIPH_ENABLE_GPS
AP_GPS gps;
#if HAL_NUM_CAN_IFACES >= 2
int8_t gps_mb_can_port = -1;
#endif
#endif
#ifdef HAL_PERIPH_ENABLE_MAG

10
Tools/AP_Periph/Parameters.cpp
View File

@ -162,6 +162,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
// @User: Advanced
// @RebootRequired: True
GSCALAR(gps_port, "GPS_PORT", HAL_PERIPH_GPS_PORT_DEFAULT),
#if HAL_NUM_CAN_IFACES >= 2
// @Param: MB_CAN_PORT
// @DisplayName: Moving Baseline CAN Port option
// @Description: Autoselect dedicated CAN port on which moving baseline data will be transmitted.
// @Values: 0:Sends moving baseline data on all ports,1:auto select remaining port for transmitting Moving baseline Data
// @User: Advanced
// @RebootRequired: True
GSCALAR(gps_mb_only_can_port, "GPS_MB_ONLY_PORT", 0),
#endif
#endif
#ifdef HAL_PERIPH_ENABLE_BATTERY

4
Tools/AP_Periph/Parameters.h
View File

@ -48,6 +48,7 @@ public:
k_param_can_protocol2,
k_param_sysid_this_mav,
k_param_serial_manager,
k_param_gps_mb_only_can_port,
};
AP_Int16 format_version;
@ -92,6 +93,9 @@ public:
#ifdef HAL_PERIPH_ENABLE_GPS
AP_Int8 gps_port;
#if HAL_NUM_CAN_IFACES >= 2
AP_Int8 gps_mb_only_can_port;
#endif
#endif
#ifdef HAL_PERIPH_ENABLE_MSP

133
Tools/AP_Periph/can.cpp
View File

@ -82,6 +82,14 @@ extern AP_Periph_FW periph;
#define HAL_CAN_POOL_SIZE 4000
#endif
#define DEBUG_PRINTS 0
#if DEBUG_PRINTS
# define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0)
#else
# define Debug(fmt, args ...)
#endif
static struct instance_t {
uint8_t index;
CanardInstance canard;
@ -102,6 +110,29 @@ static struct instance_t {
#endif
} instances[HAL_NUM_CAN_IFACES];
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1)
static ioline_t can_term_lines[] = {
HAL_GPIO_PIN_TERMCAN1
#if HAL_NUM_CAN_IFACES > 2
#ifdef HAL_GPIO_PIN_TERMCAN2
,HAL_GPIO_PIN_TERMCAN2
#else
#error "Only one Can Terminator defined with over two CAN Ifaces"
#endif
#endif
#if HAL_NUM_CAN_IFACES > 2
#ifdef HAL_GPIO_PIN_TERMCAN3
,HAL_GPIO_PIN_TERMCAN3
#else
#error "Only two Can Terminator defined with three CAN Ifaces"
#endif
#endif
};
#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1)
#ifndef CAN_APP_NODE_NAME
#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph"
#endif
@ -490,6 +521,21 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
canardSetLocalNodeID(ins, allocated_node_id);
printf("IF%d Node ID allocated: %d\n", can_ins->index, allocated_node_id);
#if defined(HAL_PERIPH_ENABLE_GPS) && (HAL_NUM_CAN_IFACES >= 2) && GPS_MOVING_BASELINE
if (periph.g.gps_mb_only_can_port) {
// we need to assign the unallocated port to be used for Moving Baseline only
periph.gps_mb_can_port = (can_ins->index+1)%HAL_NUM_CAN_IFACES;
if (canardGetLocalNodeID(&instances[periph.gps_mb_can_port].canard) == CANARD_BROADCAST_NODE_ID) {
// copy node id from the primary iface
canardSetLocalNodeID(&instances[periph.gps_mb_can_port].canard, allocated_node_id);
#ifdef HAL_GPIO_PIN_TERMCAN1
// also terminate the line as we don't have any other device on this port
palWriteLine(can_term_lines[periph.gps_mb_can_port], 1);
#endif
}
}
#endif
}
}
@ -612,6 +658,7 @@ static void handle_MovingBaselineData(CanardInstance* ins, CanardRxTransfer* tra
return;
}
periph.gps.inject_MBL_data(msg.data.data, msg.data.len);
Debug("MovingBaselineData: len=%u\n", msg.data.len);
}
#endif // GPS_MOVING_BASELINE
@ -1059,13 +1106,18 @@ static void canard_broadcast(uint64_t data_type_signature,
if (canardGetLocalNodeID(&ins.canard) == CANARD_BROADCAST_NODE_ID) {
continue;
}
canardBroadcast(&ins.canard,
data_type_signature,
data_type_id,
&ins.transfer_id,
priority,
payload,
payload_len);
#if defined(HAL_PERIPH_ENABLE_GPS) && HAL_NUM_CAN_IFACES >= 2
if (ins.index != periph.gps_mb_can_port)
#endif
{
canardBroadcast(&ins.canard,
data_type_signature,
data_type_id,
&ins.transfer_id,
priority,
payload,
payload_len);
}
}
}
@ -1624,6 +1676,8 @@ void AP_Periph_FW::can_gps_update(void)
return;
}
gps.update();
send_moving_baseline_msg();
send_relposheading_msg();
if (last_gps_update_ms == gps.last_message_time_ms()) {
return;
}
@ -1842,22 +1896,17 @@ void AP_Periph_FW::can_gps_update(void)
}
void AP_Periph_FW::send_moving_baseline_msg(const uint8_t *&data, uint16_t len)
void AP_Periph_FW::send_moving_baseline_msg()
{
#if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE
const uint8_t *data = nullptr;
uint16_t len = 0;
if (!gps.get_RTCMV3(data, len)) {
return;
}
if (len == 0 || data == nullptr) {
return;
}
// static uint32_t data_cnt = 0;
// static uint32_t last_moving_baseline_ms = 0;
// if (AP_HAL::millis() - last_moving_baseline_ms > 1000) {
// can_printf("RATE: %lu bps", data_cnt);
// data_cnt = 0;
// }
// data_cnt += len;
// if (data_cnt > 0) {
// return;
// }
// send the packet from Moving Base to be used RelPosHeading calc by GPS module
ardupilot_gnss_MovingBaselineData mbldata {};
// get the data from the moving base
@ -1866,22 +1915,44 @@ void AP_Periph_FW::send_moving_baseline_msg(const uint8_t *&data, uint16_t len)
mbldata.data.data = (uint8_t*)data;
uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {};
const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer);
canardBroadcast(&canard,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
#if HAL_NUM_CAN_IFACES >= 2
if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) {
canardBroadcast(&instances[gps_mb_can_port].canard,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
&instances[gps_mb_can_port].transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
} else
#endif
{
canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
gps.clear_RTCMV3();
#endif // HAL_PERIPH_ENABLE_GPS && GPS_MOVING_BASELINE
}
void AP_Periph_FW::send_relposheading_msg(uint32_t timestamp, float reported_heading,
float relative_distance, float relative_down_pos,
float reported_heading_acc) {
void AP_Periph_FW::send_relposheading_msg() {
#if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE
float reported_heading;
float relative_distance;
float relative_down_pos;
float reported_heading_acc;
static uint32_t last_timestamp = 0;
uint32_t curr_timestamp = 0;
gps.get_RelPosHeading(curr_timestamp, reported_heading, relative_distance, relative_down_pos, reported_heading_acc);
if (last_timestamp == curr_timestamp) {
return;
}
last_timestamp = curr_timestamp;
ardupilot_gnss_RelPosHeading relpos {};
relpos.timestamp.usec = uint64_t(timestamp)*1000LLU;
relpos.timestamp.usec = uint64_t(curr_timestamp)*1000LLU;
relpos.reported_heading_deg = reported_heading;
relpos.relative_distance_m = relative_distance;
relpos.relative_down_pos_m = relative_down_pos;
@ -1889,10 +1960,8 @@ void AP_Periph_FW::send_relposheading_msg(uint32_t timestamp, float reported_hea
relpos.reported_heading_acc_available = true;
uint8_t buffer[ARDUPILOT_GNSS_RELPOSHEADING_MAX_SIZE] {};
const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer);
canardBroadcast(&canard,
ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE,
canard_broadcast(ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE,
ARDUPILOT_GNSS_RELPOSHEADING_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);