dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MAVLink app: Implement switch toggling for simulated RC link

This commit is contained in:
Lorenz Meier 2015-08-01 16:12:03 +02:00
parent 79910ce7e0
commit 09f6b88651
2 changed files with 77 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/modules/mavlink/mavlink_receiver.cpp
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);

32
src/modules/mavlink/mavlink_receiver.h
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 &);