dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

dronecan: gps add noise, jamming, and spoofing data

This commit is contained in:
alexklimaj 2023-10-31 10:25:29 -06:00 committed by Daniel Agar
parent 9da1622436
commit db60bbc46b
3 changed files with 34 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -155,7 +155,7 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN);
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN, -1, -1, 0, 0);
}
void
@ -299,9 +299,16 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
float heading_offset = NAN;
float heading_accuracy = NAN;
int32_t noise_per_ms = -1;
int32_t jamming_indicator = -1;
uint8_t jamming_state = 0;
uint8_t spoofing_state = 0;
// Use ecef_position_velocity for now... There is no heading field
if (!msg.ecef_position_velocity.empty()) {
heading = msg.ecef_position_velocity[0].velocity_xyz[0];
if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[0])) {
heading = msg.ecef_position_velocity[0].velocity_xyz[0];
}
if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
heading_offset = msg.ecef_position_velocity[0].velocity_xyz[1];
@ -310,10 +317,16 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
heading_accuracy = msg.ecef_position_velocity[0].velocity_xyz[2];
}
noise_per_ms = msg.ecef_position_velocity[0].position_xyz_mm[0];
jamming_indicator = msg.ecef_position_velocity[0].position_xyz_mm[1];
jamming_state = msg.ecef_position_velocity[0].position_xyz_mm[2] >> 8;
spoofing_state = msg.ecef_position_velocity[0].position_xyz_mm[2] & 0xFF;
}
process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
heading_accuracy);
heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state);
}
template <typename FixType>
@ -322,7 +335,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
const float (&pos_cov)[9], const float (&vel_cov)[9],
const bool valid_pos_cov, const bool valid_vel_cov,
const float heading, const float heading_offset,
const float heading_accuracy)
const float heading_accuracy, const int32_t noise_per_ms,
const int32_t jamming_indicator, const uint8_t jamming_state,
const uint8_t spoofing_state)
{
sensor_gps_s report{};
report.device_id = get_device_id();
@ -452,6 +467,11 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
report.heading_offset = heading_offset;
report.heading_accuracy = heading_accuracy;
report.noise_per_ms = noise_per_ms;
report.jamming_indicator = jamming_indicator;
report.jamming_state = jamming_state;
report.spoofing_state = spoofing_state;
publish(msg.getSrcNodeID().get(), &report);
}

4
src/drivers/uavcan/sensors/gnss.hpp
View File

@ -89,7 +89,9 @@ private:
const float (&pos_cov)[9], const float (&vel_cov)[9],
const bool valid_pos_cov, const bool valid_vel_cov,
const float heading, const float heading_offset,
const float heading_accuracy);
const float heading_accuracy, const int32_t noise_per_ms,
const int32_t jamming_indicator, const uint8_t jamming_state,
const uint8_t spoofing_state);
void handleInjectDataTopic();
bool PublishRTCMStream(const uint8_t *data, size_t data_len);

9
src/drivers/uavcannode/Publishers/GnssFix2.hpp
View File

@ -127,6 +127,11 @@ public:
ecefpositionvelocity.velocity_xyz[1] = NAN;
ecefpositionvelocity.velocity_xyz[2] = NAN;
// Use ecef_position_velocity for now... There are no fields for these
ecefpositionvelocity.position_xyz_mm[0] = gps.noise_per_ms;
ecefpositionvelocity.position_xyz_mm[1] = gps.jamming_indicator;
ecefpositionvelocity.position_xyz_mm[2] = (gps.jamming_state << 8) | gps.spoofing_state;
// Use ecef_position_velocity for now... There is no heading field
if (!std::isnan(gps.heading)) {
ecefpositionvelocity.velocity_xyz[0] = gps.heading;
@ -138,10 +143,10 @@ public:
if (!std::isnan(gps.heading_accuracy)) {
ecefpositionvelocity.velocity_xyz[2] = gps.heading_accuracy;
}
fix2.ecef_position_velocity.push_back(ecefpositionvelocity);
}
fix2.ecef_position_velocity.push_back(ecefpositionvelocity);
uavcan::Publisher<uavcan::equipment::gnss::Fix2>::broadcast(fix2);
// ensure callback is registered