Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Pr bcast remoteid #3

Closed
wants to merge 2 commits into from
Closed

Pr bcast remoteid #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5e151ef
Modify changes
Cybra13 Jul 11, 2024
aecf516
add Bcast Param
Cybra13 Sep 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions AP_Periph/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
// @User: Advanced
GSCALAR(format_version, "FORMAT_VERSION", 0),

// @PARAM: BCAST_REMOTEID
// @DisplayName: Broadcast RemoteID
// @Description: Broadcast RemoteID
// @Range: 0 1
// @User: Advanced
GSCALAR(bcast_remoteid, "BCAST_REMOTEID", 0),

// @Param: CAN_NODE
// @DisplayName: UAVCAN node that is used for this network
// @Description: UAVCAN node should be set implicitly or 0 for dynamic node allocation
Expand Down
61 changes: 33 additions & 28 deletions AP_Periph/bcast_remoteid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_DroneCAN/AP_DroneCAN.h>
#include <stdio.h>
Expand All @@ -37,9 +38,9 @@ const AP_Param::GroupInfo Bcast_RemoteID::var_info[] = {

// @Param: ENABLE
// @DisplayName: Enable Bcast ODID subsystem
// @Description: Enable ODID subsystem
// @Description: Enable Bcast ODID subsystem
// @Values: 0:Disabled,1:Enabled
AP_GROUPINFO_FLAGS("ENABLE", 1, Bcast_RemoteID, _enable, 0, AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("_ENABLE", 1, Bcast_RemoteID, _enabled, 0, AP_PARAM_FLAG_ENABLE),

AP_GROUPEND
};
Expand All @@ -64,9 +65,14 @@ Bcast_RemoteID::Bcast_RemoteID()

void Bcast_RemoteID::init()
{
if (_enable == 0) {
if (!_enabled) {
return;
}
float value;
AP_Param::get("GPS_RAW_DATA", value);
if (value != 0.0 && value != 1.0) {
AP_Param::set_and_save_by_name("GPS_RAW_DATA", 1);
}

load_UAS_ID_from_persistent_memory();
_initialised = true;
Expand Down Expand Up @@ -121,6 +127,7 @@ void Bcast_RemoteID::get_persistent_params(ExpandingString &str) const
// str.printf("DID_UAS_ID=%s\nDID_UAS_ID_TYPE=%u\nDID_UA_TYPE=%u\n", pkt_basic_id.uas_id, pkt_basic_id.id_type, pkt_basic_id.ua_type);
// }
// TODO: implement this

}

// Perform the pre-arm checks and prevent arming if they are not satisifed
Expand All @@ -142,10 +149,10 @@ bool Bcast_RemoteID::pre_arm_check(char* failmsg, uint8_t failmsg_len)
const uint32_t max_age_ms = 3000;
const uint32_t now_ms = AP_HAL::millis();

if (last_arm_status_ms == 0 || now_ms - last_arm_status_ms > max_age_ms) {
strncpy(failmsg, "ARM_STATUS not available", failmsg_len);
return false;
}
// if (last_arm_status_ms == 0 || now_ms - last_arm_status_ms > max_age_ms) {
// strncpy(failmsg, "ARM_STATUS not available", failmsg_len);
// return false;
// }

if (last_system_ms == 0 ||
(now_ms - last_system_ms > max_age_ms &&
Expand All @@ -164,7 +171,7 @@ bool Bcast_RemoteID::pre_arm_check(char* failmsg, uint8_t failmsg_len)

void Bcast_RemoteID::update()
{
if (_enable == 0) {
if (_enabled == 0) {
return;
}

Expand Down Expand Up @@ -338,8 +345,6 @@ void Bcast_RemoteID::send_location_message()
// accuracy, as we use system timer to propogate time
timestamp_accuracy_mav = create_enum_timestamp_accuracy(1.0);



// Timestamp here is the number of seconds after into the current hour referenced to UTC time (up to one hour)

// FIX we need to only set this if w have a GPS lock is 2D good enough for that?
Expand All @@ -355,25 +360,25 @@ void Bcast_RemoteID::send_location_message()
WITH_SEMAPHORE(_sem);
// take semaphore so CAN gets a consistent packet
pkt_location = mavlink_open_drone_id_location_t{
latitude : latitude,
longitude : longitude,
altitude_barometric : ODID_INV_ALT,
altitude_geodetic : altitude_geodetic,
height : 0,
timestamp : timestamp,
direction : uint16_t(direction * 100.0), // Heading (centi-degrees)
speed_horizontal : uint16_t(speed_horizontal * 100.0), // Ground speed (cm/s)
speed_vertical : int16_t(climb_rate * 100.0), // Climb rate (cm/s)
target_system : 0,
target_component : 0,
latitude : latitude, // Required
longitude : longitude, // Required
altitude_barometric : ODID_INV_ALT, // Optional (not supported)
altitude_geodetic : altitude_geodetic, // Required
height : 0, // Optional
timestamp : timestamp, // Required
direction : uint16_t(direction * 100.0), // Required, Heading (centi-degrees)
speed_horizontal : uint16_t(speed_horizontal * 100.0), // Required, Ground speed (cm/s)
speed_vertical : int16_t(climb_rate * 100.0), // Required, Climb rate (cm/s)
target_system : 0, // Not used
target_component : 0, // Not used
id_or_mac : {},
status : uint8_t(uav_status),
height_reference : MAV_ODID_HEIGHT_REF_OVER_TAKEOFF, // height reference enum: Above takeoff location or above ground
horizontal_accuracy : uint8_t(horizontal_accuracy_mav),
vertical_accuracy : uint8_t(vertical_accuracy_mav),
barometer_accuracy : MAV_ODID_VER_ACC_UNKNOWN,
speed_accuracy : uint8_t(speed_accuracy_mav),
timestamp_accuracy : uint8_t(timestamp_accuracy_mav)
status : uint8_t(uav_status), // Optional, 0 for not declared, 4 for emergency
height_reference : MAV_ODID_HEIGHT_REF_OVER_TAKEOFF, // Optional, height reference enum: Above takeoff location or above ground
horizontal_accuracy : uint8_t(horizontal_accuracy_mav), // Required
vertical_accuracy : uint8_t(vertical_accuracy_mav), // Required
barometer_accuracy : MAV_ODID_VER_ACC_UNKNOWN, // Optional (not supported)
speed_accuracy : uint8_t(speed_accuracy_mav), // Required
timestamp_accuracy : uint8_t(timestamp_accuracy_mav) // Required
};
}

Expand Down
4 changes: 2 additions & 2 deletions AP_Periph/bcast_remoteid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ class Bcast_RemoteID {
void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);

bool enabled(void) const {
return _enable != 0;
return _enabled != 0;
}

void set_arm_status(mavlink_open_drone_id_arm_status_t &status);
Expand All @@ -86,7 +86,7 @@ class Bcast_RemoteID {
private:
bool _initialised;
// parameters
AP_Int8 _enable;
AP_Int8 _enabled;

char ua_type[3];
char id_type[3];
Expand Down
Loading