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

Merge branch 'beta_mavlink2' into mavlink2_hil

Browse Source
This commit is contained in:
Anton Babushkin
2014-03-16 11:08:54 +04:00
parent 5f847ff8a3 955bd402d2
commit 82389e9702
24 changed files with 3020 additions and 774 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gpio_led/gpio_led.c
+64 -41
View File
@@ -51,7 +51,6 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -63,8 +62,6 @@ struct gpio_led_s {
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
struct actuator_armed_s armed;
int actuator_armed_sub;
bool led_state;
int counter;
};
@@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[])
{
if (argc < 2) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
"\t-p\tUse pin:\n"
"\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
@@ -88,7 +86,14 @@ int gpio_led_main(int argc, char *argv[])
"\t\ta1\tPX4IO ACC1\n"
"\t\ta2\tPX4IO ACC2\n"
"\t\tr1\tPX4IO RELAY1\n"
"\t\tr2\tPX4IO RELAY2");
"\t\tr2\tPX4IO RELAY2"
);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
);
#endif
} else {
@@ -98,37 +103,70 @@ int gpio_led_main(int argc, char *argv[])
}
bool use_io = false;
int pin = GPIO_EXT_1;
/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
int pin = 1;
/* pin name to display */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
char *pin_name = "PX4FMU GPIO_EXT1";
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
char pin_name[] = "AUX OUT 1";
#endif
if (argc > 2) {
if (!strcmp(argv[2], "-p")) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (!strcmp(argv[3], "1")) {
use_io = false;
pin = GPIO_EXT_1;
pin_name = "PX4FMU GPIO_EXT1";
} else if (!strcmp(argv[3], "2")) {
use_io = false;
pin = GPIO_EXT_2;
pin_name = "PX4FMU GPIO_EXT2";
} else if (!strcmp(argv[3], "a1")) {
use_io = true;
pin = PX4IO_P_SETUP_RELAYS_ACC1;
pin_name = "PX4IO ACC1";
} else if (!strcmp(argv[3], "a2")) {
use_io = true;
pin = PX4IO_P_SETUP_RELAYS_ACC2;
pin_name = "PX4IO ACC2";
} else if (!strcmp(argv[3], "r1")) {
use_io = true;
pin = PX4IO_P_SETUP_RELAYS_POWER1;
pin_name = "PX4IO RELAY1";
} else if (!strcmp(argv[3], "r2")) {
use_io = true;
pin = PX4IO_P_SETUP_RELAYS_POWER2;
pin_name = "PX4IO RELAY2";
} else {
errx(1, "unsupported pin: %s", argv[3]);
}
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
unsigned int n = strtoul(argv[3], NULL, 10);
if (n >= 1 && n <= 6) {
use_io = false;
pin = 1 << (n - 1);
snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
} else {
errx(1, "unsupported pin: %s", argv[3]);
}
#endif
}
}
@@ -142,21 +180,6 @@ int gpio_led_main(int argc, char *argv[])
} else {
gpio_led_started = true;
char pin_name[24];
if (use_io) {
if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
sprintf(pin_name, "PX4IO RELAY%i", pin);
}
} else {
sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
}
warnx("start, using pin: %s", pin_name);
}
@@ -186,6 +209,7 @@ void gpio_led_start(FAR void *arg)
if (priv->use_io) {
gpio_dev = PX4IO_DEVICE_PATH;
} else {
gpio_dev = PX4FMU_DEVICE_PATH;
}
@@ -204,8 +228,10 @@ void gpio_led_start(FAR void *arg)
/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* subscribe to vehicle status topic */
/* initialize vehicle status structure */
memset(&priv->status, 0, sizeof(priv->status));
/* subscribe to vehicle status topic */
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* add worker to queue */
@@ -224,38 +250,33 @@ void gpio_led_cycle(FAR void *arg)
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* check for status updates*/
bool status_updated;
orb_check(priv->vehicle_status_sub, &status_updated);
bool updated;
orb_check(priv->vehicle_status_sub, &updated);
if (status_updated)
if (updated) {
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
}
/* select pattern for current status */
int pattern = 0;
if (priv->armed.armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
pattern = 0x2A; // *_*_*_ fast blink (armed, error)
} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
pattern = 0x3f; // ****** solid (armed)
} else {
pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning)
pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe)
}
} else {
if (priv->armed.ready_to_arm) {
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
pattern = 0x28; // *_*___ slow double blink (disarmed, error)
} else {
pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm)
}
}
/* blink pattern */
@@ -266,6 +287,7 @@ void gpio_led_cycle(FAR void *arg)
if (led_state_new) {
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
} else {
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
@@ -273,8 +295,9 @@ void gpio_led_cycle(FAR void *arg)
priv->counter++;
if (priv->counter > 5)
if (priv->counter > 5) {
priv->counter = 0;
}
/* repeat cycle at 5 Hz */
if (gpio_led_started) {
src/modules/mavlink/mavlink_main.cpp
+107 -49
View File
@@ -104,55 +104,90 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
static uint64_t last_write_times[6] = {0};
/*
* Internal function to send the bytes through the right serial port
*/
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
int uart = -1;
Mavlink *instance;
switch (channel) {
case MAVLINK_COMM_0:
uart = Mavlink::get_uart_fd(0);
instance = Mavlink::get_instance(0);
break;
case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1);
instance = Mavlink::get_instance(1);
break;
case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2);
instance = Mavlink::get_instance(2);
break;
case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3);
instance = Mavlink::get_instance(3);
break;
#ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4);
instance = Mavlink::get_instance(4);
break;
#endif
#ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5);
instance = Mavlink::get_instance(5);
break;
#endif
#ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(6);
instance = Mavlink::get_instance(6);
break;
#endif
}
/* no valid instance, bail */
if (!instance)
return;
int uart = instance->get_uart_fd();
ssize_t desired = (sizeof(uint8_t) * length);
/*
* Check if the OS buffer is full and disable HW
* flow control if it continues to be full
*/
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
} else {
/* apparently there is space left, although we might be
* partially overflooding the buffer already */
last_write_times[(unsigned)channel] = hrt_absolute_time();
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
warn("write err");
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
}
}
@@ -173,11 +208,13 @@ Mavlink::Mavlink() :
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
_wpm = &_wpm_s;
mission.count = 0;
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
_instance_id = Mavlink::instance_count();
@@ -511,7 +548,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
close(_uart_fd);
return -1;
}
@@ -527,7 +564,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
close(_uart_fd);
return -1;
}
@@ -535,14 +572,46 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
}
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
warnx("ERR SET CONF %s\n", uart_name);
close(_uart_fd);
return -1;
}
/*
* Setup hardware flow control. If the port has no RTS pin this call will fail,
* which is not an issue, but requires a separate call so we can fail silently.
*/
(void)tcgetattr(_uart_fd, &uart_config);
uart_config.c_cflag |= CRTS_IFLOW;
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
/* setup output flow control */
if (enable_flow_control(true)) {
warnx("ERR FLOW CTRL EN");
}
return _uart_fd;
}
int
Mavlink::enable_flow_control(bool enabled)
{
struct termios uart_config;
int ret = tcgetattr(_uart_fd, &uart_config);
if (enabled) {
uart_config.c_cflag |= CRTSCTS;
} else {
uart_config.c_cflag &= ~CRTSCTS;
}
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!ret) {
_flow_control_enabled = enabled;
}
return ret;
}
int
Mavlink::set_hil_enabled(bool hil_enabled)
{
@@ -639,7 +708,7 @@ int Mavlink::mavlink_pm_send_param(param_t param)
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
MAVLINK_COMM_0,
_channel,
&tx_msg,
name_buf,
val_buf,
@@ -834,7 +903,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); }
@@ -857,7 +926,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
wpc.seq = seq;
mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
} else if (seq == 0 && _wpm->size == 0) {
@@ -880,7 +949,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
wpc.target_component = compid;
wpc.count = mission.count;
mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); }
@@ -911,7 +980,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); }
@@ -931,7 +1000,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
@@ -957,7 +1026,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); }
@@ -1514,7 +1583,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
/* copy stream name */
unsigned n = strlen(stream_name) + 1;
char *s = new char[n];
memcpy(s, stream_name, n);
strcpy(s, stream_name);
/* set subscription task */
_subscribe_to_stream_rate = rate;
@@ -1530,10 +1599,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
int
Mavlink::task_main(int argc, char *argv[])
{
/* inform about start */
warnx("start");
fflush(stdout);
int ch;
_baudrate = 57600;
_datarate = 0;
@@ -1630,8 +1695,7 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
warnx("data rate: %d bytes/s", _datarate);
warnx("port: %s, baudrate: %d", _device_name, _baudrate);
warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
@@ -1676,9 +1740,6 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
warnx("started");
mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
@@ -1705,7 +1766,7 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(100000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
@@ -1884,36 +1945,33 @@ Mavlink::stream(int argc, char *argv[])
const char *stream_name = nullptr;
int ch;
argc -= 1;
argv += 1;
argc -= 2;
argv += 2;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "r:d:s:")) != EOF) {
switch (ch) {
case 'r':
rate = strtod(optarg, nullptr);
int i = 0;
while (i < argc) {
if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) {
rate = strtod(argv[i+1], nullptr);
if (rate < 0.0f) {
err_flag = true;
}
break;
case 'd':
device_name = optarg;
break;
case 's':
stream_name = optarg;
break;
default:
i++;
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) {
device_name = argv[i+1];
i++;
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) {
stream_name = argv[i+1];
i++;
} else {
err_flag = true;
break;
}
i++;
}
if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
src/modules/mavlink/mavlink_main.h
+11
View File
@@ -154,6 +154,8 @@ public:
bool get_hil_enabled() { return _hil_enabled; };
bool get_flow_control_enabled() { return _flow_control_enabled; }
/**
* Handle waypoint related messages.
*/
@@ -182,6 +184,13 @@ public:
int get_instance_id();
/**
* Enable / disable hardware flow control.
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
mavlink_channel_t get_channel();
bool _task_should_exit; /**< if true, mavlink task should exit */
@@ -239,6 +248,8 @@ private:
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
/**
* Send one parameter.
*
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -912,7 +912,7 @@ MavlinkReceiver::receive_thread(void *arg)
_mavlink->mavlink_wpm_message_handler(&msg);
/* handle packet with parameter component */
_mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
}
}
}
src/modules/px4iofirmware/mixer.cpp
+1 -1
View File
@@ -267,7 +267,7 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
if (control_group > 3)
if (control_group >= PX4IO_CONTROL_GROUPS)
return -1;
switch (source) {
src/modules/px4iofirmware/px4io.h
+1 -1
View File
@@ -53,7 +53,7 @@
*/
#define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_CONTROL_GROUPS 2
#define PX4IO_CONTROL_GROUPS 4
#define PX4IO_RC_INPUT_CHANNELS 18
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
src/modules/sdlog2/logbuffer.c
+2 -1
View File
@@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
// bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1;
if (available < 0)
if (available < 0) {
available += lb->size;
}
if (size > available) {
// buffer overflow
src/modules/sdlog2/sdlog2.c
+425 -560
View File
File diff suppressed because it is too large Load Diff
src/modules/sdlog2/sdlog2_messages.h
+7 -7
View File
@@ -267,13 +267,13 @@ struct log_DIST_s {
/* --- TELE - TELEMETRY STATUS --- */
#define LOG_TELE_MSG 22
struct log_TELE_s {
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/