mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 20:43:34 +08:00
add HIL_STATE message handler and publish to uORB
add missing break uorb topics generator: add multi-topics to the list of all topics topic names with '# TOPICS <name>' were previously not in orb_get_topics(). This means the logger could not find them. Affects for example actuator_controls_0. px_generate_uorb_topic_files.py: add multitopics to generate_topics_list_file_from_files method switch simulated attitude to new topic: vehicle_attitude_groundtruth logger: add input_rc topic. needed for web plotting input_rc.msg: remove timestamp_publication, use timestamp instead mixer.cpp: warnx -> PX4_ERR logger: initialize timer_call to 0 (hrt_call_every reads some fields) position_setpoint_triplet topic: set the timestamp when publishing px_generate_uorb_topic_files.py: add multitopics to generate_topics_list_file_from_files method add vehicle_attitude_groundtruth to default topics change to hil_state_quaternion
This commit is contained in:
committed by
Beat Küng
parent
79f2cd79d9
commit
f24b199775
@@ -18,3 +18,5 @@ float32[4] q # Quaternion (NED)
|
||||
float32[3] g_comp # Compensated gravity vector
|
||||
bool R_valid # Rotation matrix valid
|
||||
bool q_valid # Quaternion valid
|
||||
|
||||
# TOPICS vehicle_attitude vehicle_attitude_groundtruth
|
||||
|
||||
@@ -476,6 +476,7 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance,
|
||||
void Logger::add_default_topics()
|
||||
{
|
||||
add_topic("vehicle_attitude", 10);
|
||||
add_topic("vehicle_attitude_groundtruth", 10);
|
||||
add_topic("actuator_outputs", 50);
|
||||
add_topic("telemetry_status", 50);
|
||||
add_topic("vehicle_command");
|
||||
|
||||
@@ -245,6 +245,7 @@ private:
|
||||
#ifndef __PX4_QURT
|
||||
,
|
||||
_rc_channels_pub(nullptr),
|
||||
_attitude_pub(nullptr),
|
||||
_actuator_outputs_sub{},
|
||||
_vehicle_attitude_sub(-1),
|
||||
_manual_sub(-1),
|
||||
@@ -306,6 +307,7 @@ private:
|
||||
#ifndef __PX4_QURT
|
||||
// uORB publisher handlers
|
||||
orb_advert_t _rc_channels_pub;
|
||||
orb_advert_t _attitude_pub;
|
||||
|
||||
// uORB subscription handlers
|
||||
int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES];
|
||||
|
||||
@@ -41,6 +41,8 @@
|
||||
#include <netinet/in.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
extern "C" __EXPORT hrt_abstime hrt_reset(void);
|
||||
|
||||
#define SEND_INTERVAL 20
|
||||
@@ -301,7 +303,40 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
|
||||
mavlink_hil_state_quaternion_t hil_state;
|
||||
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
/* attitude */
|
||||
struct vehicle_attitude_s hil_attitude;
|
||||
{
|
||||
hil_attitude.timestamp = timestamp;
|
||||
|
||||
math::Quaternion q(hil_state.attitude_quaternion);
|
||||
math::Matrix<3, 3> C_nb = q.to_dcm();
|
||||
math::Vector<3> euler = C_nb.to_euler();
|
||||
|
||||
hil_attitude.q[0] = q(0);
|
||||
hil_attitude.q[1] = q(1);
|
||||
hil_attitude.q[2] = q(2);
|
||||
hil_attitude.q[3] = q(3);
|
||||
hil_attitude.q_valid = true;
|
||||
|
||||
hil_attitude.roll = euler(1);
|
||||
hil_attitude.pitch = euler(0);
|
||||
hil_attitude.yaw = euler(2);
|
||||
|
||||
if (true) {
|
||||
int hilstate_multi;
|
||||
orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
|
||||
|
||||
@@ -91,7 +91,7 @@ mixer_main(int argc, char *argv[])
|
||||
int ret = load(argv[2], argv[3], true);
|
||||
|
||||
if (ret != 0) {
|
||||
warnx("failed to append mixer");
|
||||
PX4_ERR("failed to append mixer");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user