fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 16:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged estimator fixes and mavlink rate config bits

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-30 00:46:29 +02:00
parent 6f75d1a20f afb5271bfb
commit 28a31708f9
20 changed files with 3709 additions and 770 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+24 -6
View File
@@ -197,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (buf_free < desired) {
/* we don't want to send anything just in half, so return */
instance->count_txerr();
return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
// XXX overflow perf
instance->count_txerr();
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
@@ -249,7 +250,8 @@ Mavlink::Mavlink() :
_param_use_hil_gps(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
_wpm = &_wpm_s;
mission.count = 0;
@@ -302,6 +304,7 @@ Mavlink::Mavlink() :
Mavlink::~Mavlink()
{
perf_free(_loop_perf);
perf_free(_txerr_perf);
if (_task_running) {
/* task wakes up every 10ms or so at the longest */
@@ -326,6 +329,12 @@ Mavlink::~Mavlink()
LL_DELETE(_mavlink_instances, this);
}
void
Mavlink::count_txerr()
{
perf_count(_txerr_perf);
}
void
Mavlink::set_mode(enum MAVLINK_MODE mode)
{
@@ -2193,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[])
/* create the instance in task context */
Mavlink *instance = new Mavlink();
/* this will actually only return once MAVLink exits */
int res = instance->task_main(argc, argv);
int res;
/* delete instance on main thread end */
delete instance;
if (!instance) {
/* out of memory */
res = -ENOMEM;
warnx("OUT OF MEM");
} else {
/* this will actually only return once MAVLink exits */
res = instance->task_main(argc, argv);
/* delete instance on main thread end */
delete instance;
}
return res;
}
src/modules/mavlink/mavlink_main.h
+10 -1
View File
@@ -215,10 +215,14 @@ public:
const mavlink_channel_t get_channel();
void configure_stream_threadsafe(const char *stream_name, const float rate);
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
MavlinkStream * get_streams() { return _streams; } const
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
@@ -232,6 +236,11 @@ public:
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
/**
* Count a transmision error
*/
void count_txerr();
protected:
Mavlink *next;
@@ -303,6 +312,7 @@ private:
pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
bool _param_initialized;
param_t _param_system_id;
@@ -371,7 +381,6 @@ private:
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
int configure_stream(const char *stream_name, const float rate);
void configure_stream_threadsafe(const char *stream_name, const float rate);
int message_buffer_init(int size);
src/modules/mavlink/mavlink_messages.cpp
+120
View File
@@ -232,6 +232,11 @@ public:
return "HEARTBEAT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_HEARTBEAT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamHeartbeat();
@@ -292,6 +297,11 @@ public:
return "SYS_STATUS";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_SYS_STATUS;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamSysStatus();
@@ -343,6 +353,11 @@ public:
return "HIGHRES_IMU";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_HIGHRES_IMU;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamHighresIMU();
@@ -428,6 +443,11 @@ public:
return "ATTITUDE";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ATTITUDE;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitude();
@@ -474,6 +494,11 @@ public:
return "ATTITUDE_QUATERNION";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeQuaternion();
@@ -526,6 +551,11 @@ public:
return "VFR_HUD";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_VFR_HUD;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamVFRHUD();
@@ -609,6 +639,11 @@ public:
return "GPS_RAW_INT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GPS_RAW_INT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamGPSRawInt();
@@ -662,6 +697,11 @@ public:
return "GLOBAL_POSITION_INT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionInt();
@@ -723,6 +763,11 @@ public:
return "LOCAL_POSITION_NED";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionNED();
@@ -774,6 +819,11 @@ public:
return "VICON_POSITION_ESTIMATE";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamViconPositionEstimate();
@@ -824,6 +874,11 @@ public:
return "GPS_GLOBAL_ORIGIN";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamGPSGlobalOrigin();
@@ -864,6 +919,11 @@ public:
return MavlinkStreamServoOutputRaw<N>::get_name_static();
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
}
static const char *get_name_static()
{
switch (N) {
@@ -941,6 +1001,11 @@ public:
return "HIL_CONTROLS";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_HIL_CONTROLS;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamHILControls();
@@ -1078,6 +1143,11 @@ public:
return "GLOBAL_POSITION_SETPOINT_INT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionSetpointInt();
@@ -1121,6 +1191,11 @@ public:
return "LOCAL_POSITION_SETPOINT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionSetpoint();
@@ -1169,6 +1244,11 @@ public:
return "ROLL_PITCH_YAW_THRUST_SETPOINT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamRollPitchYawThrustSetpoint();
@@ -1217,6 +1297,11 @@ public:
return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
@@ -1265,6 +1350,11 @@ public:
return "RC_CHANNELS_RAW";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamRCChannelsRaw();
@@ -1349,6 +1439,11 @@ public:
return "MANUAL_CONTROL";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_MANUAL_CONTROL;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamManualControl();
@@ -1398,6 +1493,11 @@ public:
return "OPTICAL_FLOW";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_OPTICAL_FLOW;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamOpticalFlow();
@@ -1446,6 +1546,11 @@ public:
return "ATTITUDE_CONTROLS";
}
uint8_t get_id()
{
return 0;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeControls();
@@ -1504,6 +1609,11 @@ public:
return "NAMED_VALUE_FLOAT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamNamedValueFloat();
@@ -1552,6 +1662,11 @@ public:
return "CAMERA_CAPTURE";
}
uint8_t get_id()
{
return 0;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamCameraCapture();
@@ -1597,6 +1712,11 @@ public:
return "DISTANCE_SENSOR";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_DISTANCE_SENSOR;
}
static MavlinkStream *new_instance()
{
return new MavlinkStreamDistanceSensor();
src/modules/mavlink/mavlink_receiver.cpp
+22
View File
@@ -159,6 +159,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_heartbeat(msg);
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
@@ -498,6 +502,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
{
mavlink_request_data_stream_t req;
mavlink_msg_request_data_stream_decode(msg, &req);
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
MavlinkStream *stream;
LL_FOREACH(_mavlink->get_streams(), stream) {
if (req.req_stream_id == stream->get_id()) {
_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
}
}
}
}
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -115,6 +115,7 @@ private:
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_heartbeat(mavlink_message_t *msg);
void handle_message_request_data_stream(mavlink_message_t *msg);
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
src/modules/mavlink/mavlink_stream.h
+1
View File
@@ -67,6 +67,7 @@ public:
static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
protected:
mavlink_channel_t _channel;