void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg)
{
// Called whenever an ESC thinks it has received user input.
warnx("UAVCAN ESC enumeration: got indication");
if (!_esc_enumeration_active) {
// Ignore any messages received when we're not expecting them
return;
}
// First, check if we've already seen an indication from this ESC. If so, just ignore this indication.
int i = 0;
for (; i < _esc_enumeration_index; i++) {
if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
return;
}
}
uavcan::protocol::param::GetSet::Request req;
req.name = msg.parameter_name; // 'esc_index' or something alike, the name is not standardized
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
}
}
static void air_data_sp_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get());
if (state != nullptr) {
state->pressure = msg.static_pressure;
state->pressure_variance = msg.static_pressure_variance;
// after all is filled, update all listeners with new data
ap_uavcan->update_baro_state(msg.getSrcNodeID().get());
}
}
}
}
static void magnetic_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_UAVCAN::Mag_Info *state = ap_uavcan->find_mag_node(msg.getSrcNodeID().get());
if (state != nullptr) {
state->mag_vector[0] = msg.magnetic_field_ga[0];
state->mag_vector[1] = msg.magnetic_field_ga[1];
state->mag_vector[2] = msg.magnetic_field_ga[2];
// after all is filled, update all listeners with new data
ap_uavcan->update_mag_state(msg.getSrcNodeID().get());
}
}
}
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
auto &ref = _esc_status.esc[msg.esc_index];
ref.esc_address = msg.getSrcNodeID().get();
ref.esc_voltage = msg.voltage;
ref.esc_current = msg.current;
ref.esc_temperature = msg.temperature;
ref.esc_setpoint = msg.power_rating_pct;
ref.esc_rpm = msg.rpm;
ref.esc_errorcount = msg.error_count;
}
}
// Temperature is not main parameter so do not update listeners when it is received
static void air_data_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get());
if (state != nullptr) {
state->temperature = msg.static_temperature;
state->temperature_variance = msg.static_temperature_variance;
}
}
}
}
static void gnss_aux_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get());
if (state != nullptr) {
if (!uavcan::isNaN(msg.hdop)) {
state->hdop = msg.hdop * 100.0;
}
if (!uavcan::isNaN(msg.vdop)) {
state->vdop = msg.vdop * 100.0;
}
}
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
&msg)
{
lock();
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
/*
* FIXME HACK
* This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
* to use an independent time source (based on hardware TIM5) instead of HRT.
* The proper solution is to be developed.
*/
_report.timestamp = hrt_absolute_time();
_report.device_id = _device_id.devid;
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
unlock();
publish(msg.getSrcNodeID().get(), &_report);
}
void UavcanBarometerBridge::air_pressure_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
{
baro_report report;
/*
* FIXME HACK
* This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
* to use an independent time source (based on hardware TIM5) instead of HRT.
* The proper solution is to be developed.
*/
report.timestamp = hrt_absolute_time();
report.temperature = last_temperature_kelvin - 273.15F;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0;
/*
* Altitude computation
* Refer to the MS5611 driver for details
*/
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s
const double R = 287.05; // ideal gas constant in J/kg/K
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer
_reports->force(&report);
publish(msg.getSrcNodeID().get(), &report);
}
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
// This bridge does not support redundant GNSS receivers yet.
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
}
}
auto report = ::vehicle_gps_position_s();
/*
* FIXME HACK
* There used to be the following line of code:
* report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
* to use an independent time source (based on hardware TIM5) instead of HRT.
* The proper solution is to be developed.
*/
report.timestamp = hrt_absolute_time();
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
// Check if the msg contains valid covariance information
const bool valid_position_covariance = !msg.position_covariance.empty();
const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
if (valid_position_covariance) {
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// Vertical position uncertainty
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
*
* Nonlinear equation:
* heading = atan2(vel_e_m_s, vel_n_m_s)
* For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
*
* To calculate the variance of heading from the variance of velocity,
* cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
*/
float vel_n = msg.ned_velocity[0];
float vel_e = msg.ned_velocity[1];
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
report.s_variance_m_s = -1.0F;
report.c_variance_rad = -1.0F;
}
report.fix_type = msg.status;
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s *
report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
report.timestamp_time_relative = 0;
report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
report.satellites_used = msg.sats_used;
// Using PDOP for HDOP and VDOP
// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
report.hdop = msg.pdop;
report.vdop = msg.pdop;
// Publish to a multi-topic
int32_t gps_orb_instance;
orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
ORB_PRIO_HIGH);
}
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
// This bridge does not support redundant GNSS receivers yet.
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
}
}
auto report = ::vehicle_gps_position_s();
report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
report.timestamp_variance = report.timestamp_position;
// Check if the msg contains valid covariance information
const bool valid_position_covariance = !msg.position_covariance.empty();
const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
if (valid_position_covariance) {
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// Vertical position uncertainty
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
*
* Nonlinear equation:
* heading = atan2(vel_e_m_s, vel_n_m_s)
* For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
*
* To calculate the variance of heading from the variance of velocity,
* cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
*/
float vel_n = msg.ned_velocity[0];
float vel_e = msg.ned_velocity[1];
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
report.s_variance_m_s = -1.0F;
report.c_variance_rad = -1.0F;
}
report.fix_type = msg.status;
report.timestamp_velocity = report.timestamp_position;
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
report.timestamp_time = report.timestamp_position;
report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
report.satellites_used = msg.sats_used;
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report);
}
}
virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
{
uptimes_[msg.getSrcNodeID().get()] = msg.uptime_sec;
}
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get());
if (state != nullptr) {
bool process = false;
if (msg.status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) {
state->status = AP_GPS::GPS_Status::NO_FIX;
} else {
if (msg.status == uavcan::equipment::gnss::Fix::STATUS_TIME_ONLY) {
state->status = AP_GPS::GPS_Status::NO_FIX;
} else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_2D_FIX) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_2D;
process = true;
} else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_3D_FIX) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D;
process = true;
}
if (msg.gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) {
uint64_t epoch_ms = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
epoch_ms /= 1000;
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
state->time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK);
state->time_week_ms = (uint32_t)(gps_ms - (state->time_week) * AP_MSEC_PER_WEEK);
}
}
if (process) {
Location loc = { };
loc.lat = msg.latitude_deg_1e8 / 10;
loc.lng = msg.longitude_deg_1e8 / 10;
loc.alt = msg.height_msl_mm / 10;
state->location = loc;
state->location.options = 0;
if (!uavcan::isNaN(msg.ned_velocity[0])) {
Vector3f vel(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]);
state->velocity = vel;
state->ground_speed = norm(vel.x, vel.y);
state->ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
state->have_vertical_velocity = true;
} else {
state->have_vertical_velocity = false;
}
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
if (!uavcan::isNaN(pos_cov[8])) {
if (pos_cov[8] > 0) {
state->vertical_accuracy = sqrtf(pos_cov[8]);
state->have_vertical_accuracy = true;
} else {
state->have_vertical_accuracy = false;
}
} else {
state->have_vertical_accuracy = false;
}
const float horizontal_pos_variance = MAX(pos_cov[0], pos_cov[4]);
if (!uavcan::isNaN(horizontal_pos_variance)) {
if (horizontal_pos_variance > 0) {
state->horizontal_accuracy = sqrtf(horizontal_pos_variance);
state->have_horizontal_accuracy = true;
} else {
state->have_horizontal_accuracy = false;
}
} else {
state->have_horizontal_accuracy = false;
}
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
if (!uavcan::isNaN(vel_cov[0])) {
state->speed_accuracy = sqrtf((vel_cov[0] + vel_cov[4] + vel_cov[8]) / 3.0);
state->have_speed_accuracy = true;
} else {
state->have_speed_accuracy = false;
}
state->num_sats = msg.sats_used;
} else {
state->have_vertical_velocity = false;
state->have_vertical_accuracy = false;
state->have_horizontal_accuracy = false;
state->have_speed_accuracy = false;
state->num_sats = 0;
}
state->last_gps_time_ms = AP_HAL::millis();
// after all is filled, update all listeners with new data
ap_uavcan->update_gps_state(msg.getSrcNodeID().get());
}
}
}
}
