diff options
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 72 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 17 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.h | 2 |
3 files changed, 33 insertions, 58 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4a75b00ab..1563a257b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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 */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 77ec344dc..7a56c36a5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 78279c08f..a6326bad1 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -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_ */ |