Skip to content

Commit

Permalink
mavlink split BATTERY_STATUS from SYS_STATUS update
Browse files Browse the repository at this point in the history 
 - fix SYS_STATUS usage of battery_status and cpuload
  • Loading branch information
@dagar
dagar committed Aug 3, 2018
1 parent c415477 commit e91d522
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 15 deletions.
14 changes: 9 additions & 5 deletions src/modules/mavlink/mavlink_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1778,6 +1778,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", 1.0f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 1.0f);
Expand All @@ -1794,8 +1795,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
configure_stream_local("PING", 0.1f);
configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f);
configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
configure_stream_local("RC_CHANNELS", 5.0f);
configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream_local("SYS_STATUS", 1.0f);
Expand All @@ -1812,6 +1813,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", 5.0f);
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 @@ -1848,6 +1850,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", 5.0f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
Expand All @@ -1874,18 +1877,19 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
configure_stream_local("ALTITUDE", 10.0f);
configure_stream_local("ATTITUDE", 50.0f);
configure_stream_local("ATTITUDE_TARGET", 8.0f);
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
configure_stream_local("ATTITUDE_TARGET", 8.0f);
configure_stream_local("BATTERY_STATUS", 5.0f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 50.0f);
configure_stream_local("DEBUG_VECT", 50.0f);
configure_stream_local("DISTANCE_SENSOR", 10.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("HIGHRES_IMU", 50.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("LOCAL_POSITION_NED", 30.0f);
Expand Down Expand Up @@ -1914,8 +1918,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ALTITUDE", 0.5f);
configure_stream_local("ATTITUDE", 10.0f);
configure_stream_local("EXTENDED_SYS_STATE", 0.1f);
configure_stream_local("GPS_RAW_INT", 0.5f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
configure_stream_local("GPS_RAW_INT", 0.5f);
configure_stream_local("HOME_POSITION", 0.1f);
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
configure_stream_local("RC_CHANNELS", 0.5f);
Expand Down
84 changes: 74 additions & 10 deletions src/modules/mavlink/mavlink_messages.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -534,8 +534,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 @@ -550,15 +548,19 @@ 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) {

// if vehicle_status updated always grab latest cpuload and battery_status

battery_status_s battery_status = {};
_battery_status_sub->update(&battery_status);

cpuload_s cpuload = {};
_cpuload_sub->update(&cpuload);

mavlink_sys_status_t msg = {};

msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
Expand All @@ -568,6 +570,7 @@ 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;
Expand All @@ -578,6 +581,67 @@ class MavlinkStreamSysStatus : public MavlinkStream

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 @@ -613,7 +677,6 @@ class MavlinkStreamSysStatus : public MavlinkStream
}
};


class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
Expand Down Expand Up @@ -4440,6 +4503,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

0 comments on commit e91d522

Please sign in to comment.