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:
Mark Whitehorn
2016-09-19 13:05:14 +02:00
committed by Beat Küng
parent 79f2cd79d9
commit f24b199775
5 changed files with 41 additions and 1 deletions
+2
View File
@@ -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
+1
View File
@@ -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");
+2
View File
@@ -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)
+1 -1
View File
@@ -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;
}