diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 56a6999147..c54c225d40 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -258,6 +258,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_GPS_RTCM_DATA: handle_message_gps_rtcm_data(msg); break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + handle_message_battery_status(msg); + break; default: break; @@ -1154,6 +1157,44 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) +{ + // external battery measurements + mavlink_battery_status_t battery_mavlink; + mavlink_msg_battery_status_decode(msg, &battery_mavlink); + + battery_status_s battery_status = {}; + battery_status.timestamp = hrt_absolute_time(); + + float voltage_sum = 0.0f; + uint8_t cell_count = 0; + for (unsigned i = 0; i < 10;i++) { + if (battery_mavlink.voltages[i] < UINT16_MAX) { + voltage_sum += (float)(battery_mavlink.voltages[i]) / 1000.0f; + } else { + cell_count = i; + break; + } + } + + battery_status.voltage_v = voltage_sum; + battery_status.voltage_filtered_v = voltage_sum; + battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 10000.0f; + battery_status.current_filtered_a = battery_status.current_a; + battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; + battery_status.discharged_mah = (float)battery_mavlink.current_consumed; + battery_status.cell_count = cell_count; + battery_status.connected = true; + + if (_battery_pub == nullptr) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status); + + } else { + orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status); + } +} + switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index bf0a5121ed..8d3b3d0396 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -142,6 +142,7 @@ private: void handle_message_follow_target(mavlink_message_t *msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); void handle_message_gps_rtcm_data(mavlink_message_t *msg); + void handle_message_battery_status(mavlink_message_t *msg); void *receive_thread(void *arg); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2de4b4216f..57313d2afb 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2023,6 +2023,22 @@ PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); */ PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); +/** + * Battery monitoring source. + * + * This parameter controls the source of battery data. The value 'Power Module' + * means that measurements are expected to come from a power module. If the value is set to + * 'External' then the system expects to receive mavlink battery status messages. + * + * @min 0 + * @max 1 + * @value 0 Power Module + * @value 1 External + * @group Battery Calibration + */ + +PARAM_DEFINE_INT32(BAT_SOURCE, 0); + /** * RC channel count diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7041d5a5a0..cea1686143 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -319,6 +319,7 @@ private: float battery_current_offset; float battery_v_div; float battery_a_per_v; + int32_t battery_source; float baro_qnh; @@ -381,6 +382,7 @@ private: param_t battery_current_offset; param_t battery_v_div; param_t battery_a_per_v; + param_t battery_source; param_t board_rotation; @@ -653,6 +655,7 @@ Sensors::Sensors() : _parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR"); _parameter_handles.battery_v_div = param_find("BAT_V_DIV"); _parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V"); + _parameter_handles.battery_source = param_find("BAT_SOURCE"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -948,6 +951,8 @@ Sensors::parameters_update() #endif } + param_get(_parameter_handles.battery_source, &(_parameters.battery_source)); + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); @@ -1718,7 +1723,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } - if (updated_battery) { + if (_parameters.battery_source == 0 && updated_battery) { actuator_controls_s ctrl; orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); _battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE],