<commit_before>/** \copyright
 * Copyright (c) 2015, Balazs Racz
 * 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.
 *
 * 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 HOLDER 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 MultiConfiguredPC.hxx
 *
 * Producer-Consumer class that uses CDI configuration and many GPIO pins to
 * export a multiple of configurable input or output pins.
 *
 * @author Balazs Racz
 * @date 28 Oct 2018
 */

#ifndef _OPENLCB_MULTICONFIGUREDPC_HXX_
#define _OPENLCB_MULTICONFIGUREDPC_HXX_

#include "openlcb/ConfigRepresentation.hxx"
#include "openlcb/EventHandlerTemplates.hxx"
#include "openlcb/RefreshLoop.hxx"
#include "utils/ConfigUpdateListener.hxx"
#include "utils/ConfigUpdateService.hxx"
#include "utils/format_utils.hxx"

namespace openlcb
{

static const char PC_ACTION_MAP[] =
    "<relation><property>0</property><value>Output</value></relation>"
    "<relation><property>1</property><value>Input</value></relation>";

CDI_GROUP(PCLineConfig);
/// Allows the user to assign a name for this output.
CDI_GROUP_ENTRY(description, StringConfigEntry<20>, //
    Name("Description"), Description("User name of this line."));

/// Specifies the event ID to set the output to ON.
CDI_GROUP_ENTRY(event_on, EventConfigEntry, //
    Name("Event On"),
    Description("This event ID will turn the output on / be produced when the "
                "input goes on."));
/// Specifies the event ID to set the output to OFF.
CDI_GROUP_ENTRY(event_off, EventConfigEntry, //
    Name("Event Off"),
    Description("This event ID will turn the output off / be produced when the "
                "input goes off."));
CDI_GROUP_END();

/// CDI Configuration for a @ref ConfiguredConsumer.
CDI_GROUP(PCConfig);
enum class ActionConfig : uint8_t
{
    OUTPUT = 0,
    INPUT = 1
};

CDI_GROUP_ENTRY(action, Uint8ConfigEntry, Default(1), MapValues(PC_ACTION_MAP),
    Name("Configuration"));

/// Configures the debounce parameter.
CDI_GROUP_ENTRY(debounce, Uint8ConfigEntry, Name("Debounce parameter"),
    Default(3),
    Description("Used for inputs only. Amount of time to wait for the input to "
                "stabilize before "
                "producing the event. Unit is 30 msec of time. Usually a value "
                "of 2-3 works well in a non-noisy environment. In high noise "
                "(train wheels for example) a setting between 8 -- 15 makes "
                "for a slower response time but a more stable "
                "signal.\nFormally, the parameter tells how many times of "
                "tries, each 30 msec apart, the input must have the same value "
                "in order for that value to be accepted and the event "
                "transition produced."),
    Default(3));

CDI_GROUP_ENTRY(unused, EmptyGroup<1>);
// We factor out the description, and event on/off to a separate group to give
// JMRI a chance to render the make turnout/make sensor buttons.
CDI_GROUP_ENTRY(pc, PCLineConfig);
CDI_GROUP_END();

class MultiConfiguredPC : public ConfigUpdateListener,
                          private SimpleEventHandler,
                          private Polling,
                          private Notifiable
{
public:
    typedef PCConfig config_entry_type;
    typedef QuiesceDebouncer debouncer_type;

    /// Usage: ```
    ///
    /// constexpr const Gpio *const kDirectGpio[] = {
    /// TDRV1_Pin::instance(), TDRV2_Pin::instance(),
    /// TDRV3_Pin::instance(), TDRV4_Pin::instance(),
    /// };
    /// openlcb::MultiConfiguredPC direct_pc(stack.node(),
    ///    kDirectGpio, ARRAYSIZE(kDirectGpio), cfg.seg().direct_pc());
    /// ```
    ///
    /// @param node is the OpenLCB node object from the stack.
    /// @param pins is the list of pins represented by the Gpio* object
    /// instances. Can be constant from FLASH space.
    /// @param size is the length of the list of pins array.
    /// @param config is the repeated group object from the configuration space
    /// that represents the locations of the events.
    template <unsigned N>
    __attribute__((noinline))
    MultiConfiguredPC(Node *node, const Gpio *const *pins, unsigned size,
        const RepeatedGroup<config_entry_type, N> &config)
        : node_(node)
        , pins_(pins)
        , size_(N)
        , offset_(config)
    {
        // Mismatched sizing of the GPIO array from the configuration array.
        HASSERT(size == N);
        ConfigUpdateService::instance()->register_update_listener(this);
        producedEvents_ = new EventId[size * 2];
        std::allocator<debouncer_type> alloc;
        debouncers_ = alloc.allocate(size_);
        for (unsigned i = 0; i < size_; ++i)
        {
            alloc.construct(debouncers_ + i, 3);
        }
    }

    ~MultiConfiguredPC()
    {
        do_unregister();
        ConfigUpdateService::instance()->unregister_update_listener(this);
        delete[] producedEvents_;
        std::allocator<debouncer_type> alloc;
        for (unsigned i = 0; i < size_; ++i)
        {
            alloc.destroy(debouncers_ + i);
        }
        alloc.deallocate(debouncers_, size_);
    }

    /// @return the instance to give to the RefreshLoop object.
    Polling *polling()
    {
        return this;
    }

    /// Call from the refresh loop.
    void poll_33hz(WriteHelper *helper, Notifiable *done) override
    {
        nextPinToPoll_ = 0;
        pollingHelper_ = helper;
        pollingDone_ = done;
        this->notify();
    }

    /// Asynchronous callback when the previous polling message has left via
    /// the bus. Used as a poor man's iterative state machine.
    void notify() override
    {
        for (; nextPinToPoll_ < size_; ++nextPinToPoll_)
        {
            auto i = nextPinToPoll_;
            if (pins_[i]->direction() == Gpio::Direction::OUTPUT)
            {
                continue;
            }
            if (debouncers_[i].update_state(pins_[i]->is_set()))
            {
                // Pin flipped.
                ++nextPinToPoll_; // avoid infinite loop.
                auto event = producedEvents_[2 * i +
                    (debouncers_[i].current_state() ? 1 : 0)];
                pollingHelper_->WriteAsync(node_, Defs::MTI_EVENT_REPORT,
                    WriteHelper::global(), eventid_to_buffer(event), this);
                return;
            }
        }
        pollingDone_->notify();
    }

    UpdateAction apply_configuration(
        int fd, bool initial_load, BarrierNotifiable *done) OVERRIDE
    {
        AutoNotify n(done);

        if (!initial_load)
        {
            // There is no way to figure out what the previously registered
            // eventid values were for the individual pins. Therefore we always
            // unregister everything and register them anew. It also causes us
            // to identify all. This is not a problem since apply_configuration
            // is coming from a user action.
            do_unregister();
        }
        RepeatedGroup<config_entry_type, UINT_MAX> grp_ref(offset_.offset());
        for (unsigned i = 0; i < size_; ++i)
        {
            const config_entry_type cfg_ref(grp_ref.entry(i));
            EventId cfg_event_on = cfg_ref.pc().event_on().read(fd);
            EventId cfg_event_off = cfg_ref.pc().event_off().read(fd);
            EventRegistry::instance()->register_handler(
                EventRegistryEntry(this, cfg_event_off, i * 2), 0);
            EventRegistry::instance()->register_handler(
                EventRegistryEntry(this, cfg_event_on, i * 2 + 1), 0);
            uint8_t action = cfg_ref.action().read(fd);
            if (action == (uint8_t)PCConfig::ActionConfig::OUTPUT)
            {
                pins_[i]->set_direction(Gpio::Direction::OUTPUT);
                producedEvents_[i * 2] = 0;
                producedEvents_[i * 2 + 1] = 0;
            }
            else
            {
                uint8_t param = cfg_ref.debounce().read(fd);
                pins_[i]->set_direction(Gpio::Direction::INPUT);
                debouncers_[i].reset_options(param);
                debouncers_[i].initialize(pins_[i]->read());
                producedEvents_[i * 2] = cfg_event_off;
                producedEvents_[i * 2 + 1] = cfg_event_on;
            }
        }
        return REINIT_NEEDED; // Causes events identify.
    }

    void factory_reset(int fd) OVERRIDE
    {
        RepeatedGroup<config_entry_type, UINT_MAX> grp_ref(offset_.offset());
        for (unsigned i = 0; i < size_; ++i)
        {
            grp_ref.entry(i).pc().description().write(fd, "");
            CDI_FACTORY_RESET(grp_ref.entry(i).action);
            CDI_FACTORY_RESET(grp_ref.entry(i).debounce);
        }
    }

    /// Factory reset helper function. Sets all names to something 1..N.
    /// @param fd pased on from factory reset argument.
    /// @param basename name of repeats.
    void factory_reset_names(int fd, const char *basename)
    {
        RepeatedGroup<config_entry_type, UINT_MAX> grp_ref(offset_.offset());
        for (unsigned i = 0; i < size_; ++i)
        {
            string v(basename);
            v.push_back(' ');
            char buf[10];
            unsigned_integer_to_buffer(i + 1, buf);
            v += buf;
            grp_ref.entry(i).pc().description().write(fd, v);
        }
    }

    void handle_identify_global(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->dst_node && event->dst_node != node_)
        {
            return;
        }
        unsigned pin = registry_entry.user_arg >> 1;
        if (pins_[pin]->direction() == Gpio::Direction::INPUT)
        {
            SendProducerIdentified(registry_entry, event, done);
        }
        else
        {
            SendConsumerIdentified(registry_entry, event, done);
        }
    }

    void handle_identify_consumer(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->event != registry_entry.event)
        {
            return;
        }
        unsigned pin = registry_entry.user_arg >> 1;
        if (pins_[pin]->direction() == Gpio::Direction::OUTPUT)
        {
            SendConsumerIdentified(registry_entry, event, done);
        }
    }

    void handle_identify_producer(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->event != registry_entry.event)
        {
            return;
        }
        unsigned pin = registry_entry.user_arg >> 1;
        if (pins_[pin]->direction() == Gpio::Direction::INPUT)
        {
            SendProducerIdentified(registry_entry, event, done);
        }
    }

    void handle_event_report(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->event != registry_entry.event)
        {
            return;
        }
        const Gpio *pin = pins_[registry_entry.user_arg >> 1];
        if (pin->direction() == Gpio::Direction::OUTPUT)
        {
            const bool is_on = (registry_entry.user_arg & 1);
            pin->write(is_on);
        }
    }

private:
    /// Removes registration of this event handler from the global event
    /// registry.
    void do_unregister()
    {
        EventRegistry::instance()->unregister_handler(this);
    }

    /// Sends out a ConsumerIdentified message for the given registration
    /// entry.
    void SendConsumerIdentified(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done)
    {
        Defs::MTI mti = Defs::MTI_CONSUMER_IDENTIFIED_VALID;
        unsigned b1 = pins_[registry_entry.user_arg >> 1]->is_set() ? 1 : 0;
        unsigned b2 = registry_entry.user_arg & 1; // on or off event?
        if (b1 ^ b2)
        {
            mti++; // INVALID
        }
        event->event_write_helper<3>()->WriteAsync(node_, mti,
            WriteHelper::global(), eventid_to_buffer(registry_entry.event),
            done->new_child());
    }

    /// Sends out a ProducerIdentified message for the given registration
    /// entry.
    void SendProducerIdentified(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done)
    {
        Defs::MTI mti = Defs::MTI_PRODUCER_IDENTIFIED_VALID;
        unsigned b1 = pins_[registry_entry.user_arg >> 1]->is_set() ? 1 : 0;
        unsigned b2 = registry_entry.user_arg & 1; // on or off event?
        if (b1 ^ b2)
        {
            mti++; // INVALID
        }
        event->event_write_helper<4>()->WriteAsync(node_, mti,
            WriteHelper::global(), eventid_to_buffer(registry_entry.event),
            done->new_child());
    }

    // Variables used for asynchronous state during the polling loop.
    /// Which pin to next check when polling.
    unsigned nextPinToPoll_;
    /// Write helper to use for producing messages during the polling loop.
    WriteHelper *pollingHelper_;
    /// Notifiable to call when the polling loop is done.
    Notifiable *pollingDone_;

    /// virtual node to export the consumer / producer on.
    Node *node_;
    /// Array of all GPIO pins to use. Externally owned.
    const Gpio *const *pins_;
    /// Number of GPIO pins to export.
    size_t size_;
    /// Offset in the configuration space for our configs.
    ConfigReference offset_;
    /// Event IDs shadowing from the config file for producing them. We own
    /// this memory.
    EventId *producedEvents_;
    /// One debouncer per pin, created for produced pins. We own this memory.
    debouncer_type *debouncers_;
};
}

#endif // _OPENLCB_MULTICONFIGUREDPC_HXX_
<commit_msg>Fixes compilation errors in bitproducerpc.<commit_after>/** \copyright
 * Copyright (c) 2015, Balazs Racz
 * 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.
 *
 * 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 HOLDER 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 MultiConfiguredPC.hxx
 *
 * Producer-Consumer class that uses CDI configuration and many GPIO pins to
 * export a multiple of configurable input or output pins.
 *
 * @author Balazs Racz
 * @date 28 Oct 2018
 */

#ifndef _OPENLCB_MULTICONFIGUREDPC_HXX_
#define _OPENLCB_MULTICONFIGUREDPC_HXX_

#include "openlcb/ConfigRepresentation.hxx"
#include "openlcb/EventHandlerTemplates.hxx"
#include "openlcb/RefreshLoop.hxx"
#include "utils/ConfigUpdateListener.hxx"
#include "utils/ConfigUpdateService.hxx"
#include "utils/format_utils.hxx"

namespace openlcb
{

static const char PC_ACTION_MAP[] =
    "<relation><property>0</property><value>Output</value></relation>"
    "<relation><property>1</property><value>Input</value></relation>";

CDI_GROUP(PCLineConfig);
/// Allows the user to assign a name for this output.
CDI_GROUP_ENTRY(description, StringConfigEntry<20>, //
    Name("Description"), Description("User name of this line."));

/// Specifies the event ID to set the output to ON.
CDI_GROUP_ENTRY(event_on, EventConfigEntry, //
    Name("Event On"),
    Description("This event ID will turn the output on / be produced when the "
                "input goes on."));
/// Specifies the event ID to set the output to OFF.
CDI_GROUP_ENTRY(event_off, EventConfigEntry, //
    Name("Event Off"),
    Description("This event ID will turn the output off / be produced when the "
                "input goes off."));
CDI_GROUP_END();

/// CDI Configuration for a @ref ConfiguredConsumer.
CDI_GROUP(PCConfig);
enum class ActionConfig : uint8_t
{
    OUTPUT = 0,
    INPUT = 1
};

CDI_GROUP_ENTRY(action, Uint8ConfigEntry, Default(1), MapValues(PC_ACTION_MAP),
    Name("Configuration"));

/// Configures the debounce parameter.
CDI_GROUP_ENTRY(debounce, Uint8ConfigEntry, Name("Debounce parameter"),
    Default(3),
    Description("Used for inputs only. Amount of time to wait for the input to "
                "stabilize before "
                "producing the event. Unit is 30 msec of time. Usually a value "
                "of 2-3 works well in a non-noisy environment. In high noise "
                "(train wheels for example) a setting between 8 -- 15 makes "
                "for a slower response time but a more stable "
                "signal.\nFormally, the parameter tells how many times of "
                "tries, each 30 msec apart, the input must have the same value "
                "in order for that value to be accepted and the event "
                "transition produced."),
    Default(3));

CDI_GROUP_ENTRY(unused, EmptyGroup<1>);
// We factor out the description, and event on/off to a separate group to give
// JMRI a chance to render the make turnout/make sensor buttons.
CDI_GROUP_ENTRY(pc, PCLineConfig);
CDI_GROUP_END();

class MultiConfiguredPC : public ConfigUpdateListener,
                          private SimpleEventHandler,
                          private Polling,
                          private Notifiable
{
public:
    typedef PCConfig config_entry_type;
    typedef QuiesceDebouncer debouncer_type;

    /// Usage: ```
    ///
    /// constexpr const Gpio *const kDirectGpio[] = {
    /// TDRV1_Pin::instance(), TDRV2_Pin::instance(),
    /// TDRV3_Pin::instance(), TDRV4_Pin::instance(),
    /// };
    /// openlcb::MultiConfiguredPC direct_pc(stack.node(),
    ///    kDirectGpio, ARRAYSIZE(kDirectGpio), cfg.seg().direct_pc());
    /// ```
    ///
    /// @param node is the OpenLCB node object from the stack.
    /// @param pins is the list of pins represented by the Gpio* object
    /// instances. Can be constant from FLASH space.
    /// @param size is the length of the list of pins array.
    /// @param config is the repeated group object from the configuration space
    /// that represents the locations of the events.
    template <unsigned N>
    __attribute__((noinline))
    MultiConfiguredPC(Node *node, const Gpio *const *pins, unsigned size,
        const RepeatedGroup<config_entry_type, N> &config)
        : node_(node)
        , pins_(pins)
        , size_(N)
        , offset_(config)
    {
        // Mismatched sizing of the GPIO array from the configuration array.
        HASSERT(size == N);
        ConfigUpdateService::instance()->register_update_listener(this);
        producedEvents_ = new EventId[size * 2];
        std::allocator<debouncer_type> alloc;
        debouncers_ = alloc.allocate(size_);
        for (unsigned i = 0; i < size_; ++i)
        {
            alloc.construct(debouncers_ + i, 3);
        }
    }

    ~MultiConfiguredPC()
    {
        do_unregister();
        ConfigUpdateService::instance()->unregister_update_listener(this);
        delete[] producedEvents_;
        std::allocator<debouncer_type> alloc;
        for (unsigned i = 0; i < size_; ++i)
        {
            alloc.destroy(debouncers_ + i);
        }
        alloc.deallocate(debouncers_, size_);
    }

    /// @return the instance to give to the RefreshLoop object.
    Polling *polling()
    {
        return this;
    }

    /// Call from the refresh loop.
    void poll_33hz(WriteHelper *helper, Notifiable *done) override
    {
        nextPinToPoll_ = 0;
        pollingHelper_ = helper;
        pollingDone_ = done;
        this->notify();
    }

    /// Asynchronous callback when the previous polling message has left via
    /// the bus. Used as a poor man's iterative state machine.
    void notify() override
    {
        for (; nextPinToPoll_ < size_; ++nextPinToPoll_)
        {
            auto i = nextPinToPoll_;
            if (pins_[i]->direction() == Gpio::Direction::DOUTPUT)
            {
                continue;
            }
            if (debouncers_[i].update_state(pins_[i]->is_set()))
            {
                // Pin flipped.
                ++nextPinToPoll_; // avoid infinite loop.
                auto event = producedEvents_[2 * i +
                    (debouncers_[i].current_state() ? 1 : 0)];
                pollingHelper_->WriteAsync(node_, Defs::MTI_EVENT_REPORT,
                    WriteHelper::global(), eventid_to_buffer(event), this);
                return;
            }
        }
        pollingDone_->notify();
    }

    UpdateAction apply_configuration(
        int fd, bool initial_load, BarrierNotifiable *done) OVERRIDE
    {
        AutoNotify n(done);

        if (!initial_load)
        {
            // There is no way to figure out what the previously registered
            // eventid values were for the individual pins. Therefore we always
            // unregister everything and register them anew. It also causes us
            // to identify all. This is not a problem since apply_configuration
            // is coming from a user action.
            do_unregister();
        }
        RepeatedGroup<config_entry_type, UINT_MAX> grp_ref(offset_.offset());
        for (unsigned i = 0; i < size_; ++i)
        {
            const config_entry_type cfg_ref(grp_ref.entry(i));
            EventId cfg_event_on = cfg_ref.pc().event_on().read(fd);
            EventId cfg_event_off = cfg_ref.pc().event_off().read(fd);
            EventRegistry::instance()->register_handler(
                EventRegistryEntry(this, cfg_event_off, i * 2), 0);
            EventRegistry::instance()->register_handler(
                EventRegistryEntry(this, cfg_event_on, i * 2 + 1), 0);
            uint8_t action = cfg_ref.action().read(fd);
            if (action == (uint8_t)PCConfig::ActionConfig::OUTPUT)
            {
                pins_[i]->set_direction(Gpio::Direction::DOUTPUT);
                producedEvents_[i * 2] = 0;
                producedEvents_[i * 2 + 1] = 0;
            }
            else
            {
                uint8_t param = cfg_ref.debounce().read(fd);
                pins_[i]->set_direction(Gpio::Direction::DINPUT);
                debouncers_[i].reset_options(param);
                debouncers_[i].initialize(pins_[i]->read());
                producedEvents_[i * 2] = cfg_event_off;
                producedEvents_[i * 2 + 1] = cfg_event_on;
            }
        }
        return REINIT_NEEDED; // Causes events identify.
    }

    void factory_reset(int fd) OVERRIDE
    {
        RepeatedGroup<config_entry_type, UINT_MAX> grp_ref(offset_.offset());
        for (unsigned i = 0; i < size_; ++i)
        {
            grp_ref.entry(i).pc().description().write(fd, "");
            CDI_FACTORY_RESET(grp_ref.entry(i).action);
            CDI_FACTORY_RESET(grp_ref.entry(i).debounce);
        }
    }

    /// Factory reset helper function. Sets all names to something 1..N.
    /// @param fd pased on from factory reset argument.
    /// @param basename name of repeats.
    void factory_reset_names(int fd, const char *basename)
    {
        RepeatedGroup<config_entry_type, UINT_MAX> grp_ref(offset_.offset());
        for (unsigned i = 0; i < size_; ++i)
        {
            string v(basename);
            v.push_back(' ');
            char buf[10];
            unsigned_integer_to_buffer(i + 1, buf);
            v += buf;
            grp_ref.entry(i).pc().description().write(fd, v);
        }
    }

    void handle_identify_global(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->dst_node && event->dst_node != node_)
        {
            return;
        }
        unsigned pin = registry_entry.user_arg >> 1;
        if (pins_[pin]->direction() == Gpio::Direction::DINPUT)
        {
            SendProducerIdentified(registry_entry, event, done);
        }
        else
        {
            SendConsumerIdentified(registry_entry, event, done);
        }
    }

    void handle_identify_consumer(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->event != registry_entry.event)
        {
            return;
        }
        unsigned pin = registry_entry.user_arg >> 1;
        if (pins_[pin]->direction() == Gpio::Direction::DOUTPUT)
        {
            SendConsumerIdentified(registry_entry, event, done);
        }
    }

    void handle_identify_producer(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->event != registry_entry.event)
        {
            return;
        }
        unsigned pin = registry_entry.user_arg >> 1;
        if (pins_[pin]->direction() == Gpio::Direction::DINPUT)
        {
            SendProducerIdentified(registry_entry, event, done);
        }
    }

    void handle_event_report(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done) override
    {
        AutoNotify an(done);
        if (event->event != registry_entry.event)
        {
            return;
        }
        const Gpio *pin = pins_[registry_entry.user_arg >> 1];
        if (pin->direction() == Gpio::Direction::DOUTPUT)
        {
            const bool is_on = (registry_entry.user_arg & 1);
            pin->write(is_on);
        }
    }

private:
    /// Removes registration of this event handler from the global event
    /// registry.
    void do_unregister()
    {
        EventRegistry::instance()->unregister_handler(this);
    }

    /// Sends out a ConsumerIdentified message for the given registration
    /// entry.
    void SendConsumerIdentified(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done)
    {
        Defs::MTI mti = Defs::MTI_CONSUMER_IDENTIFIED_VALID;
        unsigned b1 = pins_[registry_entry.user_arg >> 1]->is_set() ? 1 : 0;
        unsigned b2 = registry_entry.user_arg & 1; // on or off event?
        if (b1 ^ b2)
        {
            mti++; // INVALID
        }
        event->event_write_helper<3>()->WriteAsync(node_, mti,
            WriteHelper::global(), eventid_to_buffer(registry_entry.event),
            done->new_child());
    }

    /// Sends out a ProducerIdentified message for the given registration
    /// entry.
    void SendProducerIdentified(const EventRegistryEntry &registry_entry,
        EventReport *event, BarrierNotifiable *done)
    {
        Defs::MTI mti = Defs::MTI_PRODUCER_IDENTIFIED_VALID;
        unsigned b1 = pins_[registry_entry.user_arg >> 1]->is_set() ? 1 : 0;
        unsigned b2 = registry_entry.user_arg & 1; // on or off event?
        if (b1 ^ b2)
        {
            mti++; // INVALID
        }
        event->event_write_helper<4>()->WriteAsync(node_, mti,
            WriteHelper::global(), eventid_to_buffer(registry_entry.event),
            done->new_child());
    }

    // Variables used for asynchronous state during the polling loop.
    /// Which pin to next check when polling.
    unsigned nextPinToPoll_;
    /// Write helper to use for producing messages during the polling loop.
    WriteHelper *pollingHelper_;
    /// Notifiable to call when the polling loop is done.
    Notifiable *pollingDone_;

    /// virtual node to export the consumer / producer on.
    Node *node_;
    /// Array of all GPIO pins to use. Externally owned.
    const Gpio *const *pins_;
    /// Number of GPIO pins to export.
    size_t size_;
    /// Offset in the configuration space for our configs.
    ConfigReference offset_;
    /// Event IDs shadowing from the config file for producing them. We own
    /// this memory.
    EventId *producedEvents_;
    /// One debouncer per pin, created for produced pins. We own this memory.
    debouncer_type *debouncers_;
};
}

#endif // _OPENLCB_MULTICONFIGUREDPC_HXX_
<|endoftext|>


<commit_before>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.refractions.net
 *
 * Copyright (C) 2009-2011 Sandro Santilli <strk@keybit.net>
 * Copyright (C) 2005-2007 Refractions Research Inc.
 * Copyright (C) 2001-2002 Vivid Solutions Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 **********************************************************************
 *
 * Last port: operation/buffer/BufferOp.java r378 (JTS-1.12)
 *
 **********************************************************************/

#include <algorithm>
#include <cmath>

#include <geos/profiler.h>
#include <geos/operation/buffer/BufferOp.h>
#include <geos/operation/buffer/BufferBuilder.h>
#include <geos/geom/Geometry.h>
#include <geos/geom/GeometryFactory.h>
#include <geos/geom/PrecisionModel.h>

#include <geos/noding/ScaledNoder.h>

#include <geos/noding/snapround/MCIndexSnapRounder.h>
#include <geos/noding/snapround/MCIndexPointSnapper.h>

//FIXME: for temporary use, see other FIXME in file
#include <geos/algorithm/LineIntersector.h>
#include <geos/noding/MCIndexNoder.h>
#include <geos/noding/IntersectionAdder.h>

#ifndef GEOS_DEBUG
#define GEOS_DEBUG 0
#endif

//#define PROFILE 1

using namespace geos::noding;
using namespace geos::geom;

namespace geos {
namespace operation { // geos.operation
namespace buffer { // geos.operation.buffer

#if PROFILE
static Profiler *profiler = Profiler::instance();
#endif

namespace {

double OLDprecisionScaleFactor(const Geometry *g,
	double distance, int maxPrecisionDigits)
{
	const Envelope *env=g->getEnvelopeInternal();
	double envSize=(std::max)(env->getHeight(), env->getWidth());
	double expandByDistance=distance > 0.0 ? distance : 0.0;
	double bufEnvSize=envSize + 2 * expandByDistance;
	// the smallest power of 10 greater than the buffer envelope
	int bufEnvLog10=(int) (std::log(bufEnvSize) / std::log(10.0) + 1.0);
	int minUnitLog10=bufEnvLog10 - maxPrecisionDigits;
	// scale factor is inverse of min Unit size, so flip sign of exponent
	double scaleFactor=std::pow(10.0,-minUnitLog10);
	return scaleFactor;
}

} // anonymous namespace

/*private*/
double
BufferOp::precisionScaleFactor(const Geometry *g,
	double distance,
	int maxPrecisionDigits)
{
  const Envelope *env=g->getEnvelopeInternal();
  double envMax = std::max(
    std::max(fabs(env->getMaxX()), fabs(env->getMinX())),
    std::max(fabs(env->getMaxY()), fabs(env->getMinY()))
  );

  double expandByDistance = distance > 0.0 ? distance : 0.0;
  double bufEnvMax = envMax + 2 * expandByDistance;

  // the smallest power of 10 greater than the buffer envelope
  int bufEnvPrecisionDigits = (int) (std::log(bufEnvMax) / std::log(10) + 1.0);
  int minUnitLog10 = maxPrecisionDigits - bufEnvPrecisionDigits;

  double scaleFactor = std::pow(10.0, minUnitLog10);

  return scaleFactor;
}

/*public static*/
Geometry*
BufferOp::bufferOp(const Geometry *g, double distance,
		int quadrantSegments,
		int nEndCapStyle)
{
	BufferOp bufOp(g);
	bufOp.setQuadrantSegments(quadrantSegments);
	bufOp.setEndCapStyle(nEndCapStyle);
	return bufOp.getResultGeometry(distance);
}

/*public*/
Geometry*
BufferOp::getResultGeometry(double nDistance)
{
	distance=nDistance;
	computeGeometry();
	return resultGeometry;
}

/*private*/
void
BufferOp::computeGeometry()
{
#if GEOS_DEBUG
	std::cerr<<"BufferOp::computeGeometry: trying with original precision"<<std::endl;
#endif

	bufferOriginalPrecision();

	if (resultGeometry!=NULL) return;

#if GEOS_DEBUG
	std::cerr << "bufferOriginalPrecision failed (" << saveException.what() << "), trying with reduced precision"
	          << std::endl;
#endif

	const PrecisionModel& argPM = *(argGeom->getFactory()->getPrecisionModel());
	if ( argPM.getType() == PrecisionModel::FIXED )
		bufferFixedPrecision(argPM);
	else
		bufferReducedPrecision();
}

/*private*/
void
BufferOp::bufferReducedPrecision()
{

	// try and compute with decreasing precision
	for (int precDigits=MAX_PRECISION_DIGITS; precDigits >= 0; precDigits--)
	{
#if GEOS_DEBUG
		std::cerr<<"BufferOp::computeGeometry: trying with precDigits "<<precDigits<<std::endl;
#endif
		try {
			bufferReducedPrecision(precDigits);
		} catch (const util::TopologyException& ex) {
			saveException=ex;
			// don't propagate the exception - it will be detected by fact that resultGeometry is null
		}

		if (resultGeometry!=NULL) {
			// debug
			//if ( saveException ) std::cerr<<saveException->toString()<<std::endl;
			return;
		}
	}
	// tried everything - have to bail
	throw saveException;
}

/*private*/
void
BufferOp::bufferOriginalPrecision()
{
	BufferBuilder bufBuilder(bufParams);

	//std::cerr<<"computing with original precision"<<std::endl;
	try
	{
		resultGeometry=bufBuilder.buffer(argGeom, distance);
	}
	catch (const util::TopologyException& ex)
	{
		// don't propagate the exception - it will be detected by
		// fact that resultGeometry is null
		saveException=ex;

		//std::cerr<<ex->toString()<<std::endl;
	}
	//std::cerr<<"done"<<std::endl;
}

void
BufferOp::bufferReducedPrecision(int precisionDigits)
{
	double sizeBasedScaleFactor=precisionScaleFactor(argGeom, distance, precisionDigits);

#if GEOS_DEBUG
	std::cerr << "recomputing with precision scale factor = "
		<< sizeBasedScaleFactor
		<< std::endl;
#endif

	assert(sizeBasedScaleFactor>0);
	PrecisionModel fixedPM(sizeBasedScaleFactor);
	bufferFixedPrecision(fixedPM);
}

/*private*/
void
BufferOp::bufferFixedPrecision(const PrecisionModel& fixedPM)
{

	PrecisionModel pm(1.0); // fixed as well

#if 0 /* FIXME: MCIndexSnapRounder seems to be still bogus */
  snapround::MCIndexSnapRounder inoder(pm);
#else
  algorithm::LineIntersector li(&fixedPM);
  IntersectionAdder ia(li);
  MCIndexNoder inoder(&ia);
#endif

	ScaledNoder noder(inoder, fixedPM.getScale());

	BufferBuilder bufBuilder(bufParams);
	bufBuilder.setWorkingPrecisionModel(&fixedPM);

	bufBuilder.setNoder(&noder);

	// this may throw an exception, if robustness errors are encountered
	resultGeometry=bufBuilder.buffer(argGeom, distance);
}

} // namespace geos.operation.buffer
} // namespace geos.operation
} // namespace geos

<commit_msg>Forward port: Fix MSVC compilation of ambiguous log() call #506<commit_after>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.refractions.net
 *
 * Copyright (C) 2009-2011 Sandro Santilli <strk@keybit.net>
 * Copyright (C) 2005-2007 Refractions Research Inc.
 * Copyright (C) 2001-2002 Vivid Solutions Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 **********************************************************************
 *
 * Last port: operation/buffer/BufferOp.java r378 (JTS-1.12)
 *
 **********************************************************************/

#include <algorithm>
#include <cmath>

#include <geos/profiler.h>
#include <geos/operation/buffer/BufferOp.h>
#include <geos/operation/buffer/BufferBuilder.h>
#include <geos/geom/Geometry.h>
#include <geos/geom/GeometryFactory.h>
#include <geos/geom/PrecisionModel.h>

#include <geos/noding/ScaledNoder.h>

#include <geos/noding/snapround/MCIndexSnapRounder.h>
#include <geos/noding/snapround/MCIndexPointSnapper.h>

//FIXME: for temporary use, see other FIXME in file
#include <geos/algorithm/LineIntersector.h>
#include <geos/noding/MCIndexNoder.h>
#include <geos/noding/IntersectionAdder.h>

#ifndef GEOS_DEBUG
#define GEOS_DEBUG 0
#endif

//#define PROFILE 1

using namespace geos::noding;
using namespace geos::geom;

namespace geos {
namespace operation { // geos.operation
namespace buffer { // geos.operation.buffer

#if PROFILE
static Profiler *profiler = Profiler::instance();
#endif

namespace {

double OLDprecisionScaleFactor(const Geometry *g,
	double distance, int maxPrecisionDigits)
{
	const Envelope *env=g->getEnvelopeInternal();
	double envSize=(std::max)(env->getHeight(), env->getWidth());
	double expandByDistance=distance > 0.0 ? distance : 0.0;
	double bufEnvSize=envSize + 2 * expandByDistance;
	// the smallest power of 10 greater than the buffer envelope
	int bufEnvLog10=(int) (std::log(bufEnvSize) / std::log(10.0) + 1.0);
	int minUnitLog10=bufEnvLog10 - maxPrecisionDigits;
	// scale factor is inverse of min Unit size, so flip sign of exponent
	double scaleFactor=std::pow(10.0,-minUnitLog10);
	return scaleFactor;
}

} // anonymous namespace

/*private*/
double
BufferOp::precisionScaleFactor(const Geometry *g,
	double distance,
	int maxPrecisionDigits)
{
  const Envelope *env=g->getEnvelopeInternal();
  double envMax = std::max(
    std::max(fabs(env->getMaxX()), fabs(env->getMinX())),
    std::max(fabs(env->getMaxY()), fabs(env->getMinY()))
  );

  double expandByDistance = distance > 0.0 ? distance : 0.0;
  double bufEnvMax = envMax + 2 * expandByDistance;

  // the smallest power of 10 greater than the buffer envelope
  int bufEnvPrecisionDigits = (int) (std::log(bufEnvMax) / std::log(10.0) + 1.0);
  int minUnitLog10 = maxPrecisionDigits - bufEnvPrecisionDigits;

  double scaleFactor = std::pow(10.0, minUnitLog10);

  return scaleFactor;
}

/*public static*/
Geometry*
BufferOp::bufferOp(const Geometry *g, double distance,
		int quadrantSegments,
		int nEndCapStyle)
{
	BufferOp bufOp(g);
	bufOp.setQuadrantSegments(quadrantSegments);
	bufOp.setEndCapStyle(nEndCapStyle);
	return bufOp.getResultGeometry(distance);
}

/*public*/
Geometry*
BufferOp::getResultGeometry(double nDistance)
{
	distance=nDistance;
	computeGeometry();
	return resultGeometry;
}

/*private*/
void
BufferOp::computeGeometry()
{
#if GEOS_DEBUG
	std::cerr<<"BufferOp::computeGeometry: trying with original precision"<<std::endl;
#endif

	bufferOriginalPrecision();

	if (resultGeometry!=NULL) return;

#if GEOS_DEBUG
	std::cerr << "bufferOriginalPrecision failed (" << saveException.what() << "), trying with reduced precision"
	          << std::endl;
#endif

	const PrecisionModel& argPM = *(argGeom->getFactory()->getPrecisionModel());
	if ( argPM.getType() == PrecisionModel::FIXED )
		bufferFixedPrecision(argPM);
	else
		bufferReducedPrecision();
}

/*private*/
void
BufferOp::bufferReducedPrecision()
{

	// try and compute with decreasing precision
	for (int precDigits=MAX_PRECISION_DIGITS; precDigits >= 0; precDigits--)
	{
#if GEOS_DEBUG
		std::cerr<<"BufferOp::computeGeometry: trying with precDigits "<<precDigits<<std::endl;
#endif
		try {
			bufferReducedPrecision(precDigits);
		} catch (const util::TopologyException& ex) {
			saveException=ex;
			// don't propagate the exception - it will be detected by fact that resultGeometry is null
		}

		if (resultGeometry!=NULL) {
			// debug
			//if ( saveException ) std::cerr<<saveException->toString()<<std::endl;
			return;
		}
	}
	// tried everything - have to bail
	throw saveException;
}

/*private*/
void
BufferOp::bufferOriginalPrecision()
{
	BufferBuilder bufBuilder(bufParams);

	//std::cerr<<"computing with original precision"<<std::endl;
	try
	{
		resultGeometry=bufBuilder.buffer(argGeom, distance);
	}
	catch (const util::TopologyException& ex)
	{
		// don't propagate the exception - it will be detected by
		// fact that resultGeometry is null
		saveException=ex;

		//std::cerr<<ex->toString()<<std::endl;
	}
	//std::cerr<<"done"<<std::endl;
}

void
BufferOp::bufferReducedPrecision(int precisionDigits)
{
	double sizeBasedScaleFactor=precisionScaleFactor(argGeom, distance, precisionDigits);

#if GEOS_DEBUG
	std::cerr << "recomputing with precision scale factor = "
		<< sizeBasedScaleFactor
		<< std::endl;
#endif

	assert(sizeBasedScaleFactor>0);
	PrecisionModel fixedPM(sizeBasedScaleFactor);
	bufferFixedPrecision(fixedPM);
}

/*private*/
void
BufferOp::bufferFixedPrecision(const PrecisionModel& fixedPM)
{

	PrecisionModel pm(1.0); // fixed as well

#if 0 /* FIXME: MCIndexSnapRounder seems to be still bogus */
  snapround::MCIndexSnapRounder inoder(pm);
#else
  algorithm::LineIntersector li(&fixedPM);
  IntersectionAdder ia(li);
  MCIndexNoder inoder(&ia);
#endif

	ScaledNoder noder(inoder, fixedPM.getScale());

	BufferBuilder bufBuilder(bufParams);
	bufBuilder.setWorkingPrecisionModel(&fixedPM);

	bufBuilder.setNoder(&noder);

	// this may throw an exception, if robustness errors are encountered
	resultGeometry=bufBuilder.buffer(argGeom, distance);
}

} // namespace geos.operation.buffer
} // namespace geos.operation
} // namespace geos

<|endoftext|>


<commit_before>//
// MIT License
//
// Copyright 2017-2018
//
// 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.
//
#pragma once

#include <cstdint>

//
// CPU reference
// http://e-tradition.net/bytes/6502/6502_instruction_set.html
//
namespace jones {

  using r8_t = uint8_t;
  using r16_t = uint16_t;

  enum class register_t {

    // program counter (16 bit)
    PC,

    // accumulator (8 bit)
    AC,

    // X register (8 bit)
    X,

    // Y register (8 bit)
    Y,

    // status register (8 bit)
    SR,

    // stack pointer (8 bit)
    SP
  };
}
<commit_msg>adding register traits which will define traits for register type<commit_after>//
// MIT License
//
// Copyright 2017-2018
//
// 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.
//
#pragma once

#include <cstdint>

//
// CPU reference
// http://e-tradition.net/bytes/6502/6502_instruction_set.html
//
namespace jones {

  using r8_t = uint8_t;

  using r16_t = uint16_t;

  enum class register_t {

    // program counter (16 bit)
    PC,

    // accumulator (8 bit)
    AC,

    // X register (8 bit)
    X,

    // Y register (8 bit)
    Y,

    // status register (8 bit)
    SR,

    // stack pointer (8 bit)
    SP
  };

  template<register_t R>
  struct register_traits {
    typedef r8_t type;
    uint8_t length = sizeof(type);
  };

  template<>
  struct register_traits<register_t::PC> {
    typedef r16_t type;
    uint8_t length = sizeof(type);
  };
}
<|endoftext|>


<commit_before>/** @file
    @brief Implementation

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// 	http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Internal Includes
#include <osvr/Common/IPCRingBuffer.h>
#include "IPCRingBufferResults.h"
#include "IPCRingBufferSharedObjects.h"
#include "SharedMemory.h"
#include "SharedMemoryObjectWithMutex.h"
#include <osvr/Util/ImagingReportTypesC.h>
#include <osvr/Util/Verbosity.h>

// Library/third-party includes
#include <boost/version.hpp>

// Standard includes
#include <stdexcept>
#include <utility>
#include <type_traits>

namespace osvr {
namespace common {
#define OSVR_SHM_VERBOSE(X) OSVR_DEV_VERBOSE("IPCRingBuffer: " << X)

    /// @brief the ABI level: this must be bumped if the layout of any
    /// shared-memory objects (Bookkeeping, ElementData) changes, if Boost
    /// Interprocess changes affect the utilized ABI, or if other changes occur
    /// that would interfere with communication.
    static IPCRingBuffer::abi_level_type SHM_SOURCE_ABI_LEVEL = 0;

/// Some tests that can be automated for ensuring validity of the ABI level
/// number.
/// The base boost version test has been moved exclusively to CMake, to error
/// out earlier.
#if (BOOST_VERSION > 106000)
#error                                                                         \
    "Using an untested Boost version - inspect the Boost Interprocess release notes/changelog to see if any ABI breaks affect us."
#endif

#ifdef _WIN32
#if (BOOST_VERSION < 105400)
#error                                                                         \
    "Boost Interprocess pre-1.54 on Win32 is ABI-incompatible with newer Boost due to changed bootstamp function."
#endif
#else // !_WIN32
// No obvious ABI breaks through 1.58 seem to apply to us on non-Windows
// platforms
#endif

    static_assert(std::is_same<IPCRingBuffer::BackendType,
                               ipc::SharedMemoryBackendType>::value,
                  "The typedefs IPCRingBuffer::BackendType and "
                  "ipc::SharedMemoryBackendType must remain in sync!");
    static_assert(
        std::is_same<IPCRingBuffer::value_type, OSVR_ImageBufferElement>::value,
        "The ring buffer's individual byte type must match the image buffer "
        "element type.");

    namespace bip = boost::interprocess;
    namespace {
        typedef OSVR_ImageBufferElement BufferType;

        template <typename T, typename ManagedMemory>
        using ipc_deleter_type =
            typename ManagedMemory::template deleter<T>::type;

        typedef IPCRingBuffer::sequence_type sequence_type;

        /// @brief Destroys the "unique_instance" of a given type in a managed
        /// memory segment. If there isn't an instance, this is a no-op.
        template <typename T, typename ManagedMemory>
        inline void destroy_unique_instance(ManagedMemory &shm) {
            auto result = shm.template find<T>(bip::unique_instance);
            if (result.first) {
                shm.template destroy<T>(bip::unique_instance);
            }
        }
    } // namespace

    namespace {

        static size_t computeRequiredSpace(IPCRingBuffer::Options const &opts) {
            size_t alignedEntrySize = opts.getEntrySize() + opts.getAlignment();
            size_t dataSize = alignedEntrySize * (opts.getEntries() + 1);
            // Give 33% overhead on the raw bookkeeping data
            static const size_t BOOKKEEPING_SIZE =
                (sizeof(detail::Bookkeeping) +
                 (sizeof(detail::ElementData) * opts.getEntries())) *
                4 / 3;
            return dataSize + BOOKKEEPING_SIZE;
        }

        class SharedMemorySegmentHolder {
          public:
            SharedMemorySegmentHolder() : m_bookkeeping(nullptr) {}
            virtual ~SharedMemorySegmentHolder(){};

            detail::Bookkeeping *getBookkeeping() { return m_bookkeeping; }

            virtual uint64_t getSize() const = 0;
            virtual uint64_t getFreeMemory() const = 0;

          protected:
            detail::Bookkeeping *m_bookkeeping;
        };

        template <typename ManagedMemory>
        class SegmentHolderBase : public SharedMemorySegmentHolder {
          public:
            typedef ManagedMemory managed_memory_type;

            virtual uint64_t getSize() const { return m_shm->get_size(); }
            virtual uint64_t getFreeMemory() const {
                return m_shm->get_free_memory();
            }

          protected:
            unique_ptr<managed_memory_type> m_shm;
        };
        template <typename ManagedMemory>
        class ServerSharedMemorySegmentHolder
            : public SegmentHolderBase<ManagedMemory> {
          public:
            typedef SegmentHolderBase<ManagedMemory> Base;
            ServerSharedMemorySegmentHolder(IPCRingBuffer::Options const &opts)
                : m_name(opts.getName()) {
                OSVR_SHM_VERBOSE("Creating segment, name "
                                 << opts.getName() << ", size "
                                 << computeRequiredSpace(opts));
                try {
                    removeSharedMemory();
                    /// @todo Some shared memory types (specifically, Windows),
                    /// don't have a remove, so we should re-open.
                    Base::m_shm.reset(new ManagedMemory(
                        bip::create_only, opts.getName().c_str(),
                        computeRequiredSpace(opts)));
                } catch (bip::interprocess_exception &e) {
                    OSVR_SHM_VERBOSE("Failed to create shared memory segment "
                                     << opts.getName()
                                     << " with exception: " << e.what());
                    return;
                }
                // detail::Bookkeeping::destroy(*Base::m_shm);
                Base::m_bookkeeping =
                    detail::Bookkeeping::construct(*Base::m_shm, opts);
            }

            virtual ~ServerSharedMemorySegmentHolder() {
                detail::Bookkeeping::destroy(*Base::m_shm);
                removeSharedMemory();
            }

            void removeSharedMemory() {
                ipc::device_type<ManagedMemory>::remove(m_name.c_str());
            }

          private:
            std::string m_name;
        };

        template <typename ManagedMemory>
        class ClientSharedMemorySegmentHolder
            : public SegmentHolderBase<ManagedMemory> {
          public:
            typedef SegmentHolderBase<ManagedMemory> Base;
            ClientSharedMemorySegmentHolder(
                IPCRingBuffer::Options const &opts) {
                OSVR_SHM_VERBOSE("Finding segment, name " << opts.getName());
                try {
                    Base::m_shm.reset(new ManagedMemory(
                        bip::open_only, opts.getName().c_str()));
                } catch (bip::interprocess_exception &e) {
                    OSVR_SHM_VERBOSE("Failed to open shared memory segment "
                                     << opts.getName()
                                     << " with exception: " << e.what());
                    return;
                }
                Base::m_bookkeeping = detail::Bookkeeping::find(*Base::m_shm);
            }

            virtual ~ClientSharedMemorySegmentHolder() {}

          private:
        };

        /// @brief Factory function for constructing a memory segment holder.
        template <typename ManagedMemory>
        inline unique_ptr<SharedMemorySegmentHolder>
        constructMemorySegment(IPCRingBuffer::Options const &opts,
                               bool doCreate) {
            unique_ptr<SharedMemorySegmentHolder> ret;
            if (doCreate) {
                ret.reset(
                    new ServerSharedMemorySegmentHolder<ManagedMemory>(opts));
            } else {
                ret.reset(
                    new ClientSharedMemorySegmentHolder<ManagedMemory>(opts));
            }
            if (nullptr == ret->getBookkeeping()) {
                ret.reset();
            } else {
                OSVR_SHM_VERBOSE("size: " << ret->getSize() << ", free: "
                                          << ret->getFreeMemory());
            }
            return ret;
        }
    } // namespace

    IPCRingBuffer::BufferWriteProxy::BufferWriteProxy(
        detail::IPCPutResultPtr &&data, IPCRingBufferPtr &&shm)
        : m_buf(nullptr), m_seq(0), m_data(std::move(data)) {
        if (m_data) {
            m_buf = m_data->buffer;
            m_seq = m_data->seq;
            m_data->shm = std::move(shm);
        }
    }

    IPCRingBuffer::BufferReadProxy::BufferReadProxy(
        detail::IPCGetResultPtr &&data, IPCRingBufferPtr &&shm)
        : m_buf(nullptr), m_seq(0), m_data(std::move(data)) {
        if (nullptr != m_data) {
            m_buf = m_data->buffer;
            m_seq = m_data->seq;
            m_data->shm = std::move(shm);
        }
    }

    IPCRingBuffer::smart_pointer_type
    IPCRingBuffer::BufferReadProxy::getBufferSmartPointer() const {
        return smart_pointer_type(m_data, m_buf);
    }

    IPCRingBuffer::Options::Options()
        : m_shmBackend(ipc::DEFAULT_MANAGED_SHM_ID) {}

    IPCRingBuffer::Options::Options(std::string const &name)
        : m_name(ipc::make_name_safe(name)),
          m_shmBackend(ipc::DEFAULT_MANAGED_SHM_ID) {}

    IPCRingBuffer::Options::Options(std::string const &name,
                                    BackendType backend)
        : m_name(ipc::make_name_safe(name)), m_shmBackend(backend) {}

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setName(std::string const &name) {
        m_name = ipc::make_name_safe(name);
        return *this;
    }

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setAlignment(alignment_type alignment) {
        /// @todo ensure power of 2
        m_alignment = alignment;
        return *this;
    }

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setEntries(entry_count_type entries) {
        m_entries = entries;
        return *this;
    }

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setEntrySize(entry_size_type entrySize) {
        m_entrySize = entrySize;
        return *this;
    }
    class IPCRingBuffer::Impl {
      public:
        Impl(unique_ptr<SharedMemorySegmentHolder> &&segment,
             Options const &opts)
            : m_seg(std::move(segment)), m_bookkeeping(nullptr), m_opts(opts) {
            m_bookkeeping = m_seg->getBookkeeping();
            m_opts.setEntries(m_bookkeeping->getCapacity());
            m_opts.setEntrySize(m_bookkeeping->getBufferLength());
        }

        detail::IPCPutResultPtr put() {
            return m_bookkeeping->produceElement();
        }

        detail::IPCGetResultPtr get(sequence_type num) {
            detail::IPCGetResultPtr ret;
            auto boundsLock = m_bookkeeping->getSharableLock();
            auto elt = m_bookkeeping->getBySequenceNumber(num, boundsLock);
            if (nullptr != elt) {
                auto readerLock = elt->getSharableLock();
                auto buf = elt->getBuf(readerLock);
                /// The nullptr will be filled in by the main object.
                ret.reset(new detail::IPCGetResult{buf, std::move(readerLock),
                                                   num, nullptr});
            }
            return ret;
        }

        detail::IPCGetResultPtr getLatest() {
            detail::IPCGetResultPtr ret;
            auto boundsLock = m_bookkeeping->getSharableLock();
            auto elt = m_bookkeeping->back(boundsLock);
            if (nullptr != elt) {
                auto readerLock = elt->getSharableLock();
                auto buf = elt->getBuf(readerLock);
                /// The nullptr will be filled in by the main object.
                ret.reset(new detail::IPCGetResult{
                    buf, std::move(readerLock),
                    m_bookkeeping->backSequenceNumber(boundsLock), nullptr});
            }
            return ret;
        }

        Options const &getOpts() const { return m_opts; }

      private:
        unique_ptr<SharedMemorySegmentHolder> m_seg;
        detail::Bookkeeping *m_bookkeeping;

        Options m_opts;
    };

    IPCRingBufferPtr IPCRingBuffer::m_constructorHelper(Options const &opts,
                                                        bool doCreate) {

        IPCRingBufferPtr ret;
        unique_ptr<SharedMemorySegmentHolder> segment;

        switch (opts.getBackend()) {

        case ipc::BASIC_MANAGED_SHM_ID:
            segment =
                constructMemorySegment<ipc::basic_managed_shm>(opts, doCreate);
            break;

#ifdef OSVR_HAVE_WINDOWS_SHM
        case ipc::WINDOWS_MANAGED_SHM_ID:
            segment = constructMemorySegment<ipc::windows_managed_shm>(
                opts, doCreate);
            break;
#endif

#ifdef OSVR_HAVE_XSI_SHM
        case ipc::SYSV_MANAGED_SHM_ID:
            segment =
                constructMemorySegment<ipc::sysv_managed_shm>(opts, doCreate);
            break;
#endif

        default:
            OSVR_SHM_VERBOSE("Unsupported/unrecognized shared memory backend: "
                             << int(opts.getBackend()));
            break;
        }

        if (!segment) {
            return ret;
        }
        unique_ptr<Impl> impl(new Impl(std::move(segment), opts));
        ret.reset(new IPCRingBuffer(std::move(impl)));
        return ret;
    }

    IPCRingBuffer::abi_level_type IPCRingBuffer::getABILevel() {
        return SHM_SOURCE_ABI_LEVEL;
    }

    IPCRingBufferPtr IPCRingBuffer::create(Options const &opts) {
        return m_constructorHelper(opts, true);
    }
    IPCRingBufferPtr IPCRingBuffer::find(Options const &opts) {
        return m_constructorHelper(opts, false);
    }

    IPCRingBuffer::IPCRingBuffer(unique_ptr<Impl> &&impl)
        : m_impl(std::move(impl)) {}

    IPCRingBuffer::~IPCRingBuffer() {}

    IPCRingBuffer::BackendType IPCRingBuffer::getBackend() const {
        return m_impl->getOpts().getBackend();
    }

    std::string const &IPCRingBuffer::getName() const {
        return m_impl->getOpts().getName();
    }

    uint32_t IPCRingBuffer::getEntrySize() const {
        return m_impl->getOpts().getEntrySize();
    }

    uint16_t IPCRingBuffer::getEntries() const {
        return m_impl->getOpts().getEntries();
    }

    IPCRingBuffer::BufferWriteProxy IPCRingBuffer::put() {
        return BufferWriteProxy(m_impl->put(), shared_from_this());
    }

    IPCRingBuffer::sequence_type IPCRingBuffer::put(pointer_to_const_type data,
                                                    size_t len) {
        auto proxy = put();
        std::memcpy(proxy.get(), data, len);
        return proxy.getSequenceNumber();
    }

    IPCRingBuffer::BufferReadProxy IPCRingBuffer::get(sequence_type num) {
        return BufferReadProxy(m_impl->get(num), shared_from_this());
    }

    IPCRingBuffer::BufferReadProxy IPCRingBuffer::getLatest() {
        return BufferReadProxy(m_impl->getLatest(), shared_from_this());
    }

} // namespace common
} // namespace osvr
<commit_msg>Port IPCRingBuffer verbosity to logger.<commit_after>/** @file
    @brief Implementation

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// 	http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Internal Includes
#include "IPCRingBufferResults.h"
#include "IPCRingBufferSharedObjects.h"
#include "SharedMemory.h"
#include "SharedMemoryObjectWithMutex.h"
#include <osvr/Common/IPCRingBuffer.h>
#include <osvr/Util/ImagingReportTypesC.h>
#include <osvr/Util/Log.h>

// Library/third-party includes
#include <boost/version.hpp>

// Standard includes
#include <stdexcept>
#include <type_traits>
#include <utility>

namespace osvr {
namespace common {
    /// Grab a logger for IPCRingBuffer verbosity and errors.
    static util::log::Logger &getIPCRingBufferLogger() {
        static util::log::LoggerPtr logger =
            util::log::make_logger("IPCRingBuffer");
        return *logger;
    }

    /// @brief the ABI level: this must be bumped if the layout of any
    /// shared-memory objects (Bookkeeping, ElementData) changes, if Boost
    /// Interprocess changes affect the utilized ABI, or if other changes occur
    /// that would interfere with communication.
    static IPCRingBuffer::abi_level_type SHM_SOURCE_ABI_LEVEL = 0;

/// Some tests that can be automated for ensuring validity of the ABI level
/// number.
/// The base boost version test has been moved exclusively to CMake, to error
/// out earlier.
#if (BOOST_VERSION > 106000)
#error                                                                         \
    "Using an untested Boost version - inspect the Boost Interprocess release notes/changelog to see if any ABI breaks affect us."
#endif

#ifdef _WIN32
#if (BOOST_VERSION < 105400)
#error                                                                         \
    "Boost Interprocess pre-1.54 on Win32 is ABI-incompatible with newer Boost due to changed bootstamp function."
#endif
#else // !_WIN32
// No obvious ABI breaks through 1.58 seem to apply to us on non-Windows
// platforms
#endif

    static_assert(std::is_same<IPCRingBuffer::BackendType,
                               ipc::SharedMemoryBackendType>::value,
                  "The typedefs IPCRingBuffer::BackendType and "
                  "ipc::SharedMemoryBackendType must remain in sync!");
    static_assert(
        std::is_same<IPCRingBuffer::value_type, OSVR_ImageBufferElement>::value,
        "The ring buffer's individual byte type must match the image buffer "
        "element type.");

    namespace bip = boost::interprocess;
    namespace {
        typedef OSVR_ImageBufferElement BufferType;

        template <typename T, typename ManagedMemory>
        using ipc_deleter_type =
            typename ManagedMemory::template deleter<T>::type;

        typedef IPCRingBuffer::sequence_type sequence_type;

        /// @brief Destroys the "unique_instance" of a given type in a managed
        /// memory segment. If there isn't an instance, this is a no-op.
        template <typename T, typename ManagedMemory>
        inline void destroy_unique_instance(ManagedMemory &shm) {
            auto result = shm.template find<T>(bip::unique_instance);
            if (result.first) {
                shm.template destroy<T>(bip::unique_instance);
            }
        }
    } // namespace

    namespace {

        static size_t computeRequiredSpace(IPCRingBuffer::Options const &opts) {
            size_t alignedEntrySize = opts.getEntrySize() + opts.getAlignment();
            size_t dataSize = alignedEntrySize * (opts.getEntries() + 1);
            // Give 33% overhead on the raw bookkeeping data
            static const size_t BOOKKEEPING_SIZE =
                (sizeof(detail::Bookkeeping) +
                 (sizeof(detail::ElementData) * opts.getEntries())) *
                4 / 3;
            return dataSize + BOOKKEEPING_SIZE;
        }

        class SharedMemorySegmentHolder {
          public:
            SharedMemorySegmentHolder() : m_bookkeeping(nullptr) {}
            virtual ~SharedMemorySegmentHolder(){};

            detail::Bookkeeping *getBookkeeping() { return m_bookkeeping; }

            virtual uint64_t getSize() const = 0;
            virtual uint64_t getFreeMemory() const = 0;

          protected:
            detail::Bookkeeping *m_bookkeeping;
        };

        template <typename ManagedMemory>
        class SegmentHolderBase : public SharedMemorySegmentHolder {
          public:
            typedef ManagedMemory managed_memory_type;

            virtual uint64_t getSize() const { return m_shm->get_size(); }
            virtual uint64_t getFreeMemory() const {
                return m_shm->get_free_memory();
            }

          protected:
            unique_ptr<managed_memory_type> m_shm;
        };
        template <typename ManagedMemory>
        class ServerSharedMemorySegmentHolder
            : public SegmentHolderBase<ManagedMemory> {
          public:
            typedef SegmentHolderBase<ManagedMemory> Base;
            ServerSharedMemorySegmentHolder(IPCRingBuffer::Options const &opts)
                : m_name(opts.getName()) {
                getIPCRingBufferLogger().debug() << "Creating segment, name "
                                                 << opts.getName() << ", size "
                                                 << computeRequiredSpace(opts);
                try {
                    removeSharedMemory();
                    /// @todo Some shared memory types (specifically, Windows),
                    /// don't have a remove, so we should re-open.
                    Base::m_shm.reset(new ManagedMemory(
                        bip::create_only, opts.getName().c_str(),
                        computeRequiredSpace(opts)));
                } catch (bip::interprocess_exception &e) {
                    getIPCRingBufferLogger().error()
                        << "Failed to create shared memory segment "
                        << opts.getName() << " with exception: " << e.what();
                    return;
                }
                // detail::Bookkeeping::destroy(*Base::m_shm);
                Base::m_bookkeeping =
                    detail::Bookkeeping::construct(*Base::m_shm, opts);
            }

            virtual ~ServerSharedMemorySegmentHolder() {
                detail::Bookkeeping::destroy(*Base::m_shm);
                removeSharedMemory();
            }

            void removeSharedMemory() {
                ipc::device_type<ManagedMemory>::remove(m_name.c_str());
            }

          private:
            std::string m_name;
        };

        template <typename ManagedMemory>
        class ClientSharedMemorySegmentHolder
            : public SegmentHolderBase<ManagedMemory> {
          public:
            typedef SegmentHolderBase<ManagedMemory> Base;
            ClientSharedMemorySegmentHolder(
                IPCRingBuffer::Options const &opts) {
                getIPCRingBufferLogger().debug() << "Finding segment, name "
                                                 << opts.getName();
                try {
                    Base::m_shm.reset(new ManagedMemory(
                        bip::open_only, opts.getName().c_str()));
                } catch (bip::interprocess_exception &e) {
                    getIPCRingBufferLogger().error()
                        << "Failed to open shared memory segment "
                        << opts.getName() << " with exception: " << e.what();
                    return;
                }
                Base::m_bookkeeping = detail::Bookkeeping::find(*Base::m_shm);
            }

            virtual ~ClientSharedMemorySegmentHolder() {}

          private:
        };

        /// @brief Factory function for constructing a memory segment holder.
        template <typename ManagedMemory>
        inline unique_ptr<SharedMemorySegmentHolder>
        constructMemorySegment(IPCRingBuffer::Options const &opts,
                               bool doCreate) {
            unique_ptr<SharedMemorySegmentHolder> ret;
            if (doCreate) {
                ret.reset(
                    new ServerSharedMemorySegmentHolder<ManagedMemory>(opts));
            } else {
                ret.reset(
                    new ClientSharedMemorySegmentHolder<ManagedMemory>(opts));
            }
            if (nullptr == ret->getBookkeeping()) {
                ret.reset();
            } else {
                getIPCRingBufferLogger().debug()
                    << "size: " << ret->getSize()
                    << ", free: " << ret->getFreeMemory();
            }
            return ret;
        }
    } // namespace

    IPCRingBuffer::BufferWriteProxy::BufferWriteProxy(
        detail::IPCPutResultPtr &&data, IPCRingBufferPtr &&shm)
        : m_buf(nullptr), m_seq(0), m_data(std::move(data)) {
        if (m_data) {
            m_buf = m_data->buffer;
            m_seq = m_data->seq;
            m_data->shm = std::move(shm);
        }
    }

    IPCRingBuffer::BufferReadProxy::BufferReadProxy(
        detail::IPCGetResultPtr &&data, IPCRingBufferPtr &&shm)
        : m_buf(nullptr), m_seq(0), m_data(std::move(data)) {
        if (nullptr != m_data) {
            m_buf = m_data->buffer;
            m_seq = m_data->seq;
            m_data->shm = std::move(shm);
        }
    }

    IPCRingBuffer::smart_pointer_type
    IPCRingBuffer::BufferReadProxy::getBufferSmartPointer() const {
        return smart_pointer_type(m_data, m_buf);
    }

    IPCRingBuffer::Options::Options()
        : m_shmBackend(ipc::DEFAULT_MANAGED_SHM_ID) {}

    IPCRingBuffer::Options::Options(std::string const &name)
        : m_name(ipc::make_name_safe(name)),
          m_shmBackend(ipc::DEFAULT_MANAGED_SHM_ID) {}

    IPCRingBuffer::Options::Options(std::string const &name,
                                    BackendType backend)
        : m_name(ipc::make_name_safe(name)), m_shmBackend(backend) {}

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setName(std::string const &name) {
        m_name = ipc::make_name_safe(name);
        return *this;
    }

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setAlignment(alignment_type alignment) {
        /// @todo ensure power of 2
        m_alignment = alignment;
        return *this;
    }

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setEntries(entry_count_type entries) {
        m_entries = entries;
        return *this;
    }

    IPCRingBuffer::Options &
    IPCRingBuffer::Options::setEntrySize(entry_size_type entrySize) {
        m_entrySize = entrySize;
        return *this;
    }
    class IPCRingBuffer::Impl {
      public:
        Impl(unique_ptr<SharedMemorySegmentHolder> &&segment,
             Options const &opts)
            : m_seg(std::move(segment)), m_bookkeeping(nullptr), m_opts(opts) {
            m_bookkeeping = m_seg->getBookkeeping();
            m_opts.setEntries(m_bookkeeping->getCapacity());
            m_opts.setEntrySize(m_bookkeeping->getBufferLength());
        }

        detail::IPCPutResultPtr put() {
            return m_bookkeeping->produceElement();
        }

        detail::IPCGetResultPtr get(sequence_type num) {
            detail::IPCGetResultPtr ret;
            auto boundsLock = m_bookkeeping->getSharableLock();
            auto elt = m_bookkeeping->getBySequenceNumber(num, boundsLock);
            if (nullptr != elt) {
                auto readerLock = elt->getSharableLock();
                auto buf = elt->getBuf(readerLock);
                /// The nullptr will be filled in by the main object.
                ret.reset(new detail::IPCGetResult{buf, std::move(readerLock),
                                                   num, nullptr});
            }
            return ret;
        }

        detail::IPCGetResultPtr getLatest() {
            detail::IPCGetResultPtr ret;
            auto boundsLock = m_bookkeeping->getSharableLock();
            auto elt = m_bookkeeping->back(boundsLock);
            if (nullptr != elt) {
                auto readerLock = elt->getSharableLock();
                auto buf = elt->getBuf(readerLock);
                /// The nullptr will be filled in by the main object.
                ret.reset(new detail::IPCGetResult{
                    buf, std::move(readerLock),
                    m_bookkeeping->backSequenceNumber(boundsLock), nullptr});
            }
            return ret;
        }

        Options const &getOpts() const { return m_opts; }

      private:
        unique_ptr<SharedMemorySegmentHolder> m_seg;
        detail::Bookkeeping *m_bookkeeping;

        Options m_opts;
    };

    IPCRingBufferPtr IPCRingBuffer::m_constructorHelper(Options const &opts,
                                                        bool doCreate) {

        IPCRingBufferPtr ret;
        unique_ptr<SharedMemorySegmentHolder> segment;

        switch (opts.getBackend()) {

        case ipc::BASIC_MANAGED_SHM_ID:
            segment =
                constructMemorySegment<ipc::basic_managed_shm>(opts, doCreate);
            break;

#ifdef OSVR_HAVE_WINDOWS_SHM
        case ipc::WINDOWS_MANAGED_SHM_ID:
            segment = constructMemorySegment<ipc::windows_managed_shm>(
                opts, doCreate);
            break;
#endif

#ifdef OSVR_HAVE_XSI_SHM
        case ipc::SYSV_MANAGED_SHM_ID:
            segment =
                constructMemorySegment<ipc::sysv_managed_shm>(opts, doCreate);
            break;
#endif

        default:
            getIPCRingBufferLogger().error()
                << "Unsupported/unrecognized shared memory backend: "
                << int(opts.getBackend());
            break;
        }

        if (!segment) {
            return ret;
        }
        unique_ptr<Impl> impl(new Impl(std::move(segment), opts));
        ret.reset(new IPCRingBuffer(std::move(impl)));
        return ret;
    }

    IPCRingBuffer::abi_level_type IPCRingBuffer::getABILevel() {
        return SHM_SOURCE_ABI_LEVEL;
    }

    IPCRingBufferPtr IPCRingBuffer::create(Options const &opts) {
        return m_constructorHelper(opts, true);
    }
    IPCRingBufferPtr IPCRingBuffer::find(Options const &opts) {
        return m_constructorHelper(opts, false);
    }

    IPCRingBuffer::IPCRingBuffer(unique_ptr<Impl> &&impl)
        : m_impl(std::move(impl)) {}

    IPCRingBuffer::~IPCRingBuffer() {}

    IPCRingBuffer::BackendType IPCRingBuffer::getBackend() const {
        return m_impl->getOpts().getBackend();
    }

    std::string const &IPCRingBuffer::getName() const {
        return m_impl->getOpts().getName();
    }

    uint32_t IPCRingBuffer::getEntrySize() const {
        return m_impl->getOpts().getEntrySize();
    }

    uint16_t IPCRingBuffer::getEntries() const {
        return m_impl->getOpts().getEntries();
    }

    IPCRingBuffer::BufferWriteProxy IPCRingBuffer::put() {
        return BufferWriteProxy(m_impl->put(), shared_from_this());
    }

    IPCRingBuffer::sequence_type IPCRingBuffer::put(pointer_to_const_type data,
                                                    size_t len) {
        auto proxy = put();
        std::memcpy(proxy.get(), data, len);
        return proxy.getSequenceNumber();
    }

    IPCRingBuffer::BufferReadProxy IPCRingBuffer::get(sequence_type num) {
        return BufferReadProxy(m_impl->get(num), shared_from_this());
    }

    IPCRingBuffer::BufferReadProxy IPCRingBuffer::getLatest() {
        return BufferReadProxy(m_impl->getLatest(), shared_from_this());
    }

} // namespace common
} // namespace osvr
<|endoftext|>


<commit_before>/*!
 * \orientation_filter.cpp
 * \brief Calculates the roll and pitch of the base and the pitch of the sensor tower from CARL's IMUs.
 *
 * orientation_filter creates a ROS node that measures orientations for the base and sensor tower by combining and
 * filtering accelerometer and gyro data.
 *
 * \author David Kent - davidkent@wpi.edu
 * \date January 13, 2015
 */

#include <carl_phidgets/orientation_filter.h>

OrientationFilter::OrientationFilter()
{
  //initialize joint states
  jointStates.name.push_back("base_footprint_base_link_pitch_joint");
  jointStates.name.push_back("base_footprint_base_link_roll_joint");
  jointStates.name.push_back("left_rear_strut_link_left_rear_strut_top_link_joint");
  jointStates.name.push_back("right_rear_strut_link_right_rear_strut_top_link_joint");
  jointStates.position.resize(4);
  jointStates.velocity.resize(4);
  jointStates.effort.resize(4);

  baseOrientationInitialized = false;

  // create the ROS topics
  frameJointStatePublisher = n.advertise<sensor_msgs::JointState>("frame_joint_states", 1);
  baseImuSubscriber = n.subscribe<sensor_msgs::Imu>("imu_base/data_raw", 1, &OrientationFilter::baseImuCallback, this);
  topImuSubscriber = n.subscribe<sensor_msgs::Imu>("imu_top/data_raw", 1, &OrientationFilter::topImuCallback, this);

  //initialize filter stuff
  prevUpdateTime = ros::Time::now();
  PPrev[0] = 0;
  PPrev[1] = 0;
}

void OrientationFilter::topImuCallback(const sensor_msgs::Imu::ConstPtr& data)
{
  if (!baseOrientationInitialized)
  {
    ROS_INFO("Waiting for base orientation to be initialized before calculating frame pitch...");
    return;
  }

  float x = data->linear_acceleration.x; //value is inverted to account for IMU mounting angle
  float z = data->linear_acceleration.z;
  float pitch = atan2(x, z) + jointStates.position[0];

  jointStates.position[2] = -pitch;  //left strut
  jointStates.position[3] = pitch; //right strut
  frameJointStatePublisher.publish(jointStates);
}

void OrientationFilter::baseImuCallback(const sensor_msgs::Imu::ConstPtr& data)
{
  //**************************** Read IMU data *****************************

  //gyro measurement
  float w[2];
  w[0] = -data->angular_velocity.x; //value is inverted to account for IMU mounting angle
  w[1] = data->angular_velocity.y;

  //gyro covariance
  float Q[2];
  Q[0] = data->angular_velocity_covariance[0];
  Q[1] = data->angular_velocity_covariance[1];

  //accelerometer measurement
  float x = -data->linear_acceleration.x; //value is inverted to account for IMU mounting angle
  float y = data->linear_acceleration.y;
  float z = -data->linear_acceleration.z; //value is inverted to account for IMU mounting angle
  float a[2];
  a[0] = atan2(y, z); //base pitch
  a[1] = atan2(x, z); //base roll

  //accelerometer covariance
  float R[2];
  R[0] = data->linear_acceleration_covariance[0];
  R[1] = data->linear_acceleration_covariance[4];

  //Previous orientation
  float thetaPrev[2];
  thetaPrev[0] = jointStates.position[0];
  thetaPrev[1] = jointStates.position[1];

  //Time step
  float deltaT = (ros::Time::now() - prevUpdateTime).toSec();
  //update time for next time step
  prevUpdateTime = ros::Time::now();

  //******************** Calculate Filtered Orientation ********************
  float thetaPredicted[2];
  float P[2];
  float J[2];
  thetaPredicted[0] = thetaPrev[0] + w[0]*deltaT;
  thetaPredicted[1] = thetaPrev[1] + w[1]*deltaT;
  P[0] = PPrev[0] + Q[0];
  P[1] = PPrev[1] + Q[1];
  J[0] = P[0]/(P[0] + R[0]);
  J[1] = P[1]/(P[1] + R[1]);
  jointStates.position[0] = thetaPredicted[0] + J[0]*(a[0] - thetaPredicted[0]);
  jointStates.position[1] = thetaPredicted[1] + J[1]*(a[1] - thetaPredicted[1]);
  //update P for next time step
  PPrev[0] = (1 - J[0])*P[0];
  PPrev[1] = (1 - J[1])*P[1];

  /*
  jointStates.position[0] = atan2(y, z);  //base pitch
  jointStates.position[1] = atan2(x, z);  //base roll
  */
  frameJointStatePublisher.publish(jointStates);

  if (!baseOrientationInitialized)
  {
    ROS_INFO("Base orientation initialized!");
    baseOrientationInitialized = true;
  }
}

int main(int argc, char **argv)
{
  // initialize ROS and the node
  ros::init(argc, argv, "orientation_filter");

  // initialize the joystick controller
  OrientationFilter of;

  ros::spin();

  return EXIT_SUCCESS;
}
<commit_msg>Filter tuning<commit_after>/*!
 * \orientation_filter.cpp
 * \brief Calculates the roll and pitch of the base and the pitch of the sensor tower from CARL's IMUs.
 *
 * orientation_filter creates a ROS node that measures orientations for the base and sensor tower by combining and
 * filtering accelerometer and gyro data.
 *
 * \author David Kent - davidkent@wpi.edu
 * \date January 13, 2015
 */

#include <carl_phidgets/orientation_filter.h>

OrientationFilter::OrientationFilter()
{
  //initialize joint states
  jointStates.name.push_back("base_footprint_base_link_pitch_joint");
  jointStates.name.push_back("base_footprint_base_link_roll_joint");
  jointStates.name.push_back("left_rear_strut_link_left_rear_strut_top_link_joint");
  jointStates.name.push_back("right_rear_strut_link_right_rear_strut_top_link_joint");
  jointStates.position.resize(4);
  jointStates.velocity.resize(4);
  jointStates.effort.resize(4);

  baseOrientationInitialized = false;

  // create the ROS topics
  frameJointStatePublisher = n.advertise<sensor_msgs::JointState>("frame_joint_states", 1);
  baseImuSubscriber = n.subscribe<sensor_msgs::Imu>("imu_base/data_raw", 1, &OrientationFilter::baseImuCallback, this);
  topImuSubscriber = n.subscribe<sensor_msgs::Imu>("imu_top/data_raw", 1, &OrientationFilter::topImuCallback, this);

  //initialize filter stuff
  prevUpdateTime = ros::Time::now();
  PPrev[0] = 0;
  PPrev[1] = 0;
}

void OrientationFilter::topImuCallback(const sensor_msgs::Imu::ConstPtr& data)
{
  if (!baseOrientationInitialized)
  {
    ROS_INFO("Waiting for base orientation to be initialized before calculating frame pitch...");
    return;
  }

  float x = data->linear_acceleration.x; //value is inverted to account for IMU mounting angle
  float z = data->linear_acceleration.z;
  float pitch = atan2(x, z) + jointStates.position[0];

  jointStates.position[2] = -pitch;  //left strut
  jointStates.position[3] = pitch; //right strut
  frameJointStatePublisher.publish(jointStates);
}

void OrientationFilter::baseImuCallback(const sensor_msgs::Imu::ConstPtr& data)
{
  /**************************** Read IMU data *****************************/

  //gyro measurement
  float w[2];
  w[0] = -data->angular_velocity.x; //value is inverted to account for IMU mounting angle
  w[1] = data->angular_velocity.y;

  //gyro covariance
  float Q[2];
  Q[0] = data->angular_velocity_covariance[0];
  Q[1] = data->angular_velocity_covariance[4];

  //accelerometer measurement
  float x = -data->linear_acceleration.x; //value is inverted to account for IMU mounting angle
  float y = data->linear_acceleration.y;
  float z = -data->linear_acceleration.z; //value is inverted to account for IMU mounting angle
  float a[2];
  a[0] = atan2(y, z); //base pitch
  a[1] = atan2(x, z); //base roll

  //accelerometer covariance
  float R[2];
  R[0] = 100*data->linear_acceleration_covariance[0];
  R[1] = 100*data->linear_acceleration_covariance[4];
  //NOTE: these values are adjusted to bias the filter towards prefering the gyro measurements

  //Previous orientation
  float thetaPrev[2];
  thetaPrev[0] = jointStates.position[0];
  thetaPrev[1] = jointStates.position[1];

  //Time step
  float deltaT = (ros::Time::now() - prevUpdateTime).toSec();
  //update time for next time step
  prevUpdateTime = ros::Time::now();

  /******************** Calculate Filtered Orientation ********************/
  float thetaPredicted[2];
  float P[2];
  float J[2];
  thetaPredicted[0] = thetaPrev[0] + w[0]*deltaT;
  thetaPredicted[1] = thetaPrev[1] + w[1]*deltaT;
  P[0] = PPrev[0] + Q[0];
  P[1] = PPrev[1] + Q[1];
  J[0] = P[0]/(P[0] + R[0]);
  J[1] = P[1]/(P[1] + R[1]);
  jointStates.position[0] = thetaPredicted[0] + J[0]*(a[0] - thetaPredicted[0]);
  jointStates.position[1] = thetaPredicted[1] + J[1]*(a[1] - thetaPredicted[1]);
  //update P for next time step
  PPrev[0] = (1 - J[0])*P[0];
  PPrev[1] = (1 - J[1])*P[1];

  //ROS_INFO("a:%f, w:%f Q:%f, R:%f, P:%f, J:%f, thetaPrev:%f, thetaPredicted:%f, theta:%f", a[1], w[1], Q[1], R[1], P[1], J[1], thetaPrev[1], thetaPredicted[1], jointStates.position[1]);

  /*
  jointStates.position[0] = atan2(y, z);  //base pitch
  jointStates.position[1] = atan2(x, z);  //base roll
  */
  frameJointStatePublisher.publish(jointStates);

  if (!baseOrientationInitialized)
  {
    ROS_INFO("Base orientation initialized!");
    baseOrientationInitialized = true;
  }
}

int main(int argc, char **argv)
{
  // initialize ROS and the node
  ros::init(argc, argv, "orientation_filter");

  // initialize the joystick controller
  OrientationFilter of;

  ros::spin();

  return EXIT_SUCCESS;
}
<|endoftext|>