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.

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

[WIP] Mavlink SYS_STATUS always grab latest cpuload and battery_status #11763

Closed
wants to merge 1 commit into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
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
6 changes: 6 additions & 0 deletions src/modules/mavlink/mavlink_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1652,6 +1652,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ALTITUDE", 1.0f);
configure_stream_local("ATTITUDE", 20.0f);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 1.0f);
Expand Down Expand Up @@ -1690,6 +1691,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ATTITUDE", 100.0f);
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
configure_stream_local("ATTITUDE_TARGET", 10.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_CAPTURE", 2.0f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
Expand Down Expand Up @@ -1730,6 +1732,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ALTITUDE", 10.0f); // for avoidance
configure_stream_local("ATTITUDE", 25.0f);
configure_stream_local("ATTITUDE_TARGET", 10.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_TRIGGER", unlimited_rate); // for VIO
configure_stream_local("DISTANCE_SENSOR", 10.0f); // for avoidance
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
Expand All @@ -1751,6 +1754,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ALTITUDE", 1.0f);
configure_stream_local("ATTITUDE", 25.0f);
configure_stream_local("ATTITUDE_TARGET", 10.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
Expand Down Expand Up @@ -1779,6 +1783,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ATTITUDE", 50.0f);
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
configure_stream_local("ATTITUDE_TARGET", 8.0f);
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
Expand Down Expand Up @@ -1820,6 +1825,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
case MAVLINK_MODE_MINIMAL:
configure_stream_local("ALTITUDE", 0.5f);
configure_stream_local("ATTITUDE", 10.0f);
configure_stream_local("BATTERY_STATUS", 0.1f);
configure_stream_local("EXTENDED_SYS_STATE", 0.1f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
configure_stream_local("GPS_RAW_INT", 0.5f);
Expand Down
93 changes: 76 additions & 17 deletions src/modules/mavlink/mavlink_messages.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -544,8 +544,6 @@ class MavlinkStreamSysStatus : public MavlinkStream
MavlinkOrbSubscription *_battery_status_sub;

uint64_t _status_timestamp{0};
uint64_t _cpuload_timestamp{0};
uint64_t _battery_status_timestamp{0};

/* do not allow top copying this class */
MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete;
Expand All @@ -560,15 +558,22 @@ class MavlinkStreamSysStatus : public MavlinkStream

bool send(const hrt_abstime t)
{
vehicle_status_s status = {};
cpuload_s cpuload = {};
battery_status_s battery_status = {};

vehicle_status_s status{};
const bool updated_status = _status_sub->update(&_status_timestamp, &status);
const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload);
const bool updated_battery = _battery_status_sub->update(&_battery_status_timestamp, &battery_status);

if (updated_status || updated_battery || updated_cpuload) {
if (updated_status) {

// grab latest battery_status that's no more than 10 seconds older than vehicle_status
uint64_t battery_oldest = _status_timestamp - 10_s;
battery_status_s battery_status{};
_battery_status_sub->update(&battery_oldest, &battery_status);

// grab latest cpuload that's no more than 10 seconds older than vehicle_status
uint64_t cpuload_oldest = _status_timestamp - 10_s;
cpuload_s cpuload{};
_cpuload_sub->update(&cpuload_oldest, &cpuload);

// fill Mavlink SYS_STATUS message
mavlink_sys_status_t msg = {};

msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
Expand All @@ -578,16 +583,70 @@ class MavlinkStreamSysStatus : public MavlinkStream
msg.voltage_battery = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.0f : UINT16_MAX;
msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100.0f : -1;
msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1;
// TODO: fill in something useful in the fields below
msg.drop_rate_comm = 0;
msg.errors_comm = 0;
msg.errors_count1 = 0;
msg.errors_count2 = 0;
msg.errors_count3 = 0;
msg.errors_count4 = 0;

mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg);

return true;
}

return false;
}
};

class MavlinkStreamBatteryStatus : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamBatteryStatus::get_name_static();
}

static const char *get_name_static()
{
return "BATTERY_STATUS";
}

static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_BATTERY_STATUS;
}

uint16_t get_id()
{
return get_id_static();
}

static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamBatteryStatus(mavlink);
}

unsigned get_size()
{
return MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}

private:
MavlinkOrbSubscription *_battery_status_sub;

uint64_t _battery_status_timestamp{0};

/* do not allow top copying this class */
MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete;
MavlinkStreamBatteryStatus &operator = (const MavlinkStreamSysStatus &) = delete;

protected:
explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
_battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status)))
{}

bool send(const hrt_abstime t)
{
battery_status_s battery_status{};
const bool updated_battery = _battery_status_sub->update(&_battery_status_timestamp, &battery_status);

if (updated_battery) {

/* battery status message with higher resolution */
mavlink_battery_status_t bat_msg = {};
bat_msg.id = 0;
Expand Down Expand Up @@ -623,7 +682,6 @@ class MavlinkStreamSysStatus : public MavlinkStream
}
};


class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
Expand Down Expand Up @@ -4826,6 +4884,7 @@ static const StreamListItem streams_list[] = {
StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static),
StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static),
StreamListItem(&MavlinkStreamBatteryStatus::new_instance, &MavlinkStreamBatteryStatus::get_name_static, &MavlinkStreamBatteryStatus::get_id_static),
StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static),
StreamListItem(&MavlinkStreamScaledIMU::new_instance, &MavlinkStreamScaledIMU::get_name_static, &MavlinkStreamScaledIMU::get_id_static),
StreamListItem(&MavlinkStreamScaledIMU2::new_instance, &MavlinkStreamScaledIMU2::get_name_static, &MavlinkStreamScaledIMU2::get_id_static),
Expand Down