Пример #1
0
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);
	}
}
Пример #2
0
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());
            }
        }
    }
}
Пример #3
0
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());
            }
        }
    }
}
Пример #4
0
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;
	}
}
Пример #5
0
// 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;
            }
        }
    }
}
Пример #6
0
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;
                }
            }
        }
    }
}
Пример #7
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);
}
Пример #8
0
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);
}
Пример #9
0
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);
}
Пример #10
0
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);
	}

}
Пример #11
0
 virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
 {
     uptimes_[msg.getSrcNodeID().get()] = msg.uptime_sec;
 }
Пример #12
0
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());
            }
        }
    }
}