dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Handle battery status messages 

Handle simulator mavlink messages
Fix
This commit is contained in:
Jaeyoung-Lim 2021-09-27 23:52:19 +02:00
parent faca2b17d0
commit ba8eb47451
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/simulator/simulator.h
View File

@ -58,6 +58,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/irlock_report.h>
@ -228,6 +229,7 @@ private:
void handle_message_optical_flow(const mavlink_message_t *msg);
void handle_message_rc_channels(const mavlink_message_t *msg);
void handle_message_vision_position_estimate(const mavlink_message_t *msg);
void handle_message_battery_status(const mavlink_message_t *msg);
void parameters_update(bool force);
void poll_for_MAVLink_messages();
@ -249,6 +251,7 @@ private:
uORB::Publication<vehicle_global_position_s> _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::Publication<vehicle_local_position_s> _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
uORB::Publication<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
// HIL GPS
static constexpr int MAX_GPS = 3;

19
src/modules/simulator/simulator_mavlink.cpp
View File

@ -368,6 +368,11 @@ void Simulator::handle_message(const mavlink_message_t *msg)
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
handle_message_hil_state_quaternion(msg);
break;
case MAVLINK_MSG_ID_BATTERY_STATUS:
handle_message_battery_status(msg);
break;
}
}
@ -672,6 +677,20 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
}
}
void Simulator::handle_message_battery_status(const mavlink_message_t *msg)
{
mavlink_battery_status_t battery_status_mavlink;
mavlink_msg_battery_status_decode(msg, &battery_status_mavlink);
battery_status_s battery_status{};
battery_status.voltage_v = 14.8f;
battery_status.voltage_filtered_v = 14.8f;
battery_status.current_a = 15.0f;
battery_status.discharged_mah = -1.0f;
battery_status.timestamp = hrt_absolute_time();
_battery_pub.publish(battery_status);
}
void *Simulator::sending_trampoline(void * /*unused*/)
{
_instance->send();