<commit_before>/*
 * Copyright (c) 2015 WinT 3794 <http://wint3794.org>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <QPointer>
#include <QSpinBox>
#include <QPushButton>
#include <QProgressBar>

#include <QDebug>

#include "JoysticksTab.h"
#include "VirtualJoystick.h"

//=============================================================================
// JoysticksTab::JoysticksTab
//=============================================================================

JoysticksTab::JoysticksTab (QWidget* parent) : QWidget (parent) {
    ui.setupUi (this);

    /* Hide the indicator containers */
    ui.Axes->setVisible (false);
    ui.Hats->setVisible (false);
    ui.Buttons->setVisible (false);

    /* Send SDL and virtual joystick data to this widget */
    m_keyboardDrive = new VirtualJoystick;
    connect (m_keyboardDrive, SIGNAL (HatEvent       (GM_Hat)),
             this,            SLOT   (OnHatEvent     (GM_Hat)));
    connect (m_keyboardDrive, SIGNAL (AxisEvent      (GM_Axis)),
             this,            SLOT   (OnAxisEvent    (GM_Axis)));
    connect (m_keyboardDrive, SIGNAL (ButtonEvent    (GM_Button)),
             this,            SLOT   (OnButtonEvent  (GM_Button)));
    connect (m_keyboardDrive, SIGNAL (CountChanged   (QStringList)),
             this,            SLOT   (OnCountChanged (QStringList)));

    /* Re-generate indicators when user selects another joystick */
    connect (ui.JoystickList, SIGNAL (currentRowChanged (int)),
             this,            SLOT   (GenerateIndicators      (int)));
}

//=============================================================================
// JoysticksTab::~JoysticksTab
//=============================================================================

JoysticksTab::~JoysticksTab() {
    delete m_keyboardDrive;
}

//=============================================================================
// JoysticksTab::ReadSettings
//=============================================================================

void JoysticksTab::ReadSettings() {
    m_keyboardDrive->ReadSettings();
}

//=============================================================================
// JoysticksTab::ShowKeyboardWindow
//=============================================================================

void JoysticksTab::ShowKeyboardWindow() {
    m_keyboardDrive->show();
}

//=============================================================================
// JoysticksTab::GenerateIndicators
//=============================================================================

void JoysticksTab::GenerateIndicators (int row) {
    /* Remove all joystick indicators in the widget */
    foreach (QSpinBox *     b, findChildren<QSpinBox*>())     delete b;
    foreach (QPushButton *  c, findChildren<QPushButton*>())  delete c;
    foreach (QProgressBar * p, findChildren<QProgressBar*>()) delete p;

    /* Clear joystick data */
    m_axes.clear();
    m_hats.clear();
    m_buttons.clear();

    /* Avoid crashing the application when there are no joysticks */
    if (row < 0)
        return;

    /* Get joystick information */
    int hatCount = m_keyboardDrive->GetNumHats (row);
    int axisCount = m_keyboardDrive->GetNumAxes (row);
    int buttonCount = m_keyboardDrive->GetNumButtons (row);

    /* Make the indicator containers visible */
    ui.Hats->setVisible (hatCount > 0);
    ui.Axes->setVisible (axisCount > 0);
    ui.Buttons->setVisible (buttonCount > 0);

    /* Create a progress bar for each axis */
    for (int i = 0; i < axisCount; ++i) {
        QPointer<QProgressBar> bar = new QProgressBar (this);

        bar->setMaximumHeight (19);
        bar->setMinimumHeight (19);

        bar->setValue (0);
        bar->setMaximum (100);
        bar->setMinimum (-100);
        bar->setFormat  (tr ("Axis %1").arg (i));

        m_axes.append (bar);
        ui.AxesWidget->layout()->addWidget (bar);
    }

    /* Create a button for each joystick button */
    for (int i = 0; i < buttonCount; ++i) {
        QPointer<QPushButton> button = new QPushButton (this);

        button->setEnabled (false);
        button->setCheckable (true);
        button->setMaximumSize (18, 12);
        button->setMinimumSize (18, 12);
        button->setToolTip     (tr ("Button %1").arg (i));

        /* Distribute the button items in a nice layout */
        int row = (i <= 7) ? i : i - 8;
        int column = (i <= 7) ? 0 : (i / 8);

        m_buttons.append (button);
        ui.ButtonLayout->addWidget (button, row, column);
    }

    /* Create a spinbox for each joystick hat */
    for (int i = 0; i < hatCount; ++i) {
        QPointer<QSpinBox> box = new QSpinBox (this);

        box->setRange (0, 360);
        box->setEnabled (false);

        m_hats.append (box);
        ui.HatsWidget->layout()->addWidget (box);
    }
}

//=============================================================================
// JoysticksTab::OnCountChanged
//=============================================================================

void JoysticksTab::OnCountChanged (QStringList list) {
    /* A joystick was removed */
    if (list.count() < ui.JoystickList->count())
        emit JoystickRemoved();

    /* Clear the joystick list (we will re-generate it later) */
    ui.JoystickList->clear();

    /* Put the current joysticks in the joystick list */
    if (list.count() > 0) {
        for (int i = 1; i <= list.count(); ++i)
            ui.JoystickList->addItem (QString ("%1: ").arg (i) + list.at (i - 1));

        /* Select the first item in the joystick selector */
        ui.JoystickList->setCurrentRow (0);
    }

    /* Notify other objects that JS count has changed */
    emit StatusChanged (list.count() > 0);
}

//=============================================================================
// JoysticksTab::OnHatEvent
//=============================================================================

void JoysticksTab::OnHatEvent (const GM_Hat& hat) {
    if (ui.JoystickList->currentRow() != hat.joystick.id)
        return;

    if (hat.id < m_hats.count())
        m_hats.at (hat.id)->setValue ((hat.angle == 0) ? 360 : hat.angle);
}

//=============================================================================
// JoysticksTab::OnAxisEvent
//=============================================================================

void JoysticksTab::OnAxisEvent (const GM_Axis& axis) {
    if (ui.JoystickList->currentRow() != axis.joystick.id)
        return;

    if (axis.id < m_axes.count())
        m_axes.at (axis.id)->setValue (axis.value * 100);
}

//=============================================================================
// JoysticksTab::OnButtonEvent
//=============================================================================

void JoysticksTab::OnButtonEvent (const GM_Button& button) {
    if (ui.JoystickList->currentRow() != button.joystick.id)
        return;

    if (button.id < m_buttons.count())
        m_buttons.at (button.id)->setChecked (button.pressed);
}
<commit_msg>Update JoysticksTab.cpp<commit_after>/*
 * Copyright (c) 2015 WinT 3794 <http://wint3794.org>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <QPointer>
#include <QSpinBox>
#include <QPushButton>
#include <QProgressBar>

#include <QDebug>

#include "JoysticksTab.h"
#include "VirtualJoystick.h"

//=============================================================================
// JoysticksTab::JoysticksTab
//=============================================================================

JoysticksTab::JoysticksTab (QWidget* parent) : QWidget (parent) {
    ui.setupUi (this);

    /* Hide the indicator containers */
    ui.Axes->setVisible (false);
    ui.Hats->setVisible (false);
    ui.Buttons->setVisible (false);

    /* Send SDL and virtual joystick data to this widget */
    m_keyboardDrive = new VirtualJoystick;
    connect (m_keyboardDrive, SIGNAL (HatEvent       (GM_Hat)),
             this,            SLOT   (OnHatEvent     (GM_Hat)));
    connect (m_keyboardDrive, SIGNAL (AxisEvent      (GM_Axis)),
             this,            SLOT   (OnAxisEvent    (GM_Axis)));
    connect (m_keyboardDrive, SIGNAL (ButtonEvent    (GM_Button)),
             this,            SLOT   (OnButtonEvent  (GM_Button)));
    connect (m_keyboardDrive, SIGNAL (CountChanged   (QStringList)),
             this,            SLOT   (OnCountChanged (QStringList)));

    /* Re-generate indicators when user selects another joystick */
    connect (ui.JoystickList, SIGNAL (currentRowChanged  (int)),
             this,            SLOT   (GenerateIndicators (int)));
}

//=============================================================================
// JoysticksTab::~JoysticksTab
//=============================================================================

JoysticksTab::~JoysticksTab() {
    delete m_keyboardDrive;
}

//=============================================================================
// JoysticksTab::ReadSettings
//=============================================================================

void JoysticksTab::ReadSettings() {
    m_keyboardDrive->ReadSettings();
}

//=============================================================================
// JoysticksTab::ShowKeyboardWindow
//=============================================================================

void JoysticksTab::ShowKeyboardWindow() {
    m_keyboardDrive->show();
}

//=============================================================================
// JoysticksTab::GenerateIndicators
//=============================================================================

void JoysticksTab::GenerateIndicators (int row) {
    /* Remove all joystick indicators in the widget */
    foreach (QSpinBox *     b, findChildren<QSpinBox*>())     delete b;
    foreach (QPushButton *  c, findChildren<QPushButton*>())  delete c;
    foreach (QProgressBar * p, findChildren<QProgressBar*>()) delete p;

    /* Clear joystick data */
    m_axes.clear();
    m_hats.clear();
    m_buttons.clear();

    /* Avoid crashing the application when there are no joysticks */
    if (row < 0)
        return;

    /* Get joystick information */
    int hatCount = m_keyboardDrive->GetNumHats (row);
    int axisCount = m_keyboardDrive->GetNumAxes (row);
    int buttonCount = m_keyboardDrive->GetNumButtons (row);

    /* Make the indicator containers visible */
    ui.Hats->setVisible (hatCount > 0);
    ui.Axes->setVisible (axisCount > 0);
    ui.Buttons->setVisible (buttonCount > 0);

    /* Create a progress bar for each axis */
    for (int i = 0; i < axisCount; ++i) {
        QPointer<QProgressBar> bar = new QProgressBar (this);

        bar->setMaximumHeight (19);
        bar->setMinimumHeight (19);

        bar->setValue (0);
        bar->setMaximum (100);
        bar->setMinimum (-100);
        bar->setFormat  (tr ("Axis %1").arg (i));

        m_axes.append (bar);
        ui.AxesWidget->layout()->addWidget (bar);
    }

    /* Create a button for each joystick button */
    for (int i = 0; i < buttonCount; ++i) {
        QPointer<QPushButton> button = new QPushButton (this);

        button->setEnabled (false);
        button->setCheckable (true);
        button->setMaximumSize (18, 12);
        button->setMinimumSize (18, 12);
        button->setToolTip     (tr ("Button %1").arg (i));

        /* Distribute the button items in a nice layout */
        int row = (i <= 7) ? i : i - 8;
        int column = (i <= 7) ? 0 : (i / 8);

        m_buttons.append (button);
        ui.ButtonLayout->addWidget (button, row, column);
    }

    /* Create a spinbox for each joystick hat */
    for (int i = 0; i < hatCount; ++i) {
        QPointer<QSpinBox> box = new QSpinBox (this);

        box->setRange (0, 360);
        box->setEnabled (false);

        m_hats.append (box);
        ui.HatsWidget->layout()->addWidget (box);
    }
}

//=============================================================================
// JoysticksTab::OnCountChanged
//=============================================================================

void JoysticksTab::OnCountChanged (QStringList list) {
    /* A joystick was removed */
    if (list.count() < ui.JoystickList->count())
        emit JoystickRemoved();

    /* Clear the joystick list (we will re-generate it later) */
    ui.JoystickList->clear();

    /* Put the current joysticks in the joystick list */
    if (list.count() > 0) {
        for (int i = 1; i <= list.count(); ++i)
            ui.JoystickList->addItem (QString ("%1: ").arg (i) + list.at (i - 1));

        /* Select the first item in the joystick selector */
        ui.JoystickList->setCurrentRow (0);
    }

    /* Notify other objects that JS count has changed */
    emit StatusChanged (list.count() > 0);
}

//=============================================================================
// JoysticksTab::OnHatEvent
//=============================================================================

void JoysticksTab::OnHatEvent (const GM_Hat& hat) {
    if (ui.JoystickList->currentRow() != hat.joystick.id)
        return;

    if (hat.id < m_hats.count())
        m_hats.at (hat.id)->setValue ((hat.angle == 0) ? 360 : hat.angle);
}

//=============================================================================
// JoysticksTab::OnAxisEvent
//=============================================================================

void JoysticksTab::OnAxisEvent (const GM_Axis& axis) {
    if (ui.JoystickList->currentRow() != axis.joystick.id)
        return;

    if (axis.id < m_axes.count())
        m_axes.at (axis.id)->setValue (axis.value * 100);
}

//=============================================================================
// JoysticksTab::OnButtonEvent
//=============================================================================

void JoysticksTab::OnButtonEvent (const GM_Button& button) {
    if (ui.JoystickList->currentRow() != button.joystick.id)
        return;

    if (button.id < m_buttons.count())
        m_buttons.at (button.id)->setChecked (button.pressed);
}
<|endoftext|>


<commit_before>/**
 *
 * \section COPYRIGHT
 *
 * Copyright 2013-2015 The srsLTE Developers. See the
 * COPYRIGHT file at the top-level directory of this distribution.
 *
 * \section LICENSE
 *
 * This file is part of the srsLTE library.
 *
 * srsLTE is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as
 * published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * srsLTE is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * A copy of the GNU Affero General Public License can be found in
 * the LICENSE file in the top-level directory of this distribution
 * and at http://www.gnu.org/licenses/.
 *
 */

#include "srsapps/ue/mac/mux.h"
#include "srsapps/ue/mac/mac.h"

namespace srslte {
namespace ue {

#define IO_IDX(lch)     (lch + mac_io::MAC_LCH_CCCH_UL)
#define UL_IDX(lch)     (lch - mac_io::MAC_LCH_CCCH_UL)

mux::mux() : pdu_msg(20)
{
  msg3_buff.init(1, MSG3_BUFF_SZ);
  pdu_buff.init(1, PDU_BUFF_SZ);
  bzero(nof_tx_pkts, sizeof(uint32_t) * mac_io::NOF_UL_LCH);
  pthread_mutex_init(&mutex, NULL);
  msg3_has_been_transmitted = false;

  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
   priority[i]        = 1;
   priority_sorted[i] = 1;
   PBR[i]             = -1; // -1 is infinite
   BSD[i]             = 10;
   lchid_sorted[i]      = i;
  }
}

void mux::init(log *log_h_, mac_io *mac_io_h_, bsr_proc *bsr_procedure_)
{
  log_h      = log_h_;
  mac_io_h   = mac_io_h_;
  bsr_procedure = bsr_procedure_;
}

void mux::reset()
{
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    Bj[i] = 0;
  }
}

bool mux::is_pending_ccch_sdu()
{
  return is_pending_sdu(0);
}

bool mux::is_pending_any_sdu()
{
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    if (!mac_io_h->get(i)->isempty()) {
      return true;
    }
  }
  return false;
}

bool mux::is_pending_sdu(uint32_t lch_id) {
  lch_id += (uint32_t) mac_io::MAC_LCH_CCCH_UL;
  if (lch_id < mac_io::MAC_NOF_QUEUES) {
    return !mac_io_h->get(lch_id)->isempty();
  }
}

void mux::set_priority(uint32_t lch_id, uint32_t set_priority, int set_PBR, uint32_t set_BSD)
{
  pthread_mutex_lock(&mutex);
  if (lch_id < mac_io::NOF_UL_LCH) {
    priority[lch_id] = set_priority;
    PBR[lch_id]      = set_PBR;
    BSD[lch_id]      = set_BSD;

    // Insert priority in sorted idx array
    int new_index = 0;
    while(set_priority > priority_sorted[new_index] && new_index < mac_io::NOF_UL_LCH) {
      new_index++;
    }
    int old_index = 0;
    while(lch_id != lchid_sorted[old_index] && new_index < mac_io::NOF_UL_LCH) {
      old_index++;
    }
    if (new_index ==  mac_io::NOF_UL_LCH) {
      Error("Can't find LchID=%d in sorted list\n", lch_id);
      return;
    }
    // Replace goes in one direction or the other
    int add=new_index>old_index?1:-1;
    for (int i=old_index;i!=new_index;i+=add) {
      priority_sorted[i] = priority_sorted[i+add];
      lchid_sorted[i]    = lchid_sorted[i+add];
    }
    priority_sorted[new_index] = set_priority;
    lchid_sorted[new_index]    = lch_id;
  }
  pthread_mutex_unlock(&mutex);
}

void mux::pdu_release()
{
  pdu_buff.release();
}

bool mux::pdu_move_to_msg3(uint32_t pdu_sz)
{
  if (pdu_buff.isempty()) {
    if (assemble_pdu(pdu_sz)) {
      if (pdu_buff.pending_data() < MSG3_BUFF_SZ) {
        pdu_buff.move_to(&msg3_buff);
        return true;
      } else {
        pdu_buff.release();
        Error("Assembled PDU size exceeds Msg3 buffer size\n");
        return false;
      }
    } else {
      Error("Assembling PDU\n");
      return false;
    }
  } else {
    Error("Generating PDU: PDU pending in buffer for transmission\n");
    return false;
  }
}

// Multiplexing and logical channel priorization as defined in Section 5.4.3
uint8_t* mux::pdu_pop(uint32_t pdu_sz)
{
  if (pdu_buff.isempty()) {
    if (assemble_pdu(pdu_sz)) {
      return (uint8_t*) pdu_buff.pop();
    } else {
      return NULL;
    }
  } else {
    Error("Generating PDU: PDU pending in buffer for transmission\n");
    return NULL;
  }
}

void mux::append_crnti_ce_next_tx(uint16_t crnti) {
  pending_crnti_ce = crnti;
}

sch_subh::cetype bsr_format_convert(bsr_proc::bsr_format_t format) {
  switch(format) {
    case bsr_proc::LONG_BSR:
      return sch_subh::LONG_BSR;
    case bsr_proc::SHORT_BSR:
      return sch_subh::SHORT_BSR;
    case bsr_proc::TRUNC_BSR:
      return sch_subh::TRUNC_BSR;

  }
}

bool mux::assemble_pdu(uint32_t pdu_sz_nbits) {

  uint8_t *buff = (uint8_t*) pdu_buff.request();
  if (!buff) {
    Error("Assembling PDU: Buffer is not available\n");
    return false;
  }

  // Make sure pdu_sz is byte-aligned
  pdu_sz_nbits = 8*(pdu_sz_nbits/8);

  // Acquire mutex. Cannot change priorities, PBR or BSD after assemble finishes
  pthread_mutex_lock(&mutex);

  // Update Bj
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    // Add PRB unless it's infinity
    if (PBR[i] >= 0) {
      Bj[i] += PBR[i];
    }
    if (Bj[i] >= BSD[i]) {
      Bj[i] = BSD[i];
    }
  }

// Logical Channel Procedure

  uint32_t sdu_sz   = 0;

  pdu_msg.init(pdu_sz_nbits/8, true);

  // MAC control element for C-RNTI or data from UL-CCCH
  bool is_first = true;
  if (!allocate_sdu(UL_IDX(mac_io::MAC_LCH_CCCH_UL), &pdu_msg, &is_first)) {
    if (pending_crnti_ce) {
      if (pdu_msg.new_subh()) {
        pdu_msg.next();
        if (!pdu_msg.get()->set_c_rnti(pending_crnti_ce)) {
          Warning("Pending C-RNTI CE could not be inserted in MAC PDU\n");
        }
      }
    }
  }
  pending_crnti_ce = 0;

  uint32_t bsr_payload_sz = bsr_procedure->need_to_send_bsr_on_ul_grant(pdu_msg.rem_size());
  bsr_proc::bsr_t bsr;

  // MAC control element for BSR, with exception of BSR included for padding;
  sch_subh *bsr_subh = NULL;
  if (bsr_payload_sz) {
    Info("Including BSR CE size %d\n", bsr_payload_sz);
    if (pdu_msg.new_subh()) {
      pdu_msg.next();
      bsr_subh = pdu_msg.get();
      pdu_msg.update_space_ce(bsr_payload_sz);
    }
  }
  // MAC control element for PHR
     // TODO

  // data from any Logical Channel, except data from UL-CCCH;
  // first only those with positive Bj
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    bool res = true;
    while ((Bj[i] > 0 || PBR[i] < 0) && res) {
      res = allocate_sdu(lchid_sorted[i], &pdu_msg, &sdu_sz, &is_first);
      if (res && PBR[i] >= 0) {
        Bj[i] -= sdu_sz;
      }
    }
  }

  // If resources remain, allocate regardless of their Bj value
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    while (allocate_sdu(lchid_sorted[i], &pdu_msg));
  }

  bool send_bsr = bsr_procedure->generate_bsr_on_ul_grant(pdu_msg.rem_size(), &bsr);
  // Insert Padding BSR if not inserted Regular/Periodic BSR
  if (!bsr_payload_sz && send_bsr) {
    if (pdu_msg.new_subh()) {
      pdu_msg.next();
      bsr_subh = pdu_msg.get();
    }
  }

  // And set the BSR
  if (bsr_subh) {
    bsr_subh->set_bsr(bsr.buff_size, bsr_format_convert(bsr.format), bsr_payload_sz?false:true);
  }

  pthread_mutex_unlock(&mutex);

  /* Release all SDUs */
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    while(nof_tx_pkts[i] > 0) {
      mac_io_h->get(IO_IDX(i))->release();
      nof_tx_pkts[i]--;
    }
  }

  Info("Assembled MAC PDU msg size %d/%d bytes\n", pdu_msg.size(), pdu_sz_nbits/8);
  //pdu_msg.fprint(stdout);

  /* Generate MAC PDU and save to buffer */
  if (pdu_msg.write_packet(buff)) {
    pdu_buff.push(pdu_sz_nbits);
  } else {
    Error("Writing PDU message to packet\n");
    return false;
  }
  return true;
}

bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg)
{
  return allocate_sdu(lcid, pdu_msg, NULL, NULL);
}
bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg, bool *is_first)
{
  return allocate_sdu(lcid, pdu_msg, NULL, is_first);
}
bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg, uint32_t *sdu_sz, bool *is_first)
{

  // Get n-th pending SDU pointer and length
  uint32_t buff_len = 0;
  uint8_t *buff_ptr = (uint8_t*) mac_io_h->get(mac_io::MAC_LCH_CCCH_UL + lcid)->pop(&buff_len, nof_tx_pkts[lcid]);

  if (buff_ptr && buff_len > 0) { // there is pending SDU to allocate
    if (sdu_sz) {
      *sdu_sz = buff_len;
    }
    if (pdu_msg->new_subh()) { // there is space for a new subheader
      pdu_msg->next();
      if (pdu_msg->get()->set_sdu(lcid, buff_ptr, buff_len/8, is_first?*is_first:false)) { // new SDU could be added
        if (is_first) {
          *is_first = false;
        }
        // Increase number of pop'ed packets from queue
        nof_tx_pkts[lcid]++;
        return true;
      } else {
        pdu_msg->del_subh();
      }
    }
  }
  return false;
}

void mux::msg3_flush()
{
  msg3_buff.flush();
  msg3_has_been_transmitted = false;
}

void mux::msg3_transmitted()
{
  msg3_has_been_transmitted = true;
}

bool mux::msg3_is_transmitted()
{
  return msg3_has_been_transmitted;
}

/* Returns a pointer to the Msg3 buffer */
uint8_t* mux::msg3_pop(uint32_t TB_size)
{
  uint32_t len;
  uint8_t *msg3 = (uint8_t*) msg3_buff.pop(&len);
  if (msg3) {
    if (len < TB_size) {
      // Pad with zeros without exceeding maximum buffer size
      if (TB_size <= MSG3_BUFF_SZ) {
        bzero(&msg3[len], (TB_size-len)*sizeof(uint8_t));
      } else {
        Error("Requested TB size from Msg3 buffer exceeds buffer size (%d>%d)\n", TB_size, MSG3_BUFF_SZ);
        return NULL;
      }
    }
  }
  return msg3;
}

}
}
<commit_msg>MAC MUX rounding up SDU payload size<commit_after>/**
 *
 * \section COPYRIGHT
 *
 * Copyright 2013-2015 The srsLTE Developers. See the
 * COPYRIGHT file at the top-level directory of this distribution.
 *
 * \section LICENSE
 *
 * This file is part of the srsLTE library.
 *
 * srsLTE is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as
 * published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * srsLTE is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * A copy of the GNU Affero General Public License can be found in
 * the LICENSE file in the top-level directory of this distribution
 * and at http://www.gnu.org/licenses/.
 *
 */

#include "srsapps/ue/mac/mux.h"
#include "srsapps/ue/mac/mac.h"

namespace srslte {
namespace ue {

#define IO_IDX(lch)     (lch + mac_io::MAC_LCH_CCCH_UL)
#define UL_IDX(lch)     (lch - mac_io::MAC_LCH_CCCH_UL)

mux::mux() : pdu_msg(20)
{
  msg3_buff.init(1, MSG3_BUFF_SZ);
  pdu_buff.init(1, PDU_BUFF_SZ);
  bzero(nof_tx_pkts, sizeof(uint32_t) * mac_io::NOF_UL_LCH);
  pthread_mutex_init(&mutex, NULL);
  msg3_has_been_transmitted = false;

  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
   priority[i]        = 1;
   priority_sorted[i] = 1;
   PBR[i]             = -1; // -1 is infinite
   BSD[i]             = 10;
   lchid_sorted[i]      = i;
  }
}

void mux::init(log *log_h_, mac_io *mac_io_h_, bsr_proc *bsr_procedure_)
{
  log_h      = log_h_;
  mac_io_h   = mac_io_h_;
  bsr_procedure = bsr_procedure_;
}

void mux::reset()
{
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    Bj[i] = 0;
  }
}

bool mux::is_pending_ccch_sdu()
{
  return is_pending_sdu(0);
}

bool mux::is_pending_any_sdu()
{
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    if (!mac_io_h->get(i)->isempty()) {
      return true;
    }
  }
  return false;
}

bool mux::is_pending_sdu(uint32_t lch_id) {
  lch_id += (uint32_t) mac_io::MAC_LCH_CCCH_UL;
  if (lch_id < mac_io::MAC_NOF_QUEUES) {
    return !mac_io_h->get(lch_id)->isempty();
  }
}

void mux::set_priority(uint32_t lch_id, uint32_t set_priority, int set_PBR, uint32_t set_BSD)
{
  pthread_mutex_lock(&mutex);
  if (lch_id < mac_io::NOF_UL_LCH) {
    priority[lch_id] = set_priority;
    PBR[lch_id]      = set_PBR;
    BSD[lch_id]      = set_BSD;

    // Insert priority in sorted idx array
    int new_index = 0;
    while(set_priority > priority_sorted[new_index] && new_index < mac_io::NOF_UL_LCH) {
      new_index++;
    }
    int old_index = 0;
    while(lch_id != lchid_sorted[old_index] && new_index < mac_io::NOF_UL_LCH) {
      old_index++;
    }
    if (new_index ==  mac_io::NOF_UL_LCH) {
      Error("Can't find LchID=%d in sorted list\n", lch_id);
      return;
    }
    // Replace goes in one direction or the other
    int add=new_index>old_index?1:-1;
    for (int i=old_index;i!=new_index;i+=add) {
      priority_sorted[i] = priority_sorted[i+add];
      lchid_sorted[i]    = lchid_sorted[i+add];
    }
    priority_sorted[new_index] = set_priority;
    lchid_sorted[new_index]    = lch_id;
  }
  pthread_mutex_unlock(&mutex);
}

void mux::pdu_release()
{
  pdu_buff.release();
}

bool mux::pdu_move_to_msg3(uint32_t pdu_sz)
{
  if (pdu_buff.isempty()) {
    if (assemble_pdu(pdu_sz)) {
      if (pdu_buff.pending_data() < MSG3_BUFF_SZ) {
        pdu_buff.move_to(&msg3_buff);
        return true;
      } else {
        pdu_buff.release();
        Error("Assembled PDU size exceeds Msg3 buffer size\n");
        return false;
      }
    } else {
      Error("Assembling PDU\n");
      return false;
    }
  } else {
    Error("Generating PDU: PDU pending in buffer for transmission\n");
    return false;
  }
}

// Multiplexing and logical channel priorization as defined in Section 5.4.3
uint8_t* mux::pdu_pop(uint32_t pdu_sz)
{
  if (pdu_buff.isempty()) {
    if (assemble_pdu(pdu_sz)) {
      return (uint8_t*) pdu_buff.pop();
    } else {
      return NULL;
    }
  } else {
    Error("Generating PDU: PDU pending in buffer for transmission\n");
    return NULL;
  }
}

void mux::append_crnti_ce_next_tx(uint16_t crnti) {
  pending_crnti_ce = crnti;
}

sch_subh::cetype bsr_format_convert(bsr_proc::bsr_format_t format) {
  switch(format) {
    case bsr_proc::LONG_BSR:
      return sch_subh::LONG_BSR;
    case bsr_proc::SHORT_BSR:
      return sch_subh::SHORT_BSR;
    case bsr_proc::TRUNC_BSR:
      return sch_subh::TRUNC_BSR;

  }
}

bool mux::assemble_pdu(uint32_t pdu_sz_nbits) {

  uint8_t *buff = (uint8_t*) pdu_buff.request();
  if (!buff) {
    Error("Assembling PDU: Buffer is not available\n");
    return false;
  }

  // Make sure pdu_sz is byte-aligned
  pdu_sz_nbits = 8*(pdu_sz_nbits/8);

  // Acquire mutex. Cannot change priorities, PBR or BSD after assemble finishes
  pthread_mutex_lock(&mutex);

  // Update Bj
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    // Add PRB unless it's infinity
    if (PBR[i] >= 0) {
      Bj[i] += PBR[i];
    }
    if (Bj[i] >= BSD[i]) {
      Bj[i] = BSD[i];
    }
  }

// Logical Channel Procedure

  uint32_t sdu_sz   = 0;

  pdu_msg.init(pdu_sz_nbits/8, true);

  // MAC control element for C-RNTI or data from UL-CCCH
  bool is_first = true;
  if (!allocate_sdu(UL_IDX(mac_io::MAC_LCH_CCCH_UL), &pdu_msg, &is_first)) {
    if (pending_crnti_ce) {
      if (pdu_msg.new_subh()) {
        pdu_msg.next();
        if (!pdu_msg.get()->set_c_rnti(pending_crnti_ce)) {
          Warning("Pending C-RNTI CE could not be inserted in MAC PDU\n");
        }
      }
    }
  }
  pending_crnti_ce = 0;

  uint32_t bsr_payload_sz = bsr_procedure->need_to_send_bsr_on_ul_grant(pdu_msg.rem_size());
  bsr_proc::bsr_t bsr;

  // MAC control element for BSR, with exception of BSR included for padding;
  sch_subh *bsr_subh = NULL;
  if (bsr_payload_sz) {
    Info("Including BSR CE size %d\n", bsr_payload_sz);
    if (pdu_msg.new_subh()) {
      pdu_msg.next();
      bsr_subh = pdu_msg.get();
      pdu_msg.update_space_ce(bsr_payload_sz);
    }
  }
  // MAC control element for PHR
     // TODO
    printf("1 nof_subh=%d, rem_size=%d\n", pdu_msg.nof_subh(), pdu_msg.rem_size());

  // data from any Logical Channel, except data from UL-CCCH;
  // first only those with positive Bj
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    bool res = true;
    while ((Bj[i] > 0 || PBR[i] < 0) && res) {
      res = allocate_sdu(lchid_sorted[i], &pdu_msg, &sdu_sz, &is_first);
      if (res && PBR[i] >= 0) {
        Bj[i] -= sdu_sz;
      }
    }
  }
  printf("2 nof_subh=%d, rem_size=%d\n", pdu_msg.nof_subh(), pdu_msg.rem_size());

  // If resources remain, allocate regardless of their Bj value
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    while (allocate_sdu(lchid_sorted[i], &pdu_msg));
  }

  printf("3 nof_subh=%d, rem_size=%d\n", pdu_msg.nof_subh(), pdu_msg.rem_size());

  bool send_bsr = bsr_procedure->generate_bsr_on_ul_grant(pdu_msg.rem_size(), &bsr);
  // Insert Padding BSR if not inserted Regular/Periodic BSR
  if (!bsr_payload_sz && send_bsr) {
    if (pdu_msg.new_subh()) {
      pdu_msg.next();
      bsr_subh = pdu_msg.get();
    }
  }

  // And set the BSR
  if (bsr_subh) {
    bsr_subh->set_bsr(bsr.buff_size, bsr_format_convert(bsr.format), bsr_payload_sz?false:true);
  }

  pthread_mutex_unlock(&mutex);

  /* Release all SDUs */
  for (int i=0;i<mac_io::NOF_UL_LCH;i++) {
    while(nof_tx_pkts[i] > 0) {
      mac_io_h->get(IO_IDX(i))->release();
      nof_tx_pkts[i]--;
    }
  }

  Info("Assembled MAC PDU msg size %d/%d bytes\n", pdu_msg.size(), pdu_sz_nbits/8);
  //pdu_msg.fprint(stdout);

  /* Generate MAC PDU and save to buffer */
  if (pdu_msg.write_packet(buff)) {
    pdu_buff.push(pdu_sz_nbits);
  } else {
    Error("Writing PDU message to packet\n");
    return false;
  }
  return true;
}

bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg)
{
  return allocate_sdu(lcid, pdu_msg, NULL, NULL);
}
bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg, bool *is_first)
{
  return allocate_sdu(lcid, pdu_msg, NULL, is_first);
}
bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg, uint32_t *sdu_sz, bool *is_first)
{

  // Get n-th pending SDU pointer and length
  uint32_t buff_len = 0;
  uint8_t *buff_ptr = (uint8_t*) mac_io_h->get(mac_io::MAC_LCH_CCCH_UL + lcid)->pop(&buff_len, nof_tx_pkts[lcid]);

  uint32_t nbytes = (buff_len-1)/8 + 1;

  if (buff_ptr && buff_len > 0) { // there is pending SDU to allocate
    if (sdu_sz) {
      *sdu_sz = buff_len;
    }
    if (pdu_msg->new_subh()) { // there is space for a new subheader
      pdu_msg->next();
      if (pdu_msg->get()->set_sdu(lcid, buff_ptr, nbytes, is_first?*is_first:false)) { // new SDU could be added
        if (is_first) {
          *is_first = false;
        }
        Info("Allocated SDU lcid=%d nbytes=%d\n", lcid, nbytes);
        // Increase number of pop'ed packets from queue
        nof_tx_pkts[lcid]++;
        return true;
      } else {
        pdu_msg->del_subh();
      }
    }
  }
  return false;
}

void mux::msg3_flush()
{
  msg3_buff.flush();
  msg3_has_been_transmitted = false;
}

void mux::msg3_transmitted()
{
  msg3_has_been_transmitted = true;
}

bool mux::msg3_is_transmitted()
{
  return msg3_has_been_transmitted;
}

/* Returns a pointer to the Msg3 buffer */
uint8_t* mux::msg3_pop(uint32_t TB_size)
{
  uint32_t len;
  uint8_t *msg3 = (uint8_t*) msg3_buff.pop(&len);
  if (msg3) {
    if (len < TB_size) {
      // Pad with zeros without exceeding maximum buffer size
      if (TB_size <= MSG3_BUFF_SZ) {
        bzero(&msg3[len], (TB_size-len)*sizeof(uint8_t));
      } else {
        Error("Requested TB size from Msg3 buffer exceeds buffer size (%d>%d)\n", TB_size, MSG3_BUFF_SZ);
        return NULL;
      }
    }
  }
  return msg3;
}

}
}
<|endoftext|>


<commit_before>#include <cassert>
#include <libport/statistics.hh>

#include <iostream>

template<typename T, typename R>
static void
check(const libport::Statistics<T, R>& s,
      size_t size, T min, T max, R mean, R variance)
{
  std::cerr << "Mean: " << s.mean() << std::endl;
  std::cerr << "Variance: " << s.variance() << std::endl;
  assert(size == s.size());
  assert(min == s.min());
  assert(max == s.max());
  assert(fabs(mean - s.mean()) < 1e-4);
  assert(fabs(variance - s.variance()) < 1e-4);
}

template<typename R>
static void
check1(size_t capacity, unsigned int size, unsigned int min, unsigned int max, R mean, R variance)
{
  libport::Statistics<unsigned int, R> s(capacity);
  s.add_sample(2);
  s.add_sample(2);
  s.add_sample(3);
  s.add_sample(4);
  s.add_sample(7);
  check(s, size, min, max, mean, variance);
}

int
main()
{
  // Running samples
  check1<unsigned int>(3, 3, 3, 7, 4, 3);
  check1<double>(3, 3, 3, 7, 4.666666666, 2.8888888888);

  // Non-running samples
  check1<unsigned int>(0, 5, 2, 7, 3, 3);
  check1<double>(0, 5, 2, 7, 3.6, 3.44);

  // Running samples with non-full buffer
  check1<unsigned int>(10, 5, 2, 7, 3, 3);
  check1<double>(10, 5, 2, 7, 3.6, 3.44);
}
<commit_msg>Desambiguate "fabs" call.<commit_after>#include <cassert>
#include <libport/statistics.hh>

#include <iostream>

// Visual C++ gets confused if we use "fabs" in the template below, as
// it is an overloaded operator.
static double
_fabs(double d)
{
  return fabs(d);
}

template<typename T, typename R>
static void
check(const libport::Statistics<T, R>& s,
      size_t size, T min, T max, R mean, R variance)
{
  std::cerr << "Mean: " << s.mean() << std::endl;
  std::cerr << "Variance: " << s.variance() << std::endl;
  assert(size == s.size());
  assert(min == s.min());
  assert(max == s.max());
  assert(_fabs(mean - s.mean()) < 1e-4);
  assert(_fabs(variance - s.variance()) < 1e-4);
}

template<typename R>
static void
check1(size_t capacity, unsigned int size, unsigned int min, unsigned int max, R mean, R variance)
{
  libport::Statistics<unsigned int, R> s(capacity);
  s.add_sample(2);
  s.add_sample(2);
  s.add_sample(3);
  s.add_sample(4);
  s.add_sample(7);
  check(s, size, min, max, mean, variance);
}

int
main()
{
  // Running samples
  check1<unsigned int>(3, 3, 3, 7, 4, 3);
  check1<double>(3, 3, 3, 7, 4.666666666, 2.8888888888);

  // Non-running samples
  check1<unsigned int>(0, 5, 2, 7, 3, 3);
  check1<double>(0, 5, 2, 7, 3.6, 3.44);

  // Running samples with non-full buffer
  check1<unsigned int>(10, 5, 2, 7, 3, 3);
  check1<double>(10, 5, 2, 7, 3.6, 3.44);
}
<|endoftext|>


<commit_before>#include "bombled.h"

#define LED_PWM     255
#define SEGMENT_PWM 255

BombLED::BombLED()
{
  _blinkOn = false;
}

void BombLED::setup()
{
  _driver.setup();
}

void BombLED::reset() {
  _minutes = DEFAULT_MINUTES;
  _seconds = DEFAULT_SECONDS;
  _ledState = 0;
  _enabled = false;
  _refresh = true;
  update();
}

void BombLED::complete() {
  _ledState = 0x3FF;
  _enabled = true;
  _refresh = true;
  update();
}

bool BombLED::isFinished()
{
  return (_ledState == 0x3FF) && !isFailed();
}

bool BombLED::isFailed()
{
  return (_minutes == 0) && (_seconds == 0);
}

void BombLED::update()
{
  if (!_refresh) return;

  if (_enabled) {
    word leds = _ledState;
    if (_blinkOn) leds |= ((1 << 10) | (1 << 11));
    _setLeds(~leds, LED_PWM);

    byte duty;
    if (isFinished() & _blinkOn) duty = 0;
    else duty = SEGMENT_PWM;

    setDigits(_minutes / 10, _minutes % 10, _seconds / 10, _seconds % 10, duty);
  }
  else {
    _driver.clear();
  }
  _driver.update();
  _refresh = false;
}

void BombLED::tickSecond()
{
  if (!_enabled) return;
  if (isFinished()) return;
  if (_seconds > 0) {
    _seconds--;
    _refresh = true;
  }
  else if (_minutes > 0) {
    _seconds = 59;
    _minutes--;
    _refresh = true;
  }
}

void BombLED::toggleBlink() {
  if (!_enabled) return;
  if (isFinished()) return;
  _blinkOn = !_blinkOn;
  _refresh = true;
}

void BombLED::setLeds(word state) {
  _ledState = state;
}

word BombLED::getLeds() {
  return _ledState;
}

void BombLED::setEnabled(byte enabled)
{
  _enabled = enabled;
  _refresh = true;
}

byte BombLED::getEnabled() const
{
  return _enabled;
}

void BombLED::setMinutes(byte minutes)
{
  _minutes = (minutes > 99) ? 99 : minutes;
  _refresh = true;
}

byte BombLED::getMinutes() const
{
  return _minutes;
}

void BombLED::setSeconds(byte seconds)
{
  _seconds = (seconds > 59) ? 59 : seconds;
  _refresh = true;
}

byte BombLED::getSeconds() const
{
  return _seconds;
}

const byte BombLED::_onesmap[]={16,17,0,1,2,14,13,15};
const byte BombLED::_tensmap[]={11,12,5,6,7,9,8,10};
const byte BombLED::_ledmap[]={17,15,14,13,12,11,10,9,8,7,1,16};

const byte BombLED::_digits[11]={
  0b01111110,     // 0
  0b00110000,     // 1
  0b01101101,
  0b01111001,
  0b00110011,     // 4
  0b01011011,
  0b01011111,
  0b01110000,     // 7
  0b01111111,     // 8
  0b01111011,     // 9
  0b00000000      // OFF
};

void BombLED::_setLeds(word indata, byte duty) {
  for (byte i=0;i<12;i++){
    _driver.set(_ledmap[i]+18, (indata & (1 << i))? duty : 0);
  }
}

void BombLED::setDots(byte indata, byte duty) {
  _driver.set(_tensmap[7], (indata & (1 << 3))?duty : 0); //mTens
  _driver.set(_onesmap[7], (indata & (1 << 2))?duty : 0); //mOnes
  _driver.set(_tensmap[7]+36, (indata & (1 << 1))?duty : 0); //sTens
  _driver.set(_onesmap[7]+36, (indata & (1 << 0))?duty : 0); //sOnes
}

void BombLED::setDigit(byte index, byte value, byte duty) {
  setSegments(index, _digits[value], duty);
}

void BombLED::setSegments(byte index, byte mask, byte duty) {
  for (byte i = 0; i < 7; i++) {
    byte bit_idx;
    switch (index) {
    case 0:
      bit_idx = _tensmap[i];
      break;
    case 1:
      bit_idx = _onesmap[i];
      break;
    case 2:
      bit_idx = _tensmap[i]+36;
      break;
    case 3:
      bit_idx = _onesmap[i]+36;
      break;
    }
    _driver.set(bit_idx, (mask & (1 << (6-i)))?duty : 0);
  }
}

void BombLED::setDigits(byte mTens,byte mOnes,byte sTens,byte sOnes, byte duty) {
  byte i;
  for (i=0;i<7;i++){
    _driver.set(_tensmap[i], (_digits[mTens] & (1 << (6-i)))?duty : 0);
    _driver.set(_onesmap[i], (_digits[mOnes] & (1 << (6-i)))?duty : 0);
    _driver.set(_tensmap[i]+36, (_digits[sTens] & (1 << (6-i)))?duty : 0);
    _driver.set(_onesmap[i]+36, (_digits[sOnes] & (1 << (6-i)))?duty : 0);
  }
}

<commit_msg>Fixed last LED dimming<commit_after>#include "bombled.h"

#define LED_PWM     255
#define SEGMENT_PWM 255

BombLED::BombLED()
{
  _blinkOn = false;
}

void BombLED::setup()
{
  _driver.setup();
}

void BombLED::reset() {
  _minutes = DEFAULT_MINUTES;
  _seconds = DEFAULT_SECONDS;
  _ledState = 0;
  _enabled = false;
  _refresh = true;
  update();
}

void BombLED::complete() {
  _ledState = 0x3FF;
  _enabled = true;
  _refresh = true;
  update();
}

bool BombLED::isFinished()
{
  return (_ledState == 0x3FF) && !isFailed();
}

bool BombLED::isFailed()
{
  return (_minutes == 0) && (_seconds == 0);
}

void BombLED::update()
{
  if (!_refresh) return;

  if (_enabled) {
    word leds = _ledState;
    if (_blinkOn) leds |= ((1 << 10) | (1 << 11));
    _setLeds(~leds, LED_PWM);

    byte duty;
    if (isFinished() & _blinkOn) duty = 0;
    else duty = SEGMENT_PWM;

    setDigits(_minutes / 10, _minutes % 10, _seconds / 10, _seconds % 10, duty);
  }
  else {
    _driver.clear();
  }
  _driver.update();
  _refresh = false;
}

void BombLED::tickSecond()
{
  if (!_enabled) return;
  if (isFinished()) return;
  if (_seconds > 0) {
    _seconds--;
    _refresh = true;
  }
  else if (_minutes > 0) {
    _seconds = 59;
    _minutes--;
    _refresh = true;
  }
}

void BombLED::toggleBlink() {
  if (!_enabled) return;
  if (isFinished()) return;
  _blinkOn = !_blinkOn;
  _refresh = true;
}

void BombLED::setLeds(word state) {
  _ledState = state;
  _refresh = true;
}

word BombLED::getLeds() {
  return _ledState;
}

void BombLED::setEnabled(byte enabled)
{
  _enabled = enabled;
  _refresh = true;
}

byte BombLED::getEnabled() const
{
  return _enabled;
}

void BombLED::setMinutes(byte minutes)
{
  _minutes = (minutes > 99) ? 99 : minutes;
  _refresh = true;
}

byte BombLED::getMinutes() const
{
  return _minutes;
}

void BombLED::setSeconds(byte seconds)
{
  _seconds = (seconds > 59) ? 59 : seconds;
  _refresh = true;
}

byte BombLED::getSeconds() const
{
  return _seconds;
}

const byte BombLED::_onesmap[]={16,17,0,1,2,14,13,15};
const byte BombLED::_tensmap[]={11,12,5,6,7,9,8,10};
const byte BombLED::_ledmap[]={17,15,14,13,12,11,10,9,8,7,1,16};

const byte BombLED::_digits[11]={
  0b01111110,     // 0
  0b00110000,     // 1
  0b01101101,
  0b01111001,
  0b00110011,     // 4
  0b01011011,
  0b01011111,
  0b01110000,     // 7
  0b01111111,     // 8
  0b01111011,     // 9
  0b00000000      // OFF
};

void BombLED::_setLeds(word indata, byte duty) {
  for (byte i=0;i<12;i++){
    _driver.set(_ledmap[i]+18, (indata & (1 << i))? duty : 0);
  }
}

void BombLED::setDots(byte indata, byte duty) {
  _driver.set(_tensmap[7], (indata & (1 << 3))?duty : 0); //mTens
  _driver.set(_onesmap[7], (indata & (1 << 2))?duty : 0); //mOnes
  _driver.set(_tensmap[7]+36, (indata & (1 << 1))?duty : 0); //sTens
  _driver.set(_onesmap[7]+36, (indata & (1 << 0))?duty : 0); //sOnes
}

void BombLED::setDigit(byte index, byte value, byte duty) {
  setSegments(index, _digits[value], duty);
}

void BombLED::setSegments(byte index, byte mask, byte duty) {
  for (byte i = 0; i < 7; i++) {
    byte bit_idx;
    switch (index) {
    case 0:
      bit_idx = _tensmap[i];
      break;
    case 1:
      bit_idx = _onesmap[i];
      break;
    case 2:
      bit_idx = _tensmap[i]+36;
      break;
    case 3:
      bit_idx = _onesmap[i]+36;
      break;
    }
    _driver.set(bit_idx, (mask & (1 << (6-i)))?duty : 0);
  }
}

void BombLED::setDigits(byte mTens,byte mOnes,byte sTens,byte sOnes, byte duty) {
  byte i;
  for (i=0;i<7;i++){
    _driver.set(_tensmap[i], (_digits[mTens] & (1 << (6-i)))?duty : 0);
    _driver.set(_onesmap[i], (_digits[mOnes] & (1 << (6-i)))?duty : 0);
    _driver.set(_tensmap[i]+36, (_digits[sTens] & (1 << (6-i)))?duty : 0);
    _driver.set(_onesmap[i]+36, (_digits[sOnes] & (1 << (6-i)))?duty : 0);
  }
}

<|endoftext|>


<commit_before>#ifndef SWITCH_NODE_HPP
#define SWITCH_NODE_HPP

#include <core/tp_aware_dtm_node.hpp>
#include <core/error_handler.hpp>
#include <math/functions.hpp>
#include <helper/node_counter.hpp>
#include <memory>
#include <utility>
#include <string>

namespace mcmc_utilities
{
  template <typename T,template <typename TE> class T_vector>
  class switch_node
    :public tp_aware_dtm_node<T,T_vector>
  {
  public:
    switch_node(int ninputs)
      :tp_aware_dtm_node<T,T_vector>(ninputs+1,1)
    {}

    T do_calc(size_t idx,const T_vector<T>& parent)const override
    {
      int n=size_t(parent.back());
      if(n<0||n>=this->num_of_parents()-1)
	{
	  throw var_out_of_range();
	}
      return parent.at(n);
    }

    std::shared_ptr<node<T,T_vector> > do_clone()const override
    {
      auto p=new switch_node(this->num_of_parents()-1);
      return std::shared_ptr<node<T,T_vector> >(p);
    }

    order do_get_order(const node<T,T_vector>* pn,int n)const override
    {
      for(int i=0;i<this->num_of_parents();++i)
	{
	  order o=this->get_parent_order(0,pn,n);
	  //return order{0,false,false};
	  if(o.n!=0||
	     !o.poly)
	    {
	      return order{0,false,false};
	    }
	}
      return order{0,true,true};
    }
  };
}

#endif
<commit_msg>	修改：     nodes/switch_node.hpp<commit_after>#ifndef SWITCH_NODE_HPP
#define SWITCH_NODE_HPP

#include <core/tp_aware_dtm_node.hpp>
#include <core/error_handler.hpp>
#include <math/functions.hpp>
#include <helper/node_counter.hpp>
#include <memory>
#include <utility>
#include <string>

namespace mcmc_utilities
{
  template <typename T,template <typename TE> class T_vector>
  class switch_node
    :public tp_aware_dtm_node<T,T_vector>
  {
  public:
    switch_node(int ninputs)
      :tp_aware_dtm_node<T,T_vector>(ninputs+1,1)
    {}

    T do_calc(size_t idx,const T_vector<T>& parent)const override
    {
      int n=size_t(parent.back());
      if(n<0||n>=this->num_of_parents()-1)
	{
	  std::cerr<<"n="<<n<<std::endl;
	  std::cerr<<"nswitch="<<(this->num_of_parents()-1)<<std::endl;
	  throw var_out_of_range();
	}
      return parent.at(n);
    }

    std::shared_ptr<node<T,T_vector> > do_clone()const override
    {
      auto p=new switch_node(this->num_of_parents()-1);
      return std::shared_ptr<node<T,T_vector> >(p);
    }

    order do_get_order(const node<T,T_vector>* pn,int n)const override
    {
      for(int i=0;i<this->num_of_parents();++i)
	{
	  order o=this->get_parent_order(0,pn,n);
	  //return order{0,false,false};
	  if(o.n!=0||
	     !o.poly)
	    {
	      return order{0,false,false};
	    }
	}
      return order{0,true,true};
    }
  };
}

#endif
<|endoftext|>


<commit_before>#include <libmesh/dirichlet_boundaries.h>
#include <libmesh/dof_map.h>
#include <libmesh/elem.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/function_base.h>
#include <libmesh/linear_implicit_system.h>
#include <libmesh/mesh.h>
#include <libmesh/numeric_vector.h>
#include <libmesh/periodic_boundary.h>
#include <libmesh/quadrature_gauss.h>
#include <libmesh/sparse_matrix.h>
#include <libmesh/wrapped_function.h>

#include "test_comm.h"
#include "libmesh_cppunit.h"

using namespace libMesh;

Number quadratic_solution (const Point& p,
                           const Parameters&,
                           const std::string&,
                           const std::string&)
{
  const Real & y = LIBMESH_DIM > 1 ? p(1) : 0;

  // Discontinuity at y=1 from the forcing function there.
  // Periodic on -3 < y < 2
  return (y > 1) ? (y-3)*(y-3) : (y+1)*(y+1);
}

struct PeriodicQuadFunction : public FunctionBase<Number>
{
  PeriodicQuadFunction() = default;

  virtual std::unique_ptr<FunctionBase<Number>> clone () const
  { return libmesh_make_unique<PeriodicQuadFunction>(); }

  // We only really need the vector-valued output for projections
  virtual Number operator() (const Point &,
                             const Real /*time*/ = 0.) override
  { libmesh_error(); }

  virtual void operator() (const Point & p,
                           const Real time,
                           DenseVector<Number> & output) override
  {
    libmesh_assert_equal_to(output.size(), 1);
    Parameters params;
    output(0) = quadratic_solution(p, params, "", "");
  }

  Number component (unsigned int i,
                    const Point & p,
                    Real /* time */) override
  {
    Parameters params;
    switch (i) {
    case 0:
      return quadratic_solution(p, params, "", "");
    default:
      libmesh_error();
    }
    return 0;
  }
};

void periodic_bc_test_poisson(EquationSystems& es,
                              const std::string& system_name)
{
  const MeshBase& mesh = es.get_mesh();
  LinearImplicitSystem& system = es.get_system<LinearImplicitSystem>("PBCSys");
  const DofMap& dof_map = system.get_dof_map();

  // Interior FE to integrate smooth part of poisson problem
  FEType fe_type = dof_map.variable_type(0);
  std::unique_ptr<FEBase> fe (FEBase::build(2, fe_type));
  QGauss qrule (2, fe_type.default_quadrature_order());
  fe->attach_quadrature_rule(&qrule);

  const std::vector<Real> & JxW = fe->get_JxW();
  const std::vector<std::vector<Real>> & phi = fe->get_phi();
  const std::vector<std::vector<RealGradient>> & dphi = fe->get_dphi();

  // Face FE to integrate delta function part of forcing
  std::unique_ptr<FEBase> fe_face (FEBase::build(2, fe_type));
  QGauss qface (1, fe_type.default_quadrature_order());
  fe_face->attach_quadrature_rule(&qface);

  const std::vector<Real> & JxW_face = fe_face->get_JxW();
  const std::vector<std::vector<Real>> & phi_face = fe_face->get_phi();
  std::unique_ptr<const Elem> elem_side;

  DenseMatrix<Number> Ke;
  DenseVector<Number> Fe;

  std::vector<dof_id_type> dof_indices;

  for (const Elem * elem : mesh.active_local_element_ptr_range())
    {
      dof_map.dof_indices (elem, dof_indices);
      const unsigned int n_dofs = dof_indices.size();

      Ke.resize (n_dofs, n_dofs);
      Fe.resize (n_dofs);

      fe->reinit (elem);

      // Integrate the poisson problem over the element interior, where we have
      // a nice f=2 forcing function aside from the discontinuity
      for (unsigned int qp=0; qp<qrule.n_points(); qp++)
        {
          for (unsigned int i=0; i != n_dofs; i++)
            {
              for (unsigned int j=0; j != n_dofs; j++)
                Ke(i,j) += -JxW[qp]*(dphi[i][qp]*dphi[j][qp]);

              Fe(i) += JxW[qp]*phi[i][qp]*2;
            }
        }

      for(unsigned int s=0; s != elem->n_sides(); ++s)
      {
        elem->build_side_ptr(elem_side, s);
        Point centroid = elem_side->centroid();
        if (std::abs(centroid(1) - 1) < TOLERANCE)
          {
            fe_face->reinit(elem, s);

            for (unsigned int qp=0; qp<qface.n_points(); qp++)
              {
                // delta function divided by 2 since we hit it from
                // both sides
                for (unsigned int i=0; i != n_dofs; i++)
                  Fe(i) += JxW_face[qp]*phi_face[i][qp]*(-4);
              }
          }
      }

      dof_map.heterogenously_constrain_element_matrix_and_vector(Ke, Fe, dof_indices);
      system.matrix->add_matrix (Ke, dof_indices);
      system.rhs->add_vector    (Fe, dof_indices);
    }

  system.rhs->close();
  system.matrix->close();
}

const boundary_id_type top_id    = 50;
const boundary_id_type bottom_id = 60;
const boundary_id_type side_id   = 70;

class PeriodicBCTest : public CppUnit::TestCase {
public:
  CPPUNIT_TEST_SUITE( PeriodicBCTest );

#if LIBMESH_DIM > 1
#if defined(LIBMESH_HAVE_SOLVER) && defined(LIBMESH_HAVE_EXODUS_API) && defined(LIBMESH_HAVE_GZSTREAM)
  CPPUNIT_TEST( testPeriodicLagrange2 );
#endif
#endif // LIBMESH_DIM > 1

  CPPUNIT_TEST_SUITE_END();

private:

  void testPeriodicBC (FEType fe_type)
  {
    Mesh mesh(*TestCommWorld);
    BoundaryInfo & boundary = mesh.get_boundary_info();

    EquationSystems es(mesh);
    LinearImplicitSystem &sys =
      es.add_system<LinearImplicitSystem> ("PBCSys");

    // We need to change around BCIDs and set up the periodic BC first
    mesh.allow_remote_element_removal(false);

    mesh.read("meshes/shark_tooth_tri6.xda.gz");

    // Add some BCIDs on top and bottom for our periodic boundary
    for (const Elem * elem : mesh.active_element_ptr_range())
      {
        for (unsigned int s=0; s != 3; ++s)
          if (!elem->neighbor_ptr(s))
            {
              unsigned v2 = (s+1)%3;
              // Left-running edges are on bottom, because this xda
              // has a flipped mesh
              if (elem->point(s)(0) - elem->point(v2)(0) < -.1)
                boundary.add_side(elem, s, top_id);

              // Right-running edges are on top
              else if (elem->point(s)(0) - elem->point(v2)(0) > .1)
                boundary.add_side(elem, s, bottom_id);

              // Vertical edges are Dirichlet
              else
                boundary.add_side(elem, s, side_id);
            }
      }

    PeriodicBoundary vert(RealVectorValue(0., 4.0));
    vert.myboundary = bottom_id;
    vert.pairedboundary = top_id;
    sys.get_dof_map().add_periodic_boundary(vert);

    // Okay, *now* the mesh knows to save periodic neighbors
    mesh.allow_remote_element_removal(true);
    mesh.delete_remote_elements();

    const unsigned int u_var = sys.add_variable("u", fe_type);
    sys.attach_assemble_function (periodic_bc_test_poisson);

    std::set<boundary_id_type> side_bdy { side_id };
    std::vector<unsigned int> all_vars (1, u_var);
    WrappedFunction<Number> exact_val(sys, quadratic_solution);
    DirichletBoundary exact_bc(side_bdy, all_vars, exact_val);
    sys.get_dof_map().add_dirichlet_boundary(exact_bc);

    es.init();

    sys.solve();

    ExodusII_IO(mesh).write_equation_systems("periodic.e", es);

    Parameters params;

    for (Real x = 0.1; x < 10; x += 0.2)
      for (Real ya = -2.9; ya < 1; ya += 0.2)
        {
          // Integrate the sharktooth shape
          const Real offset = 1-std::abs(std::fmod(x,2)-1);
          const Real y = ya + offset;
          Point p{x,y};
          const Number exact = quadratic_solution(p,params,"","");
          const Number approx = sys.point_value(0,p);
          LIBMESH_ASSERT_FP_EQUAL(libmesh_real(exact),
                                  libmesh_real(approx),
                                  TOLERANCE*TOLERANCE*10);
        }
  }

  void testPeriodicLagrange2() { testPeriodicBC(FEType(SECOND, LAGRANGE)); }
};

CPPUNIT_TEST_SUITE_REGISTRATION( PeriodicBCTest );
<commit_msg>Bump up tolerance in the "sharktooth" PBC test<commit_after>#include <libmesh/dirichlet_boundaries.h>
#include <libmesh/dof_map.h>
#include <libmesh/elem.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/function_base.h>
#include <libmesh/linear_implicit_system.h>
#include <libmesh/mesh.h>
#include <libmesh/numeric_vector.h>
#include <libmesh/periodic_boundary.h>
#include <libmesh/quadrature_gauss.h>
#include <libmesh/sparse_matrix.h>
#include <libmesh/wrapped_function.h>

#include "test_comm.h"
#include "libmesh_cppunit.h"

using namespace libMesh;

Number quadratic_solution (const Point& p,
                           const Parameters&,
                           const std::string&,
                           const std::string&)
{
  const Real & y = LIBMESH_DIM > 1 ? p(1) : 0;

  // Discontinuity at y=1 from the forcing function there.
  // Periodic on -3 < y < 2
  return (y > 1) ? (y-3)*(y-3) : (y+1)*(y+1);
}

struct PeriodicQuadFunction : public FunctionBase<Number>
{
  PeriodicQuadFunction() = default;

  virtual std::unique_ptr<FunctionBase<Number>> clone () const
  { return libmesh_make_unique<PeriodicQuadFunction>(); }

  // We only really need the vector-valued output for projections
  virtual Number operator() (const Point &,
                             const Real /*time*/ = 0.) override
  { libmesh_error(); }

  virtual void operator() (const Point & p,
                           const Real time,
                           DenseVector<Number> & output) override
  {
    libmesh_assert_equal_to(output.size(), 1);
    Parameters params;
    output(0) = quadratic_solution(p, params, "", "");
  }

  Number component (unsigned int i,
                    const Point & p,
                    Real /* time */) override
  {
    Parameters params;
    switch (i) {
    case 0:
      return quadratic_solution(p, params, "", "");
    default:
      libmesh_error();
    }
    return 0;
  }
};

void periodic_bc_test_poisson(EquationSystems& es,
                              const std::string& system_name)
{
  const MeshBase& mesh = es.get_mesh();
  LinearImplicitSystem& system = es.get_system<LinearImplicitSystem>("PBCSys");
  const DofMap& dof_map = system.get_dof_map();

  // Interior FE to integrate smooth part of poisson problem
  FEType fe_type = dof_map.variable_type(0);
  std::unique_ptr<FEBase> fe (FEBase::build(2, fe_type));
  QGauss qrule (2, fe_type.default_quadrature_order());
  fe->attach_quadrature_rule(&qrule);

  const std::vector<Real> & JxW = fe->get_JxW();
  const std::vector<std::vector<Real>> & phi = fe->get_phi();
  const std::vector<std::vector<RealGradient>> & dphi = fe->get_dphi();

  // Face FE to integrate delta function part of forcing
  std::unique_ptr<FEBase> fe_face (FEBase::build(2, fe_type));
  QGauss qface (1, fe_type.default_quadrature_order());
  fe_face->attach_quadrature_rule(&qface);

  const std::vector<Real> & JxW_face = fe_face->get_JxW();
  const std::vector<std::vector<Real>> & phi_face = fe_face->get_phi();
  std::unique_ptr<const Elem> elem_side;

  DenseMatrix<Number> Ke;
  DenseVector<Number> Fe;

  std::vector<dof_id_type> dof_indices;

  for (const Elem * elem : mesh.active_local_element_ptr_range())
    {
      dof_map.dof_indices (elem, dof_indices);
      const unsigned int n_dofs = dof_indices.size();

      Ke.resize (n_dofs, n_dofs);
      Fe.resize (n_dofs);

      fe->reinit (elem);

      // Integrate the poisson problem over the element interior, where we have
      // a nice f=2 forcing function aside from the discontinuity
      for (unsigned int qp=0; qp<qrule.n_points(); qp++)
        {
          for (unsigned int i=0; i != n_dofs; i++)
            {
              for (unsigned int j=0; j != n_dofs; j++)
                Ke(i,j) += -JxW[qp]*(dphi[i][qp]*dphi[j][qp]);

              Fe(i) += JxW[qp]*phi[i][qp]*2;
            }
        }

      for(unsigned int s=0; s != elem->n_sides(); ++s)
      {
        elem->build_side_ptr(elem_side, s);
        Point centroid = elem_side->centroid();
        if (std::abs(centroid(1) - 1) < TOLERANCE)
          {
            fe_face->reinit(elem, s);

            for (unsigned int qp=0; qp<qface.n_points(); qp++)
              {
                // delta function divided by 2 since we hit it from
                // both sides
                for (unsigned int i=0; i != n_dofs; i++)
                  Fe(i) += JxW_face[qp]*phi_face[i][qp]*(-4);
              }
          }
      }

      dof_map.heterogenously_constrain_element_matrix_and_vector(Ke, Fe, dof_indices);
      system.matrix->add_matrix (Ke, dof_indices);
      system.rhs->add_vector    (Fe, dof_indices);
    }

  system.rhs->close();
  system.matrix->close();
}

const boundary_id_type top_id    = 50;
const boundary_id_type bottom_id = 60;
const boundary_id_type side_id   = 70;

class PeriodicBCTest : public CppUnit::TestCase {
public:
  CPPUNIT_TEST_SUITE( PeriodicBCTest );

#if LIBMESH_DIM > 1
#if defined(LIBMESH_HAVE_SOLVER) && defined(LIBMESH_HAVE_EXODUS_API) && defined(LIBMESH_HAVE_GZSTREAM)
  CPPUNIT_TEST( testPeriodicLagrange2 );
#endif
#endif // LIBMESH_DIM > 1

  CPPUNIT_TEST_SUITE_END();

private:

  void testPeriodicBC (FEType fe_type)
  {
    Mesh mesh(*TestCommWorld);
    BoundaryInfo & boundary = mesh.get_boundary_info();

    EquationSystems es(mesh);
    LinearImplicitSystem &sys =
      es.add_system<LinearImplicitSystem> ("PBCSys");

    // We need to change around BCIDs and set up the periodic BC first
    mesh.allow_remote_element_removal(false);

    mesh.read("meshes/shark_tooth_tri6.xda.gz");

    // Add some BCIDs on top and bottom for our periodic boundary
    for (const Elem * elem : mesh.active_element_ptr_range())
      {
        for (unsigned int s=0; s != 3; ++s)
          if (!elem->neighbor_ptr(s))
            {
              unsigned v2 = (s+1)%3;
              // Left-running edges are on bottom, because this xda
              // has a flipped mesh
              if (elem->point(s)(0) - elem->point(v2)(0) < -.1)
                boundary.add_side(elem, s, top_id);

              // Right-running edges are on top
              else if (elem->point(s)(0) - elem->point(v2)(0) > .1)
                boundary.add_side(elem, s, bottom_id);

              // Vertical edges are Dirichlet
              else
                boundary.add_side(elem, s, side_id);
            }
      }

    PeriodicBoundary vert(RealVectorValue(0., 4.0));
    vert.myboundary = bottom_id;
    vert.pairedboundary = top_id;
    sys.get_dof_map().add_periodic_boundary(vert);

    // Okay, *now* the mesh knows to save periodic neighbors
    mesh.allow_remote_element_removal(true);
    mesh.delete_remote_elements();

    const unsigned int u_var = sys.add_variable("u", fe_type);
    sys.attach_assemble_function (periodic_bc_test_poisson);

    std::set<boundary_id_type> side_bdy { side_id };
    std::vector<unsigned int> all_vars (1, u_var);
    WrappedFunction<Number> exact_val(sys, quadratic_solution);
    DirichletBoundary exact_bc(side_bdy, all_vars, exact_val);
    sys.get_dof_map().add_dirichlet_boundary(exact_bc);

    es.init();

    sys.solve();

    ExodusII_IO(mesh).write_equation_systems("periodic.e", es);

    Parameters params;

    for (Real x = 0.1; x < 10; x += 0.2)
      for (Real ya = -2.9; ya < 1; ya += 0.2)
        {
          // Integrate the sharktooth shape
          const Real offset = 1-std::abs(std::fmod(x,2)-1);
          const Real y = ya + offset;
          Point p{x,y};
          const Number exact = quadratic_solution(p,params,"","");
          const Number approx = sys.point_value(0,p);
          LIBMESH_ASSERT_FP_EQUAL(libmesh_real(exact),
                                  libmesh_real(approx),
                                  TOLERANCE*TOLERANCE*20);
        }
  }

  void testPeriodicLagrange2() { testPeriodicBC(FEType(SECOND, LAGRANGE)); }
};

CPPUNIT_TEST_SUITE_REGISTRATION( PeriodicBCTest );
<|endoftext|>


<commit_before>/*
 * The Biomechanical ToolKit
 * Copyright (c) 2009-2013, Arnaud Barré
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer.
 *     * 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.
 *     * Neither the name(s) of the copyright holders 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.
 */

#include "btkForcePlatformWrenchFilter.h"
#include "btkConvert.h"

namespace btk
{
  /**
   * @class ForcePlatformWrenchFilter btkForcePlatformWrenchFilter.h
   * @brief Calcule the wrench of the center of the force platform data, expressed in the global frame (by default).
   *
   * Based on the given collection of forceplate set in input, this filter transform the associated analog channels in forces and moments.
   * This transformation take into account the type of each force platform.
   *
   * You can use the method SetTransformToGlobalFrame() to have the wrench expressed in the frame of the force platform.
   *
   * @ingroup BTKBasicFilters
   */

  /**
   * @typedef ForcePlatformWrenchFilter::Pointer
   * Smart pointer associated with a ForcePlatformWrenchFilter object.
   */

  /**
   * @typedef ForcePlatformWrenchFilter::ConstPointer
   * Smart pointer associated with a const ForcePlatformWrenchFilter object.
   */

  /**
   * @fn static Pointer ForcePlatformWrenchFilter::New();
   * Creates a smart pointer associated with a ForcePlatformWrenchFilter object.
   */

  /**
   * @fn WrenchCollection::Pointer ForcePlatformWrenchFilter::GetInput()
   * Gets the input registered with this process.
   */

  /**
   * @fn void ForcePlatformWrenchFilter::SetInput(ForcePlatform::Pointer input)
   * Sets the input required with this process. This input is transformed in a collection force platforms with a single force platform.
   */

  /**
   * @fn void ForcePlatformWrenchFilter::SetInput(ForcePlatformCollection::Pointer input)
   * Sets the input required with this process.
   */

  /**
   * @fn PointCollection::Pointer ForcePlatformWrenchFilter::GetOutput()
   * Gets the output created with this process.
   */

  /**
   * Sets the flag to activate the transformation to the global frame.
   */
  void ForcePlatformWrenchFilter::SetTransformToGlobalFrame(bool activation)
  {
    if (this->m_GlobalTransformationActivated == activation)
      return;
    this->m_GlobalTransformationActivated = activation;
    this->Modified();
  };

  /**
   * @fn bool ForcePlatformWrenchFilter::GetTransformToGlobalFrame() const
   * Returns the activation flag for the transformation in the global frame.
   */

  /**
   * Constructor. Sets the number of inputs and outputs to 1.
   */
  ForcePlatformWrenchFilter::ForcePlatformWrenchFilter()
  : ProcessObject()
  {
    this->SetInputNumber(1);
    this->SetOutputNumber(1);
    this->m_GlobalTransformationActivated = true;
  };

  /**
   * @fn ForcePlatformCollection::Pointer ForcePlatformWrenchFilter::GetInput(int idx)
   * Returns the input at the index @a idx.
   */

  /**
   * @fn WrenchCollection::Pointer ForcePlatformWrenchFilter::GetOutput(int idx)
   * Returns the output at the index @a idx.
   */

  /**
   * Creates a WrenchCollection:Pointer object and return it as a DataObject::Pointer.
   */
  DataObject::Pointer ForcePlatformWrenchFilter::MakeOutput(int /* idx */)
  {
    return WrenchCollection::New();
  };

  /**
   * Generates the outputs' data.
   */
  void ForcePlatformWrenchFilter::GenerateData()
  {
    WrenchCollection::Pointer output = this->GetOutput();
    ForcePlatformCollection::Pointer input = this->GetInput();
    if (input.get() != 0)
    {
      int inc = 0;
      for (ForcePlatformCollection::ConstIterator it = input->Begin() ; it != input->End() ; ++it)
      {
        ++inc;
        if ((*it)->GetChannelNumber() == 0)
        {
          btkErrorMacro("Unexpected number of analog channels (0) for force platform #" + ToString(inc));
          continue;
        }
        int frameNumber = (*it)->GetChannel(0)->GetFrameNumber();
        Wrench::Pointer wrh;
        if (inc <= output->GetItemNumber())
        {
          wrh = output->GetItem(inc-1);
          wrh->SetFrameNumber(frameNumber);
          output->Modified();
        }
        else
        {
          wrh = Wrench::New(this->GetWrenchPrefix() + ToString(inc), frameNumber);
          output->InsertItem(wrh);
        }
        wrh->Modified();
        // Residuals
        wrh->GetPosition()->GetResiduals().setZero(frameNumber);
        wrh->GetForce()->GetResiduals().setZero(frameNumber);
        wrh->GetMoment()->GetResiduals().setZero(frameNumber);
        // Values
        switch((*it)->GetType())
        {
          // 6 channels
          case 1:
            wrh->GetForce()->GetValues().col(0) = (*it)->GetChannel(0)->GetValues();
            wrh->GetForce()->GetValues().col(1) = (*it)->GetChannel(1)->GetValues();
            wrh->GetForce()->GetValues().col(2) = (*it)->GetChannel(2)->GetValues();
            wrh->GetPosition()->GetValues().col(0) = (*it)->GetChannel(3)->GetValues();
            wrh->GetPosition()->GetValues().col(1) = (*it)->GetChannel(4)->GetValues();
            wrh->GetPosition()->GetValues().col(2).setZero();
            wrh->GetMoment()->GetValues().col(0).setZero();
            wrh->GetMoment()->GetValues().col(1).setZero();
            wrh->GetMoment()->GetValues().col(2) = (*it)->GetChannel(5)->GetValues();
            this->FinishTypeI(wrh, *it, inc);
            break;
          case 2:
          case 4:
          case 5:
            wrh->GetForce()->GetValues().col(0) = (*it)->GetChannel(0)->GetValues();
            wrh->GetForce()->GetValues().col(1) = (*it)->GetChannel(1)->GetValues();
            wrh->GetForce()->GetValues().col(2) = (*it)->GetChannel(2)->GetValues();
            wrh->GetMoment()->GetValues().col(0) = (*it)->GetChannel(3)->GetValues();
            wrh->GetMoment()->GetValues().col(1) = (*it)->GetChannel(4)->GetValues();
            wrh->GetMoment()->GetValues().col(2) = (*it)->GetChannel(5)->GetValues();
            this->FinishAMTI(wrh, *it, inc);
            break;
          case 3:
            // Fx
            wrh->GetForce()->GetValues().col(0) = (*it)->GetChannel(0)->GetValues() + (*it)->GetChannel(1)->GetValues();
            // Fy
            wrh->GetForce()->GetValues().col(1) = (*it)->GetChannel(2)->GetValues() + (*it)->GetChannel(3)->GetValues();
            // Fz
            wrh->GetForce()->GetValues().col(2) = (*it)->GetChannel(4)->GetValues() + (*it)->GetChannel(5)->GetValues() + (*it)->GetChannel(6)->GetValues() + (*it)->GetChannel(7)->GetValues();
            // Mx
            wrh->GetMoment()->GetValues().col(0) = (*it)->GetOrigin().y() * ((*it)->GetChannel(4)->GetValues() + (*it)->GetChannel(5)->GetValues() - (*it)->GetChannel(6)->GetValues() - (*it)->GetChannel(7)->GetValues());
            // My
            wrh->GetMoment()->GetValues().col(1) = (*it)->GetOrigin().x() * ((*it)->GetChannel(5)->GetValues() + (*it)->GetChannel(6)->GetValues() - (*it)->GetChannel(4)->GetValues() - (*it)->GetChannel(7)->GetValues());
            // Mz
            wrh->GetMoment()->GetValues().col(2) = (*it)->GetOrigin().y() * ((*it)->GetChannel(1)->GetValues() - (*it)->GetChannel(0)->GetValues()) + (*it)->GetOrigin().x() * ((*it)->GetChannel(2)->GetValues() - (*it)->GetChannel(3)->GetValues());
            this->FinishKistler(wrh, *it, inc);
            break;
          case 6:
            btkErrorMacro("Force Platform type 6 is not yet supported. Please, report this to the developers");
            break;
          case 7:
            btkErrorMacro("Force Platform type 7 is not yet supported. Please, report this to the developers");
            break;
          case 11:
            btkErrorMacro("Force Platform type 11 is not yet supported. Please, report this to the developers");
            break;
          case 12:
            btkErrorMacro("Force Platform type 12 is not yet supported. Please, report this to the developers");
            break;
          case 21:
            btkErrorMacro("Force Platform type 21 is not yet supported. Please, report this to the developers");
            break;
        }
        if (this->m_GlobalTransformationActivated)
          this->TransformToGlobal(wrh, (*it)->GetCorners());
      }
    }
  };

  /**
   * Finish the computation of the wrench for the force platform type I.
   * Because, it is force platform Type I, the position is not set to the origin, but measured to the COP. The moment must be corrected!
   */
  void ForcePlatformWrenchFilter::FinishTypeI(Wrench::Pointer wrh , ForcePlatform::Pointer /* fp */, int /* index */)
  {
    typedef Eigen::Array<double, Eigen::Dynamic, 1> Component;
    Component Fx = wrh->GetForce()->GetValues().col(0).array();
    Component Fy = wrh->GetForce()->GetValues().col(1).array();
    Component Fz = wrh->GetForce()->GetValues().col(2).array();
    Component Mx = wrh->GetMoment()->GetValues().col(0).array();
    Component My = wrh->GetMoment()->GetValues().col(1).array();
    Component Mz = wrh->GetMoment()->GetValues().col(2).array();
    Component Px = wrh->GetPosition()->GetValues().col(0).array();
    Component Py = wrh->GetPosition()->GetValues().col(1).array();
    Component Pz = wrh->GetPosition()->GetValues().col(2).array();
    Mx -= Fy * Pz - Py * Fz;
    My -= Fz * Px - Pz * Fx;
    Mz -= Fx * Py - Px * Fy;
    wrh->GetMoment()->GetValues().col(0) = Mx;
    wrh->GetMoment()->GetValues().col(1) = My;
    wrh->GetMoment()->GetValues().col(2) = Mz;
  };

  /**
   * Finish the computation of the wrench for the AMTI force platform (nothing to do).
   */
  void ForcePlatformWrenchFilter::FinishAMTI(Wrench::Pointer /* wrh */, ForcePlatform::Pointer /* fp */, int /* index */)
  {};

  /**
   * Finish the computation of the wrench for the Kistler force platform (nothing to do).
   */
  void ForcePlatformWrenchFilter::FinishKistler(Wrench::Pointer /* wrh */, ForcePlatform::Pointer /* fp */, int /* index */)
  {};
};

<commit_msg>[FIX] Loading an acquisition with 3 force platforms in Mokka and loading another acquisition with only 2 force platforms still show the force vector of the third one.<commit_after>/*
 * The Biomechanical ToolKit
 * Copyright (c) 2009-2013, Arnaud Barré
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer.
 *     * 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.
 *     * Neither the name(s) of the copyright holders 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.
 */

#include "btkForcePlatformWrenchFilter.h"
#include "btkConvert.h"

namespace btk
{
  /**
   * @class ForcePlatformWrenchFilter btkForcePlatformWrenchFilter.h
   * @brief Calcule the wrench of the center of the force platform data, expressed in the global frame (by default).
   *
   * Based on the given collection of forceplate set in input, this filter transform the associated analog channels in forces and moments.
   * This transformation take into account the type of each force platform.
   *
   * You can use the method SetTransformToGlobalFrame() to have the wrench expressed in the frame of the force platform.
   *
   * @ingroup BTKBasicFilters
   */

  /**
   * @typedef ForcePlatformWrenchFilter::Pointer
   * Smart pointer associated with a ForcePlatformWrenchFilter object.
   */

  /**
   * @typedef ForcePlatformWrenchFilter::ConstPointer
   * Smart pointer associated with a const ForcePlatformWrenchFilter object.
   */

  /**
   * @fn static Pointer ForcePlatformWrenchFilter::New();
   * Creates a smart pointer associated with a ForcePlatformWrenchFilter object.
   */

  /**
   * @fn WrenchCollection::Pointer ForcePlatformWrenchFilter::GetInput()
   * Gets the input registered with this process.
   */

  /**
   * @fn void ForcePlatformWrenchFilter::SetInput(ForcePlatform::Pointer input)
   * Sets the input required with this process. This input is transformed in a collection force platforms with a single force platform.
   */

  /**
   * @fn void ForcePlatformWrenchFilter::SetInput(ForcePlatformCollection::Pointer input)
   * Sets the input required with this process.
   */

  /**
   * @fn PointCollection::Pointer ForcePlatformWrenchFilter::GetOutput()
   * Gets the output created with this process.
   */

  /**
   * Sets the flag to activate the transformation to the global frame.
   */
  void ForcePlatformWrenchFilter::SetTransformToGlobalFrame(bool activation)
  {
    if (this->m_GlobalTransformationActivated == activation)
      return;
    this->m_GlobalTransformationActivated = activation;
    this->Modified();
  };

  /**
   * @fn bool ForcePlatformWrenchFilter::GetTransformToGlobalFrame() const
   * Returns the activation flag for the transformation in the global frame.
   */

  /**
   * Constructor. Sets the number of inputs and outputs to 1.
   */
  ForcePlatformWrenchFilter::ForcePlatformWrenchFilter()
  : ProcessObject()
  {
    this->SetInputNumber(1);
    this->SetOutputNumber(1);
    this->m_GlobalTransformationActivated = true;
  };

  /**
   * @fn ForcePlatformCollection::Pointer ForcePlatformWrenchFilter::GetInput(int idx)
   * Returns the input at the index @a idx.
   */

  /**
   * @fn WrenchCollection::Pointer ForcePlatformWrenchFilter::GetOutput(int idx)
   * Returns the output at the index @a idx.
   */

  /**
   * Creates a WrenchCollection:Pointer object and return it as a DataObject::Pointer.
   */
  DataObject::Pointer ForcePlatformWrenchFilter::MakeOutput(int /* idx */)
  {
    return WrenchCollection::New();
  };

  /**
   * Generates the outputs' data.
   */
  void ForcePlatformWrenchFilter::GenerateData()
  {
    WrenchCollection::Pointer output = this->GetOutput();
    ForcePlatformCollection::Pointer input = this->GetInput();
    if (input.get() != 0)
    {
      int inc = 0;
      for (ForcePlatformCollection::ConstIterator it = input->Begin() ; it != input->End() ; ++it)
      {
        ++inc;
        if ((*it)->GetChannelNumber() == 0)
        {
          btkErrorMacro("Unexpected number of analog channels (0) for force platform #" + ToString(inc));
          continue;
        }
        int frameNumber = (*it)->GetChannel(0)->GetFrameNumber();
        Wrench::Pointer wrh;
        if (inc <= output->GetItemNumber())
        {
          wrh = output->GetItem(inc-1);
          wrh->SetFrameNumber(frameNumber);
          output->Modified();
        }
        else
        {
          wrh = Wrench::New(this->GetWrenchPrefix() + ToString(inc), frameNumber);
          output->InsertItem(wrh);
        }
        wrh->Modified();
        // Residuals
        wrh->GetPosition()->GetResiduals().setZero(frameNumber);
        wrh->GetForce()->GetResiduals().setZero(frameNumber);
        wrh->GetMoment()->GetResiduals().setZero(frameNumber);
        // Values
        switch((*it)->GetType())
        {
          // 6 channels
          case 1:
            wrh->GetForce()->GetValues().col(0) = (*it)->GetChannel(0)->GetValues();
            wrh->GetForce()->GetValues().col(1) = (*it)->GetChannel(1)->GetValues();
            wrh->GetForce()->GetValues().col(2) = (*it)->GetChannel(2)->GetValues();
            wrh->GetPosition()->GetValues().col(0) = (*it)->GetChannel(3)->GetValues();
            wrh->GetPosition()->GetValues().col(1) = (*it)->GetChannel(4)->GetValues();
            wrh->GetPosition()->GetValues().col(2).setZero();
            wrh->GetMoment()->GetValues().col(0).setZero();
            wrh->GetMoment()->GetValues().col(1).setZero();
            wrh->GetMoment()->GetValues().col(2) = (*it)->GetChannel(5)->GetValues();
            this->FinishTypeI(wrh, *it, inc);
            break;
          case 2:
          case 4:
          case 5:
            wrh->GetForce()->GetValues().col(0) = (*it)->GetChannel(0)->GetValues();
            wrh->GetForce()->GetValues().col(1) = (*it)->GetChannel(1)->GetValues();
            wrh->GetForce()->GetValues().col(2) = (*it)->GetChannel(2)->GetValues();
            wrh->GetMoment()->GetValues().col(0) = (*it)->GetChannel(3)->GetValues();
            wrh->GetMoment()->GetValues().col(1) = (*it)->GetChannel(4)->GetValues();
            wrh->GetMoment()->GetValues().col(2) = (*it)->GetChannel(5)->GetValues();
            this->FinishAMTI(wrh, *it, inc);
            break;
          case 3:
            // Fx
            wrh->GetForce()->GetValues().col(0) = (*it)->GetChannel(0)->GetValues() + (*it)->GetChannel(1)->GetValues();
            // Fy
            wrh->GetForce()->GetValues().col(1) = (*it)->GetChannel(2)->GetValues() + (*it)->GetChannel(3)->GetValues();
            // Fz
            wrh->GetForce()->GetValues().col(2) = (*it)->GetChannel(4)->GetValues() + (*it)->GetChannel(5)->GetValues() + (*it)->GetChannel(6)->GetValues() + (*it)->GetChannel(7)->GetValues();
            // Mx
            wrh->GetMoment()->GetValues().col(0) = (*it)->GetOrigin().y() * ((*it)->GetChannel(4)->GetValues() + (*it)->GetChannel(5)->GetValues() - (*it)->GetChannel(6)->GetValues() - (*it)->GetChannel(7)->GetValues());
            // My
            wrh->GetMoment()->GetValues().col(1) = (*it)->GetOrigin().x() * ((*it)->GetChannel(5)->GetValues() + (*it)->GetChannel(6)->GetValues() - (*it)->GetChannel(4)->GetValues() - (*it)->GetChannel(7)->GetValues());
            // Mz
            wrh->GetMoment()->GetValues().col(2) = (*it)->GetOrigin().y() * ((*it)->GetChannel(1)->GetValues() - (*it)->GetChannel(0)->GetValues()) + (*it)->GetOrigin().x() * ((*it)->GetChannel(2)->GetValues() - (*it)->GetChannel(3)->GetValues());
            this->FinishKistler(wrh, *it, inc);
            break;
          case 6:
            btkErrorMacro("Force Platform type 6 is not yet supported. Please, report this to the developers");
            break;
          case 7:
            btkErrorMacro("Force Platform type 7 is not yet supported. Please, report this to the developers");
            break;
          case 11:
            btkErrorMacro("Force Platform type 11 is not yet supported. Please, report this to the developers");
            break;
          case 12:
            btkErrorMacro("Force Platform type 12 is not yet supported. Please, report this to the developers");
            break;
          case 21:
            btkErrorMacro("Force Platform type 21 is not yet supported. Please, report this to the developers");
            break;
        }
        if (this->m_GlobalTransformationActivated)
          this->TransformToGlobal(wrh, (*it)->GetCorners());
      }
      output->SetItemNumber(input->GetItemNumber());
    }
  };

  /**
   * Finish the computation of the wrench for the force platform type I.
   * Because, it is force platform Type I, the position is not set to the origin, but measured to the COP. The moment must be corrected!
   */
  void ForcePlatformWrenchFilter::FinishTypeI(Wrench::Pointer wrh , ForcePlatform::Pointer /* fp */, int /* index */)
  {
    typedef Eigen::Array<double, Eigen::Dynamic, 1> Component;
    Component Fx = wrh->GetForce()->GetValues().col(0).array();
    Component Fy = wrh->GetForce()->GetValues().col(1).array();
    Component Fz = wrh->GetForce()->GetValues().col(2).array();
    Component Mx = wrh->GetMoment()->GetValues().col(0).array();
    Component My = wrh->GetMoment()->GetValues().col(1).array();
    Component Mz = wrh->GetMoment()->GetValues().col(2).array();
    Component Px = wrh->GetPosition()->GetValues().col(0).array();
    Component Py = wrh->GetPosition()->GetValues().col(1).array();
    Component Pz = wrh->GetPosition()->GetValues().col(2).array();
    Mx -= Fy * Pz - Py * Fz;
    My -= Fz * Px - Pz * Fx;
    Mz -= Fx * Py - Px * Fy;
    wrh->GetMoment()->GetValues().col(0) = Mx;
    wrh->GetMoment()->GetValues().col(1) = My;
    wrh->GetMoment()->GetValues().col(2) = Mz;
  };

  /**
   * Finish the computation of the wrench for the AMTI force platform (nothing to do).
   */
  void ForcePlatformWrenchFilter::FinishAMTI(Wrench::Pointer /* wrh */, ForcePlatform::Pointer /* fp */, int /* index */)
  {};

  /**
   * Finish the computation of the wrench for the Kistler force platform (nothing to do).
   */
  void ForcePlatformWrenchFilter::FinishKistler(Wrench::Pointer /* wrh */, ForcePlatform::Pointer /* fp */, int /* index */)
  {};
};

<|endoftext|>