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

Merged beta into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-01 16:58:02 +02:00
parent 848dd2155a 09f6b88651
commit ea7ae7d019
87 changed files with 1386 additions and 545 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+10 -4
View File
@@ -120,6 +120,8 @@ extern mavlink_system_t mavlink_system;
static void usage(void);
bool Mavlink::_boot_complete = false;
Mavlink::Mavlink() :
#ifndef __PX4_NUTTX
VDev("mavlink-log", MAVLINK_LOG_DEVICE),
@@ -1639,15 +1641,15 @@ Mavlink::task_main(int argc, char *argv[])
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f);
configure_stream("ATTITUDE", 10.0f);
configure_stream("HIGHRES_IMU", 2.0f);
configure_stream("ATTITUDE", 20.0f);
configure_stream("VFR_HUD", 8.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS", 1.0f);
configure_stream("RC_CHANNELS", 4.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("ATTITUDE_TARGET", 8.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW_RAD", 5.0f);
break;
@@ -2163,6 +2165,10 @@ int mavlink_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream_command(argc, argv);
} else if (!strcmp(argv[1], "boot_complete")) {
Mavlink::set_boot_complete();
return 0;
} else {
usage();
return 1;
src/modules/mavlink/mavlink_main.h
+11
View File
@@ -163,6 +163,14 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
/**
* Set the boot complete flag on all instances
*
* Setting the flag unblocks parameter transmissions, which are gated
* beforehand to ensure that the system is fully initialized.
*/
static void set_boot_complete() { _boot_complete = true; }
/**
* Get the free space in the transmit buffer
*
@@ -325,6 +333,8 @@ public:
int get_socket_fd () { return _socket_fd; };
static bool boot_complete() { return _boot_complete; }
protected:
Mavlink *next;
@@ -333,6 +343,7 @@ private:
int _mavlink_fd;
bool _task_running;
static bool _boot_complete;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
src/modules/mavlink/mavlink_parameters.cpp
+1 -1
View File
@@ -193,7 +193,7 @@ void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested, but only after the system has booted */
if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) {
src/modules/mavlink/mavlink_receiver.cpp
+53 -15
View File
@@ -35,9 +35,9 @@
* @file mavlink_receiver.cpp
* MAVLink protocol message receive and dispatch
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Thomas Gubler <thomas@px4.io>
*/
/* XXX trim includes */
@@ -136,7 +136,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_rates_sp{},
_time_offset_avg_alpha(0.6),
_time_offset(0),
_orb_class_instance(-1)
_orb_class_instance(-1),
_mom_switch_pos{},
_mom_switch_state(0)
{
}
@@ -970,15 +972,45 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
}
}
static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) {
switch_pos_t
MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
{
// XXX non-standard 3 pos switch decoding
return (buttons >> (sw * 2)) & 3;
}
static int decode_switch_pos_n(uint16_t buttons, int sw) {
if (buttons & (1 << sw)) {
return 1;
int
MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
{
bool on = (buttons & (1 << sw));
if (sw < MOM_SWITCH_COUNT) {
bool last_on = (_mom_switch_state & (1 << sw));
/* first switch is 2-pos, rest is 2 pos */
unsigned state_count = (sw == 0) ? 3 : 2;
/* only transition on low state */
if (!on && (on != last_on)) {
_mom_switch_pos[sw]++;
if (_mom_switch_pos[sw] == state_count) {
_mom_switch_pos[sw] = 0;
}
}
/* state_count - 1 is the number of intervals and 1000 is the range,
* with 2 states 0 becomes 0, 1 becomes 1000. With
* 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000,
* and so on for more states.
*/
return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000;
} else {
return 0;
/* return the current state */
return on * 1000 + 1000;
}
}
@@ -1075,12 +1107,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.values[3] = 1000;
}
rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000;
rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000;
rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000;
rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000;
/* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8);
unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
if (max_switch > (max_channels - 4)) {
max_switch = (max_channels - 4);
}
/* fill all channels */
for (unsigned i = 0; i < max_switch; i++) {
rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
}
_mom_switch_state = man.buttons;
if (_rc_pub == nullptr) {
_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
src/modules/mavlink/mavlink_receiver.h
+24 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,8 +35,8 @@
* @file mavlink_orb_listener.h
* MAVLink 1.0 uORB listener definition
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#pragma once
@@ -141,15 +141,26 @@ private:
void *receive_thread(void *arg);
/**
* Convert remote timestamp to local hrt time (usec)
* Use timesync if available, monotonic boot time otherwise
*/
* Convert remote timestamp to local hrt time (usec)
* Use timesync if available, monotonic boot time otherwise
*/
uint64_t sync_stamp(uint64_t usec);
/**
* Exponential moving average filter to smooth time offset
*/
* Exponential moving average filter to smooth time offset
*/
void smooth_time_offset(uint64_t offset_ns);
/**
* Decode a switch position from a bitfield
*/
switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw);
/**
* Decode a switch position from a bitfield and state
*/
int decode_switch_pos_n(uint16_t buttons, unsigned sw);
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
struct vehicle_land_detected_s hil_land_detector;
@@ -195,6 +206,11 @@ private:
uint64_t _time_offset;
int _orb_class_instance;
static constexpr unsigned MOM_SWITCH_COUNT = 8;
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT];
uint16_t _mom_switch_state;
/* do not allow copying this class */
MavlinkReceiver(const MavlinkReceiver &);
MavlinkReceiver operator=(const MavlinkReceiver &);