From 09f6b8865102b6e2582061855e88a4c435927eb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 16:12:03 +0200 Subject: [PATCH 1/4] MAVLink app: Implement switch toggling for simulated RC link --- src/modules/mavlink/mavlink_receiver.cpp | 68 ++++++++++++++++++------ src/modules/mavlink/mavlink_receiver.h | 32 ++++++++--- 2 files changed, 77 insertions(+), 23 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8ebbe47052..cf638ca043 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -35,9 +35,9 @@ * @file mavlink_receiver.cpp * MAVLink protocol message receive and dispatch * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler */ /* 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); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 18a3dc208a..e7668c10f3 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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 - * @author Anton Babushkin + * @author Lorenz Meier + * @author Anton Babushkin */ #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 &); From c38c58291e9db5425414b1466e5ed2e1254067b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:15:10 +0200 Subject: [PATCH 2/4] GPS: silence verbose boot messages --- src/drivers/gps/ubx.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 064ce20e5f..81b580892b 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -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); } } From 3679c2512501a99e14d5610191ed9236d2694b00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:15:25 +0200 Subject: [PATCH 3/4] PX4 FLOW: Silence alarming messages on a normal boot --- src/drivers/px4flow/px4flow.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 8ee365d8df..ddc5c61094 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -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(); } /* From a446cb6b2f7736c1f38032c8a72cd5027c6799fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:15:38 +0200 Subject: [PATCH 4/4] FMU driver: Disable debug mode --- src/drivers/px4fmu/fmu.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c55289fc54..f2ec0285be 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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; }