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

Merge branch 'beta' into stable

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-01 17:16:30 +02:00
parent 57ac8c19ff a446cb6b2f
commit 40a7c5a55d
5 changed files with 106 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+10 -10
View File
@@ -83,7 +83,7 @@
/**** Warning macros, disable to save memory */
#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);}
#define UBX_DEBUG(s, ...) {/*warnx(s, ## __VA_ARGS__);*/}
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
_fd(fd),
@@ -212,7 +212,7 @@ UBX::configure(unsigned &baudrate)
} else {
_use_nav_pvt = true;
}
UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
if (!_use_nav_pvt) {
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
@@ -271,9 +271,9 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
ret = 0; // ACK received ok
} else if (report) {
if (_ack_state == UBX_ACK_GOT_NAK) {
UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
} else {
UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
}
}
@@ -566,7 +566,7 @@ UBX::payload_rx_init()
break;
case UBX_RXMSG_DISABLE: // disable unexpected messages
UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
{
hrt_abstime t = hrt_absolute_time();
@@ -574,7 +574,7 @@ UBX::payload_rx_init()
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
/* don't attempt for every message to disable, some might not be disabled */
_disable_cmd_last = t;
UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
configure_message_rate(_rx_msg, 0);
}
}
@@ -677,16 +677,16 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
UBX_WARN("VER hash 0x%08x", _ubx_version);
UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
UBX_DEBUG("VER hash 0x%08x", _ubx_version);
UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
}
// fill Part 2 buffer
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
_buf.raw[buf_index] = b;
if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
// Part 2 complete: decode Part 2 buffer
UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
}
}
src/drivers/px4flow/px4flow.cpp
+13 -8
View File
@@ -602,7 +602,7 @@ bool start_in_progress = false;
const int START_RETRY_COUNT = 5;
const int START_RETRY_TIMEOUT = 1000;
void start();
int start();
void stop();
void test();
void reset();
@@ -611,22 +611,26 @@ void info();
/**
* Start the driver.
*/
void
int
start()
{
int fd;
/* entry check: */
if (start_in_progress) {
errx(1, "start in progress");
warnx("start already in progress");
return 1;
}
start_in_progress = true;
if (g_dev != nullptr) {
start_in_progress = false;
errx(1, "already started");
warnx("already started");
return 1;
}
warnx("scanning I2C buses for device..");
int retry_nr = 0;
while (1) {
const int busses_to_try[] = {
@@ -684,11 +688,12 @@ start()
/* success! */
start_in_progress = false;
exit(0);
return 0;
}
if (retry_nr < START_RETRY_COUNT) {
warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
/* lets not be too verbose */
// warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
usleep(START_RETRY_TIMEOUT * 1000);
retry_nr++;
} else {
@@ -702,7 +707,7 @@ start()
}
start_in_progress = false;
errx(1, "PX4FLOW could not be started over I2C");
return 1;
}
/**
@@ -852,7 +857,7 @@ px4flow_main(int argc, char *argv[])
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
px4flow::start();
return px4flow::start();
}
/*
src/drivers/px4fmu/fmu.cpp
+6 -5
View File
@@ -295,7 +295,8 @@ PX4FMU::PX4FMU() :
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
_debug_enabled = true;
/* only enable this during development */
_debug_enabled = false;
}
PX4FMU::~PX4FMU()
@@ -342,9 +343,9 @@ PX4FMU::init()
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
log("default PWM output device");
/* lets not be too verbose */
} else if (_class_instance < 0) {
log("FAILED registering class device");
warnx("FAILED registering class device");
}
/* reset GPIOs */
@@ -536,11 +537,11 @@ PX4FMU::subscribe()
_poll_fds_num = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
debug("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
debug("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
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 */
@@ -133,7 +133,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_sp{},
_rates_sp{},
_time_offset_avg_alpha(0.6),
_time_offset(0)
_time_offset(0),
_mom_switch_pos{},
_mom_switch_state(0)
{
}
@@ -904,15 +906,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;
}
}
@@ -1009,12 +1041,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 <= 0) {
_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
@@ -139,15 +139,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;
@@ -192,6 +203,11 @@ private:
double _time_offset_avg_alpha;
uint64_t _time_offset;
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 &);