Skip to content

Commit

Permalink
update bp_mavlink, incoude two CAN PRs
Browse files Browse the repository at this point in the history
  • Loading branch information
olliw42 committed Sep 21, 2024
1 parent 9f4bf1b commit 8f49cdd
Show file tree
Hide file tree
Showing 5 changed files with 46 additions and 5 deletions.
5 changes: 4 additions & 1 deletion bp_mavlink/ardupilotmega.xml
Original file line number Diff line number Diff line change
Expand Up @@ -314,7 +314,7 @@
<description>Change target altitude at a given rate. This slews the vehicle at a controllable rate between it's previous altitude and the new one. (affects GUIDED only. Outside GUIDED, aircraft ignores these commands. Designed for onboard companion-computer command-and-control, not normally operator/GCS control.)</description>
<param index="1">Empty</param>
<param index="2">Empty</param>
<param index="3" label="alt rate-of-change" units="m/s/s" minValue="0">Rate of change, toward new altitude. 0 for maximum rate change. Positive numbers only, as negative numbers will not converge on the new target alt.</param>
<param index="3" label="alt rate-of-change" units="m/s" minValue="0">Rate of change, toward new altitude. 0 for maximum rate change. Positive numbers only, as negative numbers will not converge on the new target alt.</param>
<param index="4">Empty</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
Expand Down Expand Up @@ -998,6 +998,9 @@
<entry value="512" name="EKF_PRED_POS_HORIZ_ABS">
<description>Set if EKF's predicted horizontal position (absolute) estimate is good.</description>
</entry>
<entry value="32768" name="EKF_GPS_GLITCHING">
<description>Set if EKF believes the GPS input data is faulty.</description>
</entry>
<entry value="1024" name="EKF_UNINITIALIZED">
<description>Set if EKF has never been healthy.</description>
</entry>
Expand Down
20 changes: 20 additions & 0 deletions bp_mavlink/common.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3310,6 +3310,9 @@
<entry value="2048" name="CAMERA_CAP_FLAGS_HAS_TRACKING_GEO_STATUS">
<description>Camera supports tracking geo status (CAMERA_TRACKING_GEO_STATUS).</description>
</entry>
<entry value="4096" name="CAMERA_CAP_FLAGS_HAS_THERMAL_RANGE">
<description>Camera supports absolute thermal range (request CAMERA_THERMAL_RANGE with MAV_CMD_REQUEST_MESSAGE) (WIP).</description>
</entry>
</enum>
<enum name="VIDEO_STREAM_STATUS_FLAGS" bitmask="true">
<description>Stream status flags (Bitmap)</description>
Expand All @@ -3319,6 +3322,9 @@
<entry value="2" name="VIDEO_STREAM_STATUS_FLAGS_THERMAL">
<description>Stream is thermal imaging</description>
</entry>
<entry value="4" name="VIDEO_STREAM_STATUS_FLAGS_THERMAL_RANGE_ENABLED">
<description>Stream can report absolute thermal range (see CAMERA_THERMAL_RANGE). (WIP).</description>
</entry>
</enum>
<enum name="VIDEO_STREAM_TYPE">
<description>Video stream types</description>
Expand Down Expand Up @@ -6105,6 +6111,20 @@
<field type="float" name="hdg" units="rad" invalid="NaN">Heading in radians, in NED. NAN if unknown</field>
<field type="float" name="hdg_acc" units="rad" invalid="NaN">Accuracy of heading, in NED. NAN if unknown</field>
</message>
<message id="277" name="CAMERA_THERMAL_RANGE">
<wip/>
<!-- This message is work-in-progress and it can therefore change. It should NOT be used in stable production environments. -->
<description>Camera absolute thermal range. This can be streamed when the associated `VIDEO_STREAM_STATUS.flag` bit `VIDEO_STREAM_STATUS_FLAGS_THERMAL_RANGE_ENABLED` is set, but a GCS may choose to only request it for the current active stream. Use MAV_CMD_SET_MESSAGE_INTERVAL to define message interval (param3 indicates the stream id of the current camera, or 0 for all streams, param4 indicates the target camera_device_id for autopilot-attached cameras or 0 for MAVLink cameras).</description>
<field type="uint32_t" name="time_boot_ms" units="ms">Timestamp (time since system boot).</field>
<field type="uint8_t" name="stream_id" minValue="1" instance="true">Video Stream ID (1 for first, 2 for second, etc.)</field>
<field type="uint8_t" name="camera_device_id" default="0" minValue="0" maxValue="6">Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).</field>
<field type="float" name="max" units="degC">Temperature max.</field>
<field type="float" name="max_point_x" invalid="NaN">Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown.</field>
<field type="float" name="max_point_y" invalid="NaN">Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown.</field>
<field type="float" name="min" units="degC">Temperature min.</field>
<field type="float" name="min_point_x" invalid="NaN">Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown.</field>
<field type="float" name="min_point_y" invalid="NaN">Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown.</field>
</message>
<message id="280" name="GIMBAL_MANAGER_INFORMATION">
<description>Information about a high level gimbal manager. This message should be requested by a ground station using MAV_CMD_REQUEST_MESSAGE.</description>
<field type="uint32_t" name="time_boot_ms" units="ms">Timestamp (time since system boot).</field>
Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_CANManager/AP_CANManager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -430,6 +430,8 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
for (auto can_iface : hal.can) {
if (can_iface && can_forward.callback_id != 0) {
can_iface->unregister_frame_callback(can_forward.callback_id);
//OW https://github.com/ArduPilot/ardupilot/pull/28182
can_forward.callback_id = 0;
}
}
return true;
Expand All @@ -451,6 +453,8 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
if (i != bus && hal.can[i] != nullptr && can_forward.callback_id != 0) {
hal.can[i]->unregister_frame_callback(can_forward.callback_id);
//OW https://github.com/ArduPilot/ardupilot/pull/28182
can_forward.callback_id = 0;
}
}

Expand Down Expand Up @@ -647,6 +651,8 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
if (can_forward.callback_id != 0 &&
AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) {
hal.can[bus]->unregister_frame_callback(can_forward.callback_id);
//OW https://github.com/ArduPilot/ardupilot/pull/28182
can_forward.callback_id = 0;
return;
}
can_forward.frame_counter = 0;
Expand Down
11 changes: 9 additions & 2 deletions libraries/AP_DroneCAN/AP_DroneCAN.h
Original file line number Diff line number Diff line change
Expand Up @@ -235,10 +235,16 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {

uint32_t *mem_pool;

AP_DroneCAN_DNA_Server _dna_server;
//OW https://github.com/ArduPilot/ardupilot/pull/28157
// AP_DroneCAN_DNA_Server _dna_server;

uint8_t _driver_index;

//OW https://github.com/ArduPilot/ardupilot/pull/28157
CanardInterface canard_iface;

AP_DroneCAN_DNA_Server _dna_server;

char _thread_name[13];
bool _initialized;
///// SRV output /////
Expand Down Expand Up @@ -292,7 +298,8 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
} _relay;
#endif

CanardInterface canard_iface;
//OW https://github.com/ArduPilot/ardupilot/pull/28157
// CanardInterface canard_iface;

#if AP_DRONECAN_SERIAL_ENABLED
AP_DroneCAN_Serial serial;
Expand Down
9 changes: 7 additions & 2 deletions libraries/bp_version.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#pragma once

#define BETAPILOTVERSION "v060i"
#define DATEOFBASEBRANCH "20240903"
#define BETAPILOTVERSION "v060j"
#define DATEOFBASEBRANCH "20240921"

/*
search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release
Expand All @@ -11,6 +11,11 @@ on-top features:
- RC_Channel, AUX_FUNC: CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) (OW CAMERA)
- Plane THR (OW THR_SUPP)
includes
- https://github.com/ArduPilot/ardupilot/pull/28182
- https://github.com/ArduPilot/ardupilot/pull/28157
remove zflags, not used, could use now options
remove GCS_serial_control: SERIAL_CONTROL 8E1
2024.09.03: v060
Expand Down

0 comments on commit 8f49cdd

Please sign in to comment.