diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-08-22 01:52:23 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-08-22 01:52:23 +0400 |
commit | 0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2 (patch) | |
tree | bf90731a92fe45f271c046af1220a04fad89863c /src/modules/uavcan/esc_controller.cpp | |
parent | f87860bc96a61c538de50b5b74aba0452f9b5784 (diff) | |
download | px4-firmware-0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2.tar.gz px4-firmware-0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2.tar.bz2 px4-firmware-0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2.zip |
UAVCAN: Refactored and generalized sensor bridge support
Diffstat (limited to 'src/modules/uavcan/esc_controller.cpp')
-rw-r--r-- | src/modules/uavcan/esc_controller.cpp | 138 |
1 files changed, 0 insertions, 138 deletions
diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp deleted file mode 100644 index 406eba88c..000000000 --- a/src/modules/uavcan/esc_controller.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file esc_controller.cpp - * - * @author Pavel Kirienko <pavel.kirienko@gmail.com> - */ - -#include "esc_controller.hpp" -#include <systemlib/err.h> - -UavcanEscController::UavcanEscController(uavcan::INode &node) : - _node(node), - _uavcan_pub_raw_cmd(node), - _uavcan_sub_status(node), - _orb_timer(node) -{ -} - -UavcanEscController::~UavcanEscController() -{ - perf_free(_perfcnt_invalid_input); - perf_free(_perfcnt_scaling_error); -} - -int UavcanEscController::init() -{ - // ESC status subscription - int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { - warnx("ESC status sub failed %i", res); - return res; - } - - // ESC status will be relayed from UAVCAN bus into ORB at this rate - _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); - _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); - - return res; -} - -void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) -{ - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { - perf_count(_perfcnt_invalid_input); - return; - } - - /* - * Rate limiting - we don't want to congest the bus - */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - _prev_cmd_pub = timestamp; - - /* - * Fill the command message - * If unarmed, we publish an empty message anyway - */ - uavcan::equipment::esc::RawCommand msg; - - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - - float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - - if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; - perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; - perf_count(_perfcnt_scaling_error); - } else { - ; // Correct value - } - - msg.cmd.push_back(static_cast<unsigned>(scaled)); - } - } - - /* - * Publish the command message to the bus - * Note that for a quadrotor it takes one CAN frame - */ - (void)_uavcan_pub_raw_cmd.broadcast(msg); -} - -void UavcanEscController::arm_esc(bool arm) -{ - _armed = arm; -} - -void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg) -{ - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() -} - -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) -{ - // TODO publish to ORB -} |