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

mavlink: HIGHRES_IMU message added, default message streams added

This commit is contained in:
Anton Babushkin 2014-02-26 00:38:21 +04:00
parent e291af990f
commit 769a2af1f8
3 changed files with 33 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
src/modules/mavlink/mavlink_main.cpp
View File

@ -1526,62 +1526,6 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize waypoint manager */
mavlink_wpm_init(wpm);
/* all subscriptions are now active, set up initial guess about rate limits */
// if (_baudrate >= 230400) {
// /* 200 Hz / 5 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20);
// /* 50 Hz / 20 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30);
// /* 20 Hz / 50 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
// /* 10 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100);
// /* 10 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
//
// } else if (_baudrate >= 115200) {
// /* 20 Hz / 50 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
// /* 5 Hz / 200 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
// /* 5 Hz / 200 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200);
// /* 2 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
//
// } else if (_baudrate >= 57600) {
// /* 10 Hz / 100 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300);
// /* 10 Hz / 100 ms ATTITUDE */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200);
// /* 5 Hz / 200 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
// /* 5 Hz / 200 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
// /* 2 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
// /* 2 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500);
//
// } else {
// /* very low baud rate, limit to 1 Hz / 1000 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
// /* 1 Hz / 1000 ms */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
// /* 0.5 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
// /* 0.1 Hz */
// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
// }
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
@ -1595,8 +1539,22 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data;
/* add default streams, intervals depend on baud rate */
// if (_baudrate >= 230400) {
// } else if (_baudrate >= 115200) {
// } else if (_baudrate >= 57600) {
// }
add_stream("HEARTBEAT", 1000);
add_stream("SYS_STATUS", 100);
add_stream("SYS_STATUS", 1000);
add_stream("HIGHRES_IMU", 300);
add_stream("RAW_IMU", 300);
add_stream("ATTITUDE", 200);
add_stream("NAMED_VALUE_FLOAT", 200);
add_stream("SERVO_OUTPUT_RAW", 500);
add_stream("GPS_RAW_INT", 500);
add_stream("MANUAL_CONTROL", 500);
while (!_task_should_exit) {
/* main loop */

17
src/modules/mavlink/mavlink_messages.cpp
View File

@ -135,6 +135,23 @@ msg_sys_status(const MavlinkStream *stream)
status->errors_count4);
}
void
msg_highres_imu(const MavlinkStream *stream)
{
struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data;
uint16_t fields_updated = 0;
if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
}
//void
//MavlinkOrbListener::l_sensor_combined(const struct listener *l)
//{

2
src/modules/mavlink/mavlink_messages.h
View File

@ -23,6 +23,6 @@ extern struct msgs_list_s msgs_list[];
static void msg_heartbeat(const MavlinkStream *stream);
static void msg_sys_status(const MavlinkStream *stream);
static void msg_highres_imu(const MavlinkStream *stream);
#endif /* MAVLINK_MESSAGES_H_ */