From 09f6b8865102b6e2582061855e88a4c435927eb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 16:12:03 +0200 Subject: [PATCH] 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 &);