<commit_before>/*
 * Copyright (C) 2010, British Broadcasting Corporation
 * All Rights Reserved.
 *
 * Author: Philip de Nier
 *
 * 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 of the British Broadcasting Corporation 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.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <cstring>

#include <im/essence_parser/SoundConversion.h>
#include <im/IMException.h>
#include <im/Logging.h>

uint8_t im::get_aes3_channel_valid_flags(const unsigned char *aes3_data, uint32_t aes3_data_size)
{
    IM_CHECK(aes3_data_size >= 3);
    return aes3_data[3];
}

uint16_t im::get_aes3_sample_count(const unsigned char *aes3_data, uint32_t aes3_data_size)
{
    IM_CHECK(aes3_data_size >= 2);
    return ((uint16_t)aes3_data[2] << 8) | aes3_data[1];
}

uint32_t im::convert_aes3_to_pcm(const unsigned char *aes3_data, uint32_t aes3_data_size,
                                 uint32_t bits_per_sample, uint8_t channel_num,
                                 unsigned char *pcm_data, uint32_t pcm_data_size)
{
    uint16_t sample_count   = get_aes3_sample_count(aes3_data, aes3_data_size);
    uint8_t valid_flags     = get_aes3_channel_valid_flags(aes3_data, aes3_data_size);
    uint32_t block_align    = (bits_per_sample + 7) / 8;

    IM_CHECK(sample_count <= (aes3_data_size - 4) / (8 * 4)); // 4 bytes per sample, 8 channels
    IM_CHECK(block_align == 2 || block_align == 3); // only 16-bit to 24-bit sample size allowed
    IM_CHECK(channel_num < 8);
    IM_CHECK(pcm_data_size >= block_align * sample_count);

    if (!(valid_flags & (1 << channel_num))) {
        memset(pcm_data, 0, sample_count * block_align);
        return 4 + sample_count * 4 * 8;
    }

    const unsigned char *aes_data_ptr = &aes3_data[4];
    unsigned char *pcm_data_ptr = &pcm_data[0];
    uint16_t sample_num;

    if (block_align == 2) {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            aes_data_ptr += channel_num * 4;
            pcm_data_ptr[0] = (aes_data_ptr[1] >> 4) |
                              (aes_data_ptr[2] << 4);
            pcm_data_ptr[1] = (aes_data_ptr[2] >> 4) |
                              (aes_data_ptr[3] << 4);
            pcm_data_ptr += 2;
            aes_data_ptr += (8 - channel_num) * 4;
        }
    } else {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            aes_data_ptr += channel_num * 4;
            pcm_data_ptr[0] = (aes_data_ptr[0] >> 4) |
                              (aes_data_ptr[1] << 4);
            pcm_data_ptr[1] = (aes_data_ptr[1] >> 4) |
                              (aes_data_ptr[2] << 4);
            pcm_data_ptr[2] = (aes_data_ptr[2] >> 4) |
                              (aes_data_ptr[3] << 4);
            pcm_data_ptr += 3;
            aes_data_ptr += (8 - channel_num) * 4;
        }
    }

    return 4 + sample_count * 4 * 8;
}

uint32_t im::convert_aes3_to_mc_pcm(const unsigned char *aes3_data, uint32_t aes3_data_size,
                                    uint32_t bits_per_sample, uint8_t channel_count,
                                    unsigned char *pcm_data, uint32_t pcm_data_size)
{
    uint16_t sample_count       = get_aes3_sample_count(aes3_data, aes3_data_size);
    uint8_t valid_flags         = get_aes3_channel_valid_flags(aes3_data, aes3_data_size);
    uint32_t bytes_per_sample   = (bits_per_sample + 7) / 8;

    IM_CHECK(sample_count <= (aes3_data_size - 4) / (8 * 4)); // 4 bytes per sample, 8 channels
    IM_CHECK(bytes_per_sample == 2 || bytes_per_sample == 3); // only 16-bit to 24-bit sample size allowed
    IM_CHECK(channel_count <= 8);
    IM_CHECK(pcm_data_size >= channel_count * bytes_per_sample * sample_count);

    const unsigned char *aes_data_ptr = &aes3_data[4];
    unsigned char *pcm_data_ptr = &pcm_data[0];
    uint16_t sample_num, channel_num;

    if (bytes_per_sample == 2) {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            for (channel_num = 0; channel_num < channel_count; channel_num++) {
                if (valid_flags & (1 << channel_num)) {
                    pcm_data_ptr[0] = (aes_data_ptr[1] >> 4) |
                                      (aes_data_ptr[2] << 4);
                    pcm_data_ptr[1] = (aes_data_ptr[2] >> 4) |
                                      (aes_data_ptr[3] << 4);
                } else {
                    pcm_data_ptr[0] = 0x00;
                    pcm_data_ptr[1] = 0x00;
                }
                pcm_data_ptr += 2;
                aes_data_ptr += 4;
            }
            aes_data_ptr += (8 - channel_count) * 4;
        }
    } else {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            for (channel_num = 0; channel_num < channel_count; channel_num++) {
                if (valid_flags & (1 << channel_num)) {
                    pcm_data_ptr[0] = (aes_data_ptr[0] >> 4) |
                                      (aes_data_ptr[1] << 4);
                    pcm_data_ptr[1] = (aes_data_ptr[1] >> 4) |
                                      (aes_data_ptr[2] << 4);
                    pcm_data_ptr[2] = (aes_data_ptr[2] >> 4) |
                                      (aes_data_ptr[3] << 4);
                } else {
                    pcm_data_ptr[0] = 0x00;
                    pcm_data_ptr[1] = 0x00;
                    pcm_data_ptr[3] = 0x00;
                }
                pcm_data_ptr += 3;
                aes_data_ptr += 4;
            }
            aes_data_ptr += (8 - channel_count) * 4;
        }
    }

    return 4 + sample_count * 4 * 8;
}

void im::deinterleave_audio(const unsigned char *input_data, uint32_t input_data_size,
                            uint32_t bits_per_sample, uint8_t channel_count, uint8_t channel_num,
                            unsigned char *output_data, uint32_t output_data_size)
{
    uint32_t input_block_align = channel_count * ((bits_per_sample + 7) / 8);
    uint32_t output_block_align = (bits_per_sample + 7) / 8;
    uint32_t channel_offset = channel_num * output_block_align;
    uint32_t sample_count = input_data_size / input_block_align;
    uint32_t i, j;

    IM_CHECK(output_data_size >= sample_count * output_block_align);

    for (i = 0; i < sample_count; i++) {
        for (j = 0; j < output_block_align; j++)
            output_data[i * output_block_align + j] = input_data[i * input_block_align + channel_offset + j];
    }
}

<commit_msg>sound conv: fix array index typo in aes3 multichannel conversion<commit_after>/*
 * Copyright (C) 2010, British Broadcasting Corporation
 * All Rights Reserved.
 *
 * Author: Philip de Nier
 *
 * 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 of the British Broadcasting Corporation 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.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <cstring>

#include <im/essence_parser/SoundConversion.h>
#include <im/IMException.h>
#include <im/Logging.h>

uint8_t im::get_aes3_channel_valid_flags(const unsigned char *aes3_data, uint32_t aes3_data_size)
{
    IM_CHECK(aes3_data_size >= 3);
    return aes3_data[3];
}

uint16_t im::get_aes3_sample_count(const unsigned char *aes3_data, uint32_t aes3_data_size)
{
    IM_CHECK(aes3_data_size >= 2);
    return ((uint16_t)aes3_data[2] << 8) | aes3_data[1];
}

uint32_t im::convert_aes3_to_pcm(const unsigned char *aes3_data, uint32_t aes3_data_size,
                                 uint32_t bits_per_sample, uint8_t channel_num,
                                 unsigned char *pcm_data, uint32_t pcm_data_size)
{
    uint16_t sample_count   = get_aes3_sample_count(aes3_data, aes3_data_size);
    uint8_t valid_flags     = get_aes3_channel_valid_flags(aes3_data, aes3_data_size);
    uint32_t block_align    = (bits_per_sample + 7) / 8;

    IM_CHECK(sample_count <= (aes3_data_size - 4) / (8 * 4)); // 4 bytes per sample, 8 channels
    IM_CHECK(block_align == 2 || block_align == 3); // only 16-bit to 24-bit sample size allowed
    IM_CHECK(channel_num < 8);
    IM_CHECK(pcm_data_size >= block_align * sample_count);

    if (!(valid_flags & (1 << channel_num))) {
        memset(pcm_data, 0, sample_count * block_align);
        return 4 + sample_count * 4 * 8;
    }

    const unsigned char *aes_data_ptr = &aes3_data[4];
    unsigned char *pcm_data_ptr = &pcm_data[0];
    uint16_t sample_num;

    if (block_align == 2) {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            aes_data_ptr += channel_num * 4;
            pcm_data_ptr[0] = (aes_data_ptr[1] >> 4) |
                              (aes_data_ptr[2] << 4);
            pcm_data_ptr[1] = (aes_data_ptr[2] >> 4) |
                              (aes_data_ptr[3] << 4);
            pcm_data_ptr += 2;
            aes_data_ptr += (8 - channel_num) * 4;
        }
    } else {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            aes_data_ptr += channel_num * 4;
            pcm_data_ptr[0] = (aes_data_ptr[0] >> 4) |
                              (aes_data_ptr[1] << 4);
            pcm_data_ptr[1] = (aes_data_ptr[1] >> 4) |
                              (aes_data_ptr[2] << 4);
            pcm_data_ptr[2] = (aes_data_ptr[2] >> 4) |
                              (aes_data_ptr[3] << 4);
            pcm_data_ptr += 3;
            aes_data_ptr += (8 - channel_num) * 4;
        }
    }

    return 4 + sample_count * 4 * 8;
}

uint32_t im::convert_aes3_to_mc_pcm(const unsigned char *aes3_data, uint32_t aes3_data_size,
                                    uint32_t bits_per_sample, uint8_t channel_count,
                                    unsigned char *pcm_data, uint32_t pcm_data_size)
{
    uint16_t sample_count       = get_aes3_sample_count(aes3_data, aes3_data_size);
    uint8_t valid_flags         = get_aes3_channel_valid_flags(aes3_data, aes3_data_size);
    uint32_t bytes_per_sample   = (bits_per_sample + 7) / 8;

    IM_CHECK(sample_count <= (aes3_data_size - 4) / (8 * 4)); // 4 bytes per sample, 8 channels
    IM_CHECK(bytes_per_sample == 2 || bytes_per_sample == 3); // only 16-bit to 24-bit sample size allowed
    IM_CHECK(channel_count <= 8);
    IM_CHECK(pcm_data_size >= channel_count * bytes_per_sample * sample_count);

    const unsigned char *aes_data_ptr = &aes3_data[4];
    unsigned char *pcm_data_ptr = &pcm_data[0];
    uint16_t sample_num, channel_num;

    if (bytes_per_sample == 2) {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            for (channel_num = 0; channel_num < channel_count; channel_num++) {
                if (valid_flags & (1 << channel_num)) {
                    pcm_data_ptr[0] = (aes_data_ptr[1] >> 4) |
                                      (aes_data_ptr[2] << 4);
                    pcm_data_ptr[1] = (aes_data_ptr[2] >> 4) |
                                      (aes_data_ptr[3] << 4);
                } else {
                    pcm_data_ptr[0] = 0x00;
                    pcm_data_ptr[1] = 0x00;
                }
                pcm_data_ptr += 2;
                aes_data_ptr += 4;
            }
            aes_data_ptr += (8 - channel_count) * 4;
        }
    } else {
        for (sample_num = 0; sample_num < sample_count; sample_num++) {
            for (channel_num = 0; channel_num < channel_count; channel_num++) {
                if (valid_flags & (1 << channel_num)) {
                    pcm_data_ptr[0] = (aes_data_ptr[0] >> 4) |
                                      (aes_data_ptr[1] << 4);
                    pcm_data_ptr[1] = (aes_data_ptr[1] >> 4) |
                                      (aes_data_ptr[2] << 4);
                    pcm_data_ptr[2] = (aes_data_ptr[2] >> 4) |
                                      (aes_data_ptr[3] << 4);
                } else {
                    pcm_data_ptr[0] = 0x00;
                    pcm_data_ptr[1] = 0x00;
                    pcm_data_ptr[2] = 0x00;
                }
                pcm_data_ptr += 3;
                aes_data_ptr += 4;
            }
            aes_data_ptr += (8 - channel_count) * 4;
        }
    }

    return 4 + sample_count * 4 * 8;
}

void im::deinterleave_audio(const unsigned char *input_data, uint32_t input_data_size,
                            uint32_t bits_per_sample, uint8_t channel_count, uint8_t channel_num,
                            unsigned char *output_data, uint32_t output_data_size)
{
    uint32_t input_block_align = channel_count * ((bits_per_sample + 7) / 8);
    uint32_t output_block_align = (bits_per_sample + 7) / 8;
    uint32_t channel_offset = channel_num * output_block_align;
    uint32_t sample_count = input_data_size / input_block_align;
    uint32_t i, j;

    IM_CHECK(output_data_size >= sample_count * output_block_align);

    for (i = 0; i < sample_count; i++) {
        for (j = 0; j < output_block_align; j++)
            output_data[i * output_block_align + j] = input_data[i * input_block_align + channel_offset + j];
    }
}

<|endoftext|>


<commit_before>/*
 * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
 *
 * 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 names of Stanford University or Willow Garage, Inc. 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 <px4.h>

using namespace px4;

/**
 * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
 */
void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
{
	PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid);
}
// void rc_channels_callback(int i)
// {
	// PX4_INFO("I heard: [%d]", i);
// }
namespace px4
{
bool task_should_exit = false;
}

PX4_MAIN_FUNCTION(subscriber)
{
	/**
	 * The ros::init() function needs to see argc and argv so that it can perform
	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
	 * remappings you can use a different version of init() which takes remappings
	 * directly, but for most command-line programs, passing argc and argv is the easiest
	 * way to do it.  The third argument to init() is the name of the node.
	 *
	 * You must call one of the versions of px4::init() before using any other
	 * part of the PX4/ ROS system.
	 */
	px4::init(argc, argv, "listener");

	/**
	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
	 * NodeHandle destructed will close down the node.
	 */
	px4::NodeHandle n;

	/**
	 * The subscribe() call is how you tell ROS that you want to receive messages
	 * on a given topic.  This invokes a call to the ROS
	 * master node, which keeps a registry of who is publishing and who
	 * is subscribing.  Messages are passed to a callback function, here
	 * called chatterCallback.  subscribe() returns a Subscriber object that you
	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
	 * object go out of scope, this callback will automatically be unsubscribed from
	 * this topic.
	 *
	 * The second parameter to the subscribe() function is the size of the message
	 * queue.  If messages are arriving faster than they are being processed, this
	 * is the number of messages that will be buffered up before beginning to throw
	 * away the oldest ones.
	 */
	// n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);});
	// n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);});
	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
	std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
	PX4_INFO("subscribed");

	/**
	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
	 * callbacks will be called from within this thread (the main one).  px4::spin()
	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
	 */
	n.spin();

	return 0;
}
<commit_msg>remove unnecessary variable<commit_after>/*
 * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
 *
 * 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 names of Stanford University or Willow Garage, Inc. 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 <px4.h>

using namespace px4;

/**
 * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
 */
void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
{
	PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid);
}
// void rc_channels_callback(int i)
// {
	// PX4_INFO("I heard: [%d]", i);
// }
namespace px4
{
bool task_should_exit = false;
}

PX4_MAIN_FUNCTION(subscriber)
{
	/**
	 * The ros::init() function needs to see argc and argv so that it can perform
	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
	 * remappings you can use a different version of init() which takes remappings
	 * directly, but for most command-line programs, passing argc and argv is the easiest
	 * way to do it.  The third argument to init() is the name of the node.
	 *
	 * You must call one of the versions of px4::init() before using any other
	 * part of the PX4/ ROS system.
	 */
	px4::init(argc, argv, "listener");

	/**
	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
	 * NodeHandle destructed will close down the node.
	 */
	px4::NodeHandle n;

	/**
	 * The subscribe() call is how you tell ROS that you want to receive messages
	 * on a given topic.  This invokes a call to the ROS
	 * master node, which keeps a registry of who is publishing and who
	 * is subscribing.  Messages are passed to a callback function, here
	 * called chatterCallback.  subscribe() returns a Subscriber object that you
	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
	 * object go out of scope, this callback will automatically be unsubscribed from
	 * this topic.
	 *
	 * The second parameter to the subscribe() function is the size of the message
	 * queue.  If messages are arriving faster than they are being processed, this
	 * is the number of messages that will be buffered up before beginning to throw
	 * away the oldest ones.
	 */
	// n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);});
	// n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);});
	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
	// std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
			[](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
	PX4_INFO("subscribed");

	/**
	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
	 * callbacks will be called from within this thread (the main one).  px4::spin()
	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
	 */
	n.spin();

	return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright 2015 Google Inc.
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "src/gpu/GrAuditTrail.h"
#include "src/gpu/GrGpu.h"
#include "src/gpu/GrSurfaceDrawContext.h"
#include "src/gpu/geometry/GrStyledShape.h"
#include "src/gpu/ops/GrDashLinePathRenderer.h"
#include "src/gpu/ops/GrDashOp.h"
#include "src/gpu/ops/GrMeshDrawOp.h"

GrPathRenderer::CanDrawPath
GrDashLinePathRenderer::onCanDrawPath(const CanDrawPathArgs& args) const {
    SkPoint pts[2];
    bool inverted;
    if (args.fShape->style().isDashed() && args.fShape->asLine(pts, &inverted)) {
        // We should never have an inverse dashed case.
        SkASSERT(!inverted);
        if (!GrDashOp::CanDrawDashLine(pts, args.fShape->style(), *args.fViewMatrix)) {
            return CanDrawPath::kNo;
        }
        return CanDrawPath::kYes;
    }
    return CanDrawPath::kNo;
}

bool GrDashLinePathRenderer::onDrawPath(const DrawPathArgs& args) {
    GR_AUDIT_TRAIL_AUTO_FRAME(args.fContext->priv().auditTrail(),
                              "GrDashLinePathRenderer::onDrawPath");
    GrDashOp::AAMode aaMode;
    switch (args.fAAType) {
        case GrAAType::kNone:
            aaMode = GrDashOp::AAMode::kNone;
            break;
        case GrAAType::kMSAA:
            if (args.fSurfaceDrawContext->canUseDynamicMSAA() &&
                args.fContext->priv().caps()->multisampleDisableSupport()) {
                // The coverage-only mode works as well as MSAA, so try to avoid DMSAA triggers.
                aaMode = GrDashOp::AAMode::kCoverage;
            } else {
                // In this mode we will use aa between dashes but the outer border uses MSAA.
                // Otherwise, we can wind up with external edges antialiased and internal edges
                // unantialiased.
                aaMode = GrDashOp::AAMode::kCoverageWithMSAA;
            }
            break;
        case GrAAType::kCoverage:
            aaMode = GrDashOp::AAMode::kCoverage;
            break;
    }
    SkPoint pts[2];
    SkAssertResult(args.fShape->asLine(pts, nullptr));
    GrOp::Owner op =
            GrDashOp::MakeDashLineOp(args.fContext, std::move(args.fPaint), *args.fViewMatrix, pts,
                                     aaMode, args.fShape->style(), args.fUserStencilSettings);
    if (!op) {
        return false;
    }
    args.fSurfaceDrawContext->addDrawOp(args.fClip, std::move(op));
    return true;
}
<commit_msg>Don't disable multisample for GrDashLinePathRenderer<commit_after>/*
 * Copyright 2015 Google Inc.
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "src/gpu/GrAuditTrail.h"
#include "src/gpu/GrGpu.h"
#include "src/gpu/GrSurfaceDrawContext.h"
#include "src/gpu/geometry/GrStyledShape.h"
#include "src/gpu/ops/GrDashLinePathRenderer.h"
#include "src/gpu/ops/GrDashOp.h"
#include "src/gpu/ops/GrMeshDrawOp.h"

GrPathRenderer::CanDrawPath
GrDashLinePathRenderer::onCanDrawPath(const CanDrawPathArgs& args) const {
    SkPoint pts[2];
    bool inverted;
    if (args.fShape->style().isDashed() && args.fShape->asLine(pts, &inverted)) {
        // We should never have an inverse dashed case.
        SkASSERT(!inverted);
        if (!GrDashOp::CanDrawDashLine(pts, args.fShape->style(), *args.fViewMatrix)) {
            return CanDrawPath::kNo;
        }
        return CanDrawPath::kYes;
    }
    return CanDrawPath::kNo;
}

bool GrDashLinePathRenderer::onDrawPath(const DrawPathArgs& args) {
    GR_AUDIT_TRAIL_AUTO_FRAME(args.fContext->priv().auditTrail(),
                              "GrDashLinePathRenderer::onDrawPath");
    GrDashOp::AAMode aaMode;
    switch (args.fAAType) {
        case GrAAType::kNone:
            aaMode = GrDashOp::AAMode::kNone;
            break;
        case GrAAType::kMSAA:
            // In this mode we will use aa between dashes but the outer border uses MSAA. Otherwise,
            // we can wind up with external edges antialiased and internal edges unantialiased.
            aaMode = GrDashOp::AAMode::kCoverageWithMSAA;
            break;
        case GrAAType::kCoverage:
            aaMode = GrDashOp::AAMode::kCoverage;
            break;
    }
    SkPoint pts[2];
    SkAssertResult(args.fShape->asLine(pts, nullptr));
    GrOp::Owner op =
            GrDashOp::MakeDashLineOp(args.fContext, std::move(args.fPaint), *args.fViewMatrix, pts,
                                     aaMode, args.fShape->style(), args.fUserStencilSettings);
    if (!op) {
        return false;
    }
    args.fSurfaceDrawContext->addDrawOp(args.fClip, std::move(op));
    return true;
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2017 Darius Rückert
 * Licensed under the MIT License.
 * See LICENSE file for more information.
 */

#include "Buffer.h"
#include "saiga/util/imath.h"
#include "saiga/vulkan/buffer/StagingBuffer.h"

namespace Saiga
{
namespace Vulkan
{
void Buffer::destroy()
{
    if (m_memoryLocation && bufferUsage != vk::BufferUsageFlags())
    {
        base->memory.getAllocator(this->bufferUsage, this->memoryProperties).deallocate(m_memoryLocation);
        m_memoryLocation.buffer = nullptr;
    }
}

void Buffer::createBuffer(Saiga::Vulkan::VulkanBase& base, size_t size, vk::BufferUsageFlags bufferUsage,
                          const vk::MemoryPropertyFlags& memoryProperties, vk::SharingMode sharingMode)
{
    // TODO: Sharing mode is not used yet
    this->base = &base;
    this->bufferUsage       = bufferUsage;
    this->memoryProperties = memoryProperties;
    m_memoryLocation = base.memory.getAllocator(this->bufferUsage, this->memoryProperties).allocate(size);
    if (size != m_memoryLocation.size)
    {
        LOG(WARNING) << "Unequal sizes " << size << " " << m_memoryLocation.size;
    }
}

void Buffer::stagedUpload(VulkanBase& base, size_t size, const void* data)
{
    vk::CommandBuffer cmd = base.createAndBeginTransferCommand();

    StagingBuffer staging;
    staging.init(base, size, data);

    vk::BufferCopy bc(0, m_memoryLocation.offset, size);
    cmd.copyBuffer(staging.m_memoryLocation.buffer, m_memoryLocation.buffer, bc);

    base.endTransferWait(cmd);
}

vk::DescriptorBufferInfo Buffer::createInfo()
{
    return {m_memoryLocation.buffer, m_memoryLocation.offset, m_memoryLocation.size};
}

void Buffer::flush(VulkanBase &base, vk::DeviceSize size, vk::DeviceSize offset) {
    vk::MappedMemoryRange mappedRange = {};
    mappedRange.memory                = m_memoryLocation.memory;
    mappedRange.offset                = offset;
    mappedRange.size                  = size;
    base.device.flushMappedMemoryRanges(mappedRange);
}

void Buffer::copyTo(vk::CommandBuffer cmd, Buffer &target, vk::DeviceSize srcOffset, vk::DeviceSize dstOffset,
                    vk::DeviceSize size) {
    if (size == VK_WHOLE_SIZE)
    {
        size = this->size() - srcOffset;
    }
    SAIGA_ASSERT(this->size() - srcOffset >= size, "Source buffer is not large enough");
    SAIGA_ASSERT(target.size() - dstOffset >= size, "Destination buffer is not large enough");
    vk::BufferCopy bc{m_memoryLocation.offset + srcOffset, target.m_memoryLocation.offset + dstOffset, size};
    cmd.copyBuffer(m_memoryLocation.buffer, target.m_memoryLocation.buffer, bc);
}

void Buffer::copyTo(vk::CommandBuffer cmd, vk::Image dstImage, vk::ImageLayout dstImageLayout,
                    vk::ArrayProxy<const vk::BufferImageCopy> regions) {
    cmd.copyBufferToImage(m_memoryLocation.buffer, dstImage, dstImageLayout, regions);
}

vk::BufferImageCopy Buffer::getBufferImageCopy(vk::DeviceSize offset) const { return {m_memoryLocation.offset + offset}; }

void Buffer::update(vk::CommandBuffer cmd, size_t size, void *data, vk::DeviceSize offset) {
    cmd.updateBuffer(m_memoryLocation.buffer, m_memoryLocation.offset + offset, size, data);
}

}  // namespace Vulkan
}  // namespace Saiga
<commit_msg>Cleanup in Buffer.cpp<commit_after>/**
 * Copyright (c) 2017 Darius Rückert
 * Licensed under the MIT License.
 * See LICENSE file for more information.
 */

#include "Buffer.h"
#include "saiga/util/imath.h"
#include "saiga/vulkan/buffer/StagingBuffer.h"

namespace Saiga
{
namespace Vulkan
{
void Buffer::destroy()
{
    if (m_memoryLocation && bufferUsage != vk::BufferUsageFlags())
    {
        base->memory.getAllocator(this->bufferUsage, this->memoryProperties).deallocate(m_memoryLocation);
        m_memoryLocation.buffer = nullptr;
    }
}

void Buffer::createBuffer(Saiga::Vulkan::VulkanBase& base, size_t size, vk::BufferUsageFlags bufferUsage,
                          const vk::MemoryPropertyFlags& memoryProperties, vk::SharingMode sharingMode)
{
    // TODO: Sharing mode is not used yet
    this->base             = &base;
    this->bufferUsage      = bufferUsage;
    this->memoryProperties = memoryProperties;
    m_memoryLocation       = base.memory.getAllocator(this->bufferUsage, this->memoryProperties).allocate(size);
}

void Buffer::stagedUpload(VulkanBase& base, size_t size, const void* data)
{
    vk::CommandBuffer cmd = base.createAndBeginTransferCommand();

    StagingBuffer staging;
    staging.init(base, size, data);

    vk::BufferCopy bc(0, m_memoryLocation.offset, size);
    cmd.copyBuffer(staging.m_memoryLocation.buffer, m_memoryLocation.buffer, bc);

    base.endTransferWait(cmd);
}

vk::DescriptorBufferInfo Buffer::createInfo()
{
    return {m_memoryLocation.buffer, m_memoryLocation.offset, m_memoryLocation.size};
}

void Buffer::flush(VulkanBase& base, vk::DeviceSize size, vk::DeviceSize offset)
{
    vk::MappedMemoryRange mappedRange = {};
    mappedRange.memory                = m_memoryLocation.memory;
    mappedRange.offset                = offset;
    mappedRange.size                  = size;
    base.device.flushMappedMemoryRanges(mappedRange);
}

void Buffer::copyTo(vk::CommandBuffer cmd, Buffer& target, vk::DeviceSize srcOffset, vk::DeviceSize dstOffset,
                    vk::DeviceSize size)
{
    if (size == VK_WHOLE_SIZE)
    {
        size = this->size() - srcOffset;
    }
    SAIGA_ASSERT(this->size() - srcOffset >= size, "Source buffer is not large enough");
    SAIGA_ASSERT(target.size() - dstOffset >= size, "Destination buffer is not large enough");
    vk::BufferCopy bc{m_memoryLocation.offset + srcOffset, target.m_memoryLocation.offset + dstOffset, size};
    cmd.copyBuffer(m_memoryLocation.buffer, target.m_memoryLocation.buffer, bc);
}

void Buffer::copyTo(vk::CommandBuffer cmd, vk::Image dstImage, vk::ImageLayout dstImageLayout,
                    vk::ArrayProxy<const vk::BufferImageCopy> regions)
{
    cmd.copyBufferToImage(m_memoryLocation.buffer, dstImage, dstImageLayout, regions);
}

vk::BufferImageCopy Buffer::getBufferImageCopy(vk::DeviceSize offset) const
{
    return {m_memoryLocation.offset + offset};
}

void Buffer::update(vk::CommandBuffer cmd, size_t size, void* data, vk::DeviceSize offset)
{
    cmd.updateBuffer(m_memoryLocation.buffer, m_memoryLocation.offset + offset, size, data);
}

}  // namespace Vulkan
}  // namespace Saiga
<|endoftext|>


<commit_before>#include "../../../YaizuComLib/src/stkpl/StkPl.h"
#include "../../../YaizuComLib/src/stkwebapp_um/ApiBase.h"
#include "../../../YaizuComLib/src/commonfunc/StkStringParser.h"
#include "../../../YaizuComLib/src/commonfunc/msgproc.h"
#include "sample.h"
#include "dataaccess.h"
#include "ApiGetCommandResult.h"

StkObject* ApiGetCommandResult::ExecuteImpl(StkObject* ReqObj, int Method, wchar_t UrlPath[StkWebAppExec::URL_PATH_LENGTH], int* ResultCode, wchar_t Locale[3], wchar_t* Token)
{
	StkObject* TmpObj = new StkObject(L"");

	wchar_t UserName[ApiBase::MAXLEN_OF_USERNAME];
	int UserId = -1;
	if (!CheckCredentials(Token, UserName, &UserId)) {
		AddCodeAndMsg(TmpObj, MSG_COMMON_AUTH_ERROR, MessageProc::GetMsgEng(MSG_COMMON_AUTH_ERROR), MessageProc::GetMsgJpn(MSG_COMMON_AUTH_ERROR));
		*ResultCode = 401;
		return TmpObj;
	}

	wchar_t TargetCommandResult[16];

	if (StkStringParser::ParseInto1Param(UrlPath, L"/api/commandresult/$/", L'$', TargetCommandResult, 16) > 0) {
		int ResultId = StkPlWcsToL(TargetCommandResult);
		char Output[DA_MAXLEN_OF_CMDOUTPUT];
		int Length = DataAccess::GetInstance()->GetOutput(ResultId, Output);
		if (Length == -1) {
			AddCodeAndMsg(TmpObj, MSG_RESULT_NOTEXIST, MessageProc::GetMsgEng(MSG_RESULT_NOTEXIST), MessageProc::GetMsgJpn(MSG_RESULT_NOTEXIST));
			*ResultCode = 400;
			return TmpObj;
		}

		AddCodeAndMsg(TmpObj, 0, L"", L"");
		StkObject* TmpObjD = new StkObject(L"Data");
		StkObject* TmpObjC = new StkObject(L"Result");
		wchar_t* TmpBuf = StkPlCreateWideCharFromUtf8(Output);
		size_t TmpBufLen = StkPlWcsLen(TmpBuf);
		///////////////////////////////////////////////////////////////////////
		char TBuf[50] = "";
		StkPlSPrintf(TBuf, 50, "file size (2) = %d", TmpBufLen);
		MessageProc::AddLog(TBuf, MessageProc::LOG_TYPE_INFO);
		///////////////////////////////////////////////////////////////////////

		wchar_t* TmpOut = new wchar_t[TmpBufLen * 6 + 1];
		TmpOut[0] = L'\0';
		int Index = 0;
		for (int Loop = 0; Loop < TmpBufLen; Loop++) {
			if (TmpBuf[Loop] == L'\n') {
				StkPlWcsCpy(&TmpOut[Index], 6, L"
");
				Index += 5;
				if (Loop < TmpBufLen - 1 && TmpBuf[Loop + 1] == L'\r') {
					Loop++;
				}
			} else if (TmpBuf[Loop] == L'\r') {
				StkPlWcsCpy(&TmpOut[Index], 6, L"
");
				Index += 5;
				if (Loop < TmpBufLen - 1 && TmpBuf[Loop + 1] == L'\n') {
					Loop++;
				}
			} else if (TmpBuf[Loop] == L'<') {
				StkPlWcsCpy(&TmpOut[Index], 5, L"<");
				Index += 4;
			} else if (TmpBuf[Loop] == L'>') {
				StkPlWcsCpy(&TmpOut[Index], 5, L">");
				Index += 4;
			} else if (TmpBuf[Loop] == L'&') {
				StkPlWcsCpy(&TmpOut[Index], 6, L"&");
				Index += 5;
			} else if (TmpBuf[Loop] == L' ') {
				StkPlWcsCpy(&TmpOut[Index], 7, L" ");
				Index += 6;
			} else if (TmpBuf[Loop] == L'\t') {
				StkPlWcsCpy(&TmpOut[Index], 4, L"&#9");
				Index += 3;
			} else {
				TmpOut[Index] = TmpBuf[Loop];
				Index++;
			}
		}
		TmpObjC->AppendChildElement(new StkObject(L"Output", TmpOut));
		delete [] TmpOut;
		delete TmpBuf;
		TmpObjD->AppendChildElement(TmpObjC);
		TmpObj->AppendChildElement(TmpObjD);
		*ResultCode = 200;
		return TmpObj;
	} else {
		wchar_t AgentName[DA_MAXNUM_OF_RESULT][DA_MAXLEN_OF_AGTNAME];
		wchar_t CommandName[DA_MAXNUM_OF_RESULT][DA_MAXLEN_OF_CMDNAME];
		long long UpdTime[DA_MAXNUM_OF_RESULT];
		int Id[DA_MAXNUM_OF_RESULT];
		int Cnt = DataAccess::GetInstance()->GetCommandResult(AgentName, CommandName, UpdTime, Id);

		AddCodeAndMsg(TmpObj, 0, L"", L"");
		if (Cnt != 0) {
			StkObject* TmpObjD = new StkObject(L"Data");
			for (int Loop = 0; Loop < Cnt; Loop++) {
				StkObject* TmpObjC = new StkObject(L"Result");
				wchar_t UpdTimeStr[DA_MAXLEN_OF_UNIXTIME * 2 + 1] = L"";
				StkPlSwPrintf(UpdTimeStr, DA_MAXLEN_OF_UNIXTIME * 2 + 1, L"%016x", UpdTime[Loop]);

				TmpObjC->AppendChildElement(new StkObject(L"AgentName", AgentName[Loop]));
				TmpObjC->AppendChildElement(new StkObject(L"CommandName", CommandName[Loop]));
				TmpObjC->AppendChildElement(new StkObject(L"UpdTime", UpdTimeStr));
				TmpObjC->AppendChildElement(new StkObject(L"Id", Id[Loop]));
				TmpObjD->AppendChildElement(TmpObjC);
			}
			TmpObj->AppendChildElement(TmpObjD);
		}
		*ResultCode = 200;
		return TmpObj;
	}
}
<commit_msg>#81: Debug : printf for DB length of console<commit_after>#include "../../../YaizuComLib/src/stkpl/StkPl.h"
#include "../../../YaizuComLib/src/stkwebapp_um/ApiBase.h"
#include "../../../YaizuComLib/src/commonfunc/StkStringParser.h"
#include "../../../YaizuComLib/src/commonfunc/msgproc.h"
#include "sample.h"
#include "dataaccess.h"
#include "ApiGetCommandResult.h"

StkObject* ApiGetCommandResult::ExecuteImpl(StkObject* ReqObj, int Method, wchar_t UrlPath[StkWebAppExec::URL_PATH_LENGTH], int* ResultCode, wchar_t Locale[3], wchar_t* Token)
{
	StkObject* TmpObj = new StkObject(L"");

	wchar_t UserName[ApiBase::MAXLEN_OF_USERNAME];
	int UserId = -1;
	if (!CheckCredentials(Token, UserName, &UserId)) {
		AddCodeAndMsg(TmpObj, MSG_COMMON_AUTH_ERROR, MessageProc::GetMsgEng(MSG_COMMON_AUTH_ERROR), MessageProc::GetMsgJpn(MSG_COMMON_AUTH_ERROR));
		*ResultCode = 401;
		return TmpObj;
	}

	wchar_t TargetCommandResult[16];

	if (StkStringParser::ParseInto1Param(UrlPath, L"/api/commandresult/$/", L'$', TargetCommandResult, 16) > 0) {
		int ResultId = StkPlWcsToL(TargetCommandResult);
		char Output[DA_MAXLEN_OF_CMDOUTPUT];
		int Length = DataAccess::GetInstance()->GetOutput(ResultId, Output);
		if (Length == -1) {
			AddCodeAndMsg(TmpObj, MSG_RESULT_NOTEXIST, MessageProc::GetMsgEng(MSG_RESULT_NOTEXIST), MessageProc::GetMsgJpn(MSG_RESULT_NOTEXIST));
			*ResultCode = 400;
			return TmpObj;
		}

		AddCodeAndMsg(TmpObj, 0, L"", L"");
		StkObject* TmpObjD = new StkObject(L"Data");
		StkObject* TmpObjC = new StkObject(L"Result");
		wchar_t* TmpBuf = StkPlCreateWideCharFromUtf8(Output);
		size_t TmpBufLen = StkPlWcsLen(TmpBuf);
		///////////////////////////////////////////////////////////////////////
		char TBuf[50] = "";
		StkPlSPrintf(TBuf, 50, "file size (2) = %d, db length = %d", TmpBufLen, Length);
		MessageProc::AddLog(TBuf, MessageProc::LOG_TYPE_INFO);
		///////////////////////////////////////////////////////////////////////

		wchar_t* TmpOut = new wchar_t[TmpBufLen * 6 + 1];
		TmpOut[0] = L'\0';
		int Index = 0;
		for (int Loop = 0; Loop < TmpBufLen; Loop++) {
			if (TmpBuf[Loop] == L'\n') {
				StkPlWcsCpy(&TmpOut[Index], 6, L"
");
				Index += 5;
				if (Loop < TmpBufLen - 1 && TmpBuf[Loop + 1] == L'\r') {
					Loop++;
				}
			} else if (TmpBuf[Loop] == L'\r') {
				StkPlWcsCpy(&TmpOut[Index], 6, L"
");
				Index += 5;
				if (Loop < TmpBufLen - 1 && TmpBuf[Loop + 1] == L'\n') {
					Loop++;
				}
			} else if (TmpBuf[Loop] == L'<') {
				StkPlWcsCpy(&TmpOut[Index], 5, L"<");
				Index += 4;
			} else if (TmpBuf[Loop] == L'>') {
				StkPlWcsCpy(&TmpOut[Index], 5, L">");
				Index += 4;
			} else if (TmpBuf[Loop] == L'&') {
				StkPlWcsCpy(&TmpOut[Index], 6, L"&");
				Index += 5;
			} else if (TmpBuf[Loop] == L' ') {
				StkPlWcsCpy(&TmpOut[Index], 7, L" ");
				Index += 6;
			} else if (TmpBuf[Loop] == L'\t') {
				StkPlWcsCpy(&TmpOut[Index], 4, L"&#9");
				Index += 3;
			} else {
				TmpOut[Index] = TmpBuf[Loop];
				Index++;
			}
		}
		TmpObjC->AppendChildElement(new StkObject(L"Output", TmpOut));
		delete [] TmpOut;
		delete TmpBuf;
		TmpObjD->AppendChildElement(TmpObjC);
		TmpObj->AppendChildElement(TmpObjD);
		*ResultCode = 200;
		return TmpObj;
	} else {
		wchar_t AgentName[DA_MAXNUM_OF_RESULT][DA_MAXLEN_OF_AGTNAME];
		wchar_t CommandName[DA_MAXNUM_OF_RESULT][DA_MAXLEN_OF_CMDNAME];
		long long UpdTime[DA_MAXNUM_OF_RESULT];
		int Id[DA_MAXNUM_OF_RESULT];
		int Cnt = DataAccess::GetInstance()->GetCommandResult(AgentName, CommandName, UpdTime, Id);

		AddCodeAndMsg(TmpObj, 0, L"", L"");
		if (Cnt != 0) {
			StkObject* TmpObjD = new StkObject(L"Data");
			for (int Loop = 0; Loop < Cnt; Loop++) {
				StkObject* TmpObjC = new StkObject(L"Result");
				wchar_t UpdTimeStr[DA_MAXLEN_OF_UNIXTIME * 2 + 1] = L"";
				StkPlSwPrintf(UpdTimeStr, DA_MAXLEN_OF_UNIXTIME * 2 + 1, L"%016x", UpdTime[Loop]);

				TmpObjC->AppendChildElement(new StkObject(L"AgentName", AgentName[Loop]));
				TmpObjC->AppendChildElement(new StkObject(L"CommandName", CommandName[Loop]));
				TmpObjC->AppendChildElement(new StkObject(L"UpdTime", UpdTimeStr));
				TmpObjC->AppendChildElement(new StkObject(L"Id", Id[Loop]));
				TmpObjD->AppendChildElement(TmpObjC);
			}
			TmpObj->AppendChildElement(TmpObjD);
		}
		*ResultCode = 200;
		return TmpObj;
	}
}
<|endoftext|>


<commit_before>// Copyright (c) 2017 offa
// Copyright 2011 Ciaran McHale.
//
// 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 "danek/internal/UidIdentifierProcessor.h"
#include "danek/internal/Util.h"
#include "danek/internal/Compat.h"
#include "danek/ConfigurationException.h"
#include "danek/StringVector.h"
#include <sstream>
#include <iomanip>
#include <string.h>
#include <ctype.h>
#include <stdio.h>

namespace danek
{
    namespace
    {
        std::string formatCount(std::size_t count)
        {
            constexpr std::size_t numDigits = 9;
            std::ostringstream ss;
            ss << std::setw(numDigits) << std::setfill('0') << count;
            return ss.str();
        }
    }

    UidIdentifierProcessor::UidIdentifierProcessor() : m_count(0), m_uidToken("uid-")
    {
    }

    //----------------------------------------------------------------------
    // Spelling must be in one of the following forms:
    //      - "foo"                 -->  "foo"
    //      - "uid-<foo>"           -->  "uid-<digits>-<foo>"
    //      - "uid-<digits>-<foo>"  -->  "uid-<new-digits>-<foo>"
    // where "<foo>" does NOT start with a digit or "-"
    //----------------------------------------------------------------------

    void UidIdentifierProcessor::expand(StringBuffer& spelling)
    {
        //--------
        // Common case optimizations
        //--------
        if (strchr(spelling.str().c_str(), '.') == nullptr)
        {
            spelling = expandOne(spelling.str());
            return;
        }
        if (strstr(spelling.str().c_str(), "uid-") == nullptr)
        {
            return;
        }

        //--------
        // Let's break apart the scoped name, expand each local part
        // and then recombine the parts into an expanded scoped name.
        //--------
        StringBuffer buf;
        StringBuffer msg;
        StringBuffer result;

        StringVector vec{util::splitScopes(spelling.str())};
        const int len = vec.size();
        for (int i = 0; i < len; ++i)
        {
            buf = vec[i];
            try
            {
                buf = expandOne(buf.str());
            }
            catch (const ConfigurationException&)
            {
                msg << "'" << spelling << "' is not a legal identifier";
                throw ConfigurationException(msg.str());
            }
            result.append(buf);
            if (i < len - 1)
            {
                result.append(".");
            }
        }
        spelling = result;
    }

    std::string UidIdentifierProcessor::expandOne(const std::string& spelling)
    {
        // If spelling does not start with "uid-" then do nothing.
        if( std::equal(m_uidToken.cbegin(), m_uidToken.cend(), spelling.cbegin()) == false )
        {
            return spelling;
        }

        const std::string errorMessage = "'" + spelling + "' is not a legal identifier";

        // Check for "uid-" (with no suffix) and "uid--foo", because that is illegal
        if( spelling == m_uidToken || spelling.at(m_uidToken.size()) == '-' )
        {
            throw ConfigurationException(errorMessage);
        }

        auto itr = std::next(spelling.cbegin(), m_uidToken.size()); // skip over "uid-"

        if (!isdigit(*itr))
        {
            //--------
            // "uid-foo"  --> "uid-<digits>-foo"
            //--------
            compat::checkAssertion(m_count < 1000 * 1000 * 1000);
            const std::string suffix = &(spelling.c_str()[4]); // deep copy

            std::stringstream ss;
            ss << "uid-" << formatCount(m_count) << "-" << suffix;
            ++m_count;
            return ss.str();
        }

        std::size_t count = 0;

        while (isdigit(*itr))
        {
            std::advance(itr, 1);
            ++count;
        }
        compat::checkAssertion(count > 0);
        if (*itr == '\0' || *itr != '-')
        {
            // illegal: "uid-<digits>" or "uid-<digits>foo"
            throw ConfigurationException(errorMessage);
        }
        std::advance(itr, 1); // point to just after "uid-<digits>-"
        if (*itr == '\0')
        {
            // illegal: "uid-<digits>-"
            throw ConfigurationException(errorMessage);
        }
        if (*itr == '-')
        {
            // illegal: "uid-<digits>--"
            throw ConfigurationException(errorMessage);
        }
        if (isdigit(*(itr)))
        {
            // illegal: "uid-<digits>-<digits>foo"
            throw ConfigurationException(errorMessage);
        }

        compat::checkAssertion(m_count < 1000 * 1000 * 1000);
        const std::string suffix(itr, spelling.cend()); // deep copy just after "uid-<digits>-"
        const std::string result(m_uidToken + formatCount(m_count) + "-" + suffix);
        ++m_count;
        return result;
    }

    const char* UidIdentifierProcessor::unexpand(const char* spelling, StringBuffer& buf) const
    {
        //--------
        // Common case optimizations
        //--------
        if (strchr(spelling, '.') == nullptr)
        {
            return unexpandOne(spelling, buf);
        }
        if (strstr(spelling, "uid-") == nullptr)
        {
            return spelling;
        }

        //--------
        // Let's break apart the scoped name, unexpand each local part
        // and then recombine the parts into an unexpanded scoped name.
        //--------
        StringBuffer result;
        StringBuffer msg;
        const char* str;

        StringVector vec{util::splitScopes(spelling)};
        const int len = vec.size();
        for (int i = 0; i < len; ++i)
        {
            try
            {
                str = unexpandOne(vec[i].c_str(), buf);
            }
            catch (const ConfigurationException&)
            {
                msg << "'" << spelling << "' is not a legal identifier";
                throw ConfigurationException(msg.str());
            }
            result.append(str);
            if (i < len - 1)
            {
                result.append(".");
            }
        }
        buf = result.str();
        return buf.str().c_str();
    }

    const char* UidIdentifierProcessor::unexpandOne(const char* spelling, StringBuffer& buf) const

    {
        int count;
        const char* ptr;

        //--------
        // If spelling does not start with "uid-<digits>-" then do nothing.
        //--------
        ptr = spelling;
        if (strncmp(ptr, "uid-", 4) != 0)
        {
            return spelling;
        }
        ptr += 4; // skip over "uid-"
        count = 0;
        while (isdigit(*ptr))
        {
            ++ptr;
            ++count;
        }
        if (count == 0 || *ptr != '-')
        {
            return spelling;
        }

        //--------
        // Okay, let's returned a modified spelling.
        //--------
        StringBuffer suffix;

        suffix = (ptr + 1); // deep copy from just after "uid-<digits>-"
        buf.clear();
        buf << "uid-" << suffix;
        return buf.str().c_str();
    }
}
<commit_msg>Some improvements.<commit_after>// Copyright (c) 2017 offa
// Copyright 2011 Ciaran McHale.
//
// 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 "danek/internal/UidIdentifierProcessor.h"
#include "danek/internal/Util.h"
#include "danek/internal/Compat.h"
#include "danek/ConfigurationException.h"
#include "danek/StringVector.h"
#include <sstream>
#include <iomanip>
#include <string.h>
#include <ctype.h>
#include <stdio.h>

namespace danek
{
    namespace
    {
        std::string formatCount(std::size_t count)
        {
            constexpr std::size_t numDigits = 9;
            std::ostringstream ss;
            ss << std::setw(numDigits) << std::setfill('0') << count;
            return ss.str();
        }
    }

    UidIdentifierProcessor::UidIdentifierProcessor() : m_count(0), m_uidToken("uid-")
    {
    }

    //----------------------------------------------------------------------
    // Spelling must be in one of the following forms:
    //      - "foo"                 -->  "foo"
    //      - "uid-<foo>"           -->  "uid-<digits>-<foo>"
    //      - "uid-<digits>-<foo>"  -->  "uid-<new-digits>-<foo>"
    // where "<foo>" does NOT start with a digit or "-"
    //----------------------------------------------------------------------

    void UidIdentifierProcessor::expand(StringBuffer& spelling)
    {
        //--------
        // Common case optimizations
        //--------
        if (strchr(spelling.str().c_str(), '.') == nullptr)
        {
            spelling = expandOne(spelling.str());
            return;
        }
        if (strstr(spelling.str().c_str(), "uid-") == nullptr)
        {
            return;
        }

        //--------
        // Let's break apart the scoped name, expand each local part
        // and then recombine the parts into an expanded scoped name.
        //--------
        StringBuffer buf;
        StringBuffer msg;
        StringBuffer result;

        StringVector vec{util::splitScopes(spelling.str())};
        const int len = vec.size();
        for (int i = 0; i < len; ++i)
        {
            buf = vec[i];
            try
            {
                buf = expandOne(buf.str());
            }
            catch (const ConfigurationException&)
            {
                msg << "'" << spelling << "' is not a legal identifier";
                throw ConfigurationException(msg.str());
            }
            result.append(buf);
            if (i < len - 1)
            {
                result.append(".");
            }
        }
        spelling = result;
    }

    std::string UidIdentifierProcessor::expandOne(const std::string& spelling)
    {
        // If spelling does not start with "uid-" then do nothing.
        if( std::equal(m_uidToken.cbegin(), m_uidToken.cend(), spelling.cbegin()) == false )
        {
            return spelling;
        }

        const std::string errorMessage = "'" + spelling + "' is not a legal identifier";

        // Check for "uid-" (with no suffix) and "uid--foo", because that is illegal
        if( spelling == m_uidToken || spelling.at(m_uidToken.size()) == '-' )
        {
            throw ConfigurationException(errorMessage);
        }

        auto itr = std::next(spelling.cbegin(), m_uidToken.size()); // skip over "uid-"

        if (!isdigit(*itr))
        {
            //--------
            // "uid-foo"  --> "uid-<digits>-foo"
            //--------
            compat::checkAssertion(m_count < 1000 * 1000 * 1000);
            const std::string suffix = &(spelling.c_str()[4]); // deep copy
            const std::string result = m_uidToken + formatCount(m_count) + "-" + suffix;
            ++m_count;
            return result;
        }

        std::size_t count = 0;

        while (isdigit(*itr))
        {
            std::advance(itr, 1);
            ++count;
        }
        compat::checkAssertion(count > 0);
        if (*itr == '\0' || *itr != '-')
        {
            // illegal: "uid-<digits>" or "uid-<digits>foo"
            throw ConfigurationException(errorMessage);
        }
        std::advance(itr, 1); // point to just after "uid-<digits>-"
        if (*itr == '\0')
        {
            // illegal: "uid-<digits>-"
            throw ConfigurationException(errorMessage);
        }
        if (*itr == '-')
        {
            // illegal: "uid-<digits>--"
            throw ConfigurationException(errorMessage);
        }
        if (isdigit(*(itr)))
        {
            // illegal: "uid-<digits>-<digits>foo"
            throw ConfigurationException(errorMessage);
        }

        compat::checkAssertion(m_count < 1000 * 1000 * 1000);
        const std::string suffix(itr, spelling.cend()); // deep copy just after "uid-<digits>-"
        const std::string result = m_uidToken + formatCount(m_count) + "-" + suffix;
        ++m_count;
        return result;
    }

    const char* UidIdentifierProcessor::unexpand(const char* spelling, StringBuffer& buf) const
    {
        //--------
        // Common case optimizations
        //--------
        if (strchr(spelling, '.') == nullptr)
        {
            return unexpandOne(spelling, buf);
        }
        if (strstr(spelling, "uid-") == nullptr)
        {
            return spelling;
        }

        //--------
        // Let's break apart the scoped name, unexpand each local part
        // and then recombine the parts into an unexpanded scoped name.
        //--------
        StringBuffer result;
        StringBuffer msg;
        const char* str;

        StringVector vec{util::splitScopes(spelling)};
        const int len = vec.size();
        for (int i = 0; i < len; ++i)
        {
            try
            {
                str = unexpandOne(vec[i].c_str(), buf);
            }
            catch (const ConfigurationException&)
            {
                msg << "'" << spelling << "' is not a legal identifier";
                throw ConfigurationException(msg.str());
            }
            result.append(str);
            if (i < len - 1)
            {
                result.append(".");
            }
        }
        buf = result.str();
        return buf.str().c_str();
    }

    const char* UidIdentifierProcessor::unexpandOne(const char* spelling, StringBuffer& buf) const

    {
        int count;
        const char* ptr;

        //--------
        // If spelling does not start with "uid-<digits>-" then do nothing.
        //--------
        ptr = spelling;
        if (strncmp(ptr, "uid-", 4) != 0)
        {
            return spelling;
        }
        ptr += 4; // skip over "uid-"
        count = 0;
        while (isdigit(*ptr))
        {
            ++ptr;
            ++count;
        }
        if (count == 0 || *ptr != '-')
        {
            return spelling;
        }

        //--------
        // Okay, let's returned a modified spelling.
        //--------
        StringBuffer suffix;

        suffix = (ptr + 1); // deep copy from just after "uid-<digits>-"
        buf.clear();
        buf << "uid-" << suffix;
        return buf.str().c_str();
    }
}
<|endoftext|>


<commit_before>/*
 * This is a sample of a UPnP service that runs on a IoT device.
 *
 * UPnP commands/queries can be used from an application or a script.
 * This service represents the BMP180 barometric pressure and temperature sensor.
 *
 * Copyright (c) 2015 Danny Backx
 *
 * License (MIT license):
 *   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 "UPnP.h"
#include "UPnP/UPnPService.h"
#include "SFE_BMP180.h"
#include "UPnP/BMP180SensorService.h"
#include "UPnP/WebServer.h"

extern WebServer HTTP;
static void GetVersion();

#undef DEBUG
// #define DEBUG Serial

// Printf style template, parameters : serviceType, state
static const char *gsh_template = "<u:GetStateResponse xmlns=\"%s\">\r\n<Temperature>%s</Temperature>\r\n<Pressure>%s</Pressure>\r\n</u:GetStateResponse>\r\n";

static const char *getStateXML = "<action>"
  "<name>getState</name>"
  "<argumentList>"
  "<argument>"
  "<retval/>"
  "<name>Temperature</name>"
  "<relatedStateVariable>Temperature</relatedStateVariable>"
  "<direction>out</direction>"
  "</argument>"
  "<argument>"
  "<retval/>"
  "<name>Pressure</name>"
  "<relatedStateVariable>Pressure</relatedStateVariable>"
  "<direction>out</direction>"
  "</argument>"
  "</argumentList>"
  "</action>";

static const char *getVersionXML = "<action>"
  "<name>getVersion</name>"
  "<argumentList>"
  "<argument>"
  "<retval/>"
  "<name>Version</name>"
  "<direction>out</direction>"
  "</argument>"
  "</argumentList>"
  "</action>";

// These are static/global variables as a demo that you can query such variables.
static const char *versionTemplate = "%s %s %s\n";
static const char *versionFileInfo = __FILE__;
static const char *versionDateInfo = __DATE__;
static const char *versionTimeInfo = __TIME__;

static const char *myServiceName = "PressureSensor";
static const char *myServiceType = "urn:danny-backx-info:service:bmp180sensor:1";
static const char *myServiceId = "urn:danny-backx-info:serviceId:bmp180sensor1";
static const char *temperatureString = "Temperature";
static const char *pressureString = "Pressure";
static const char *percentageString = "Percentage";
static const char *getStateString = "getState";
static const char *getVersionString = "getVersion";
static const char *stringString = "string";

BMP180SensorService::BMP180SensorService() :
  UPnPService(myServiceName, myServiceType, myServiceId)
{
  addAction(getStateString, static_cast<MemberActionFunction>(&BMP180SensorService::GetPressureHandler), getStateXML);
  addAction(getVersionString, GetVersion, getVersionXML);
  addStateVariable(temperatureString, stringString, true);
  addStateVariable(pressureString, stringString, true);
  addStateVariable(percentageString, stringString, false);
}

BMP180SensorService::BMP180SensorService(const char *deviceURN) :
  UPnPService(myServiceName, myServiceType, myServiceId)
{
  addAction(getStateString, static_cast<MemberActionFunction>(&BMP180SensorService::GetPressureHandler), getStateXML);
  addAction(getVersionString, GetVersion, getVersionXML);
  addStateVariable(temperatureString, stringString, true);
  addStateVariable(pressureString, stringString, true);
  addStateVariable(percentageString, stringString, false);
}

BMP180SensorService::BMP180SensorService(const char *serviceType, const char *serviceId) :
  UPnPService(myServiceName, serviceType, serviceId)
{
  addAction(getStateString, static_cast<MemberActionFunction>(&BMP180SensorService::GetPressureHandler), getStateXML);
  addAction(getVersionString, GetVersion, getVersionXML);
  addStateVariable(temperatureString, stringString, true);
  addStateVariable(pressureString, stringString, true);
  addStateVariable(percentageString, stringString, false);
}

BMP180SensorService::~BMP180SensorService() {
#ifdef DEBUG
  DEBUG.println("BMP180SensorService DTOR");
#endif
}

void BMP180SensorService::begin() {
#ifdef DEBUG
  DEBUG.println("BMP180SensorService::begin");
#endif
  temperature[0] = 0;
  pressure[0] = 0;

  config = new Configuration("BMP180",
    new ConfigurationItem("name", ""),
    new ConfigurationItem(percentageString, 3),
    NULL);
  UPnPService::begin(config);
  percentage = config->GetValue(percentageString);

  bmp = new SFE_BMP180();
  bmp->begin();

#ifdef DEBUG
  DEBUG.printf("BMP180::begin\n");
#endif
}

// Return true if a noticable difference
bool BMP180SensorService::Difference(float oldval, float newval) {
  float cmp1, cmp2;
  if (oldval == 0 && newval == 0)
    return false;
  if (oldval == 0)
    cmp1 = newval;
  else
    cmp1 = oldval;
  cmp2 = ((oldval - newval) / cmp1) * 100;
#ifdef DEBUG
  DEBUG.print("Difference ");
  DEBUG.print(cmp2);
  DEBUG.print(" = ");
  DEBUG.print(oldval);
  DEBUG.print(" - ");
  DEBUG.print(newval);
  DEBUG.print(" / ");
  DEBUG.print(cmp1);
  DEBUG.print(" * 100");
#endif

  if (cmp2 < 0)
    cmp2 = -cmp2;
  if (percentage < cmp2) {
#ifdef DEBUG
    DEBUG.println(" --> true");
    delay(10000);
#endif
    return true;
  }
#ifdef DEBUG
  DEBUG.println(" --> false");
  delay(10000);
#endif
  return false;
}

void BMP180SensorService::poll() {
  oldTemperature = newTemperature;
  oldPressure = newPressure;

  char d = bmp->startTemperature();
  delay(d);
  d = bmp->getTemperature(newTemperature);
  d = bmp->startPressure(0);
  delay(d);
  d = bmp->getPressure(newPressure, newTemperature);

  if (isnan(newTemperature) || isnan(newPressure)) {
    newTemperature = oldTemperature;
    newPressure = oldPressure;
    return;
  }

  if (Difference(oldTemperature, newTemperature)) {
    UpdateTemperature();
    SendNotify(temperatureString);
  }
  if (Difference(oldPressure, newPressure)) {
    UpdatePressure();
    SendNotify(pressureString);

#ifdef DEBUG
  DEBUG.print("BMP180 : Temp ");
  DEBUG.print(newTemperature);
  DEBUG.print(" Pressure ");
  DEBUG.println(newPressure);
#endif
  }
}

const char *BMP180SensorService::GetTemperature() {
  return temperature;
}

const char *BMP180SensorService::GetPressure() {
  return pressure;
}

// Example of a static function to handle UPnP requests : only access to global variables here.
static void GetVersion() {
  char msg[128];
  sprintf(msg, versionTemplate, versionFileInfo, versionDateInfo, versionTimeInfo);
  HTTP.send(200, UPnPClass::mimeTypeXML, msg);
}

// Example of a member function to handle UPnP requests : this can access stuff in the class
void BMP180SensorService::GetPressureHandler() {
  int l2 = strlen(gsh_template) + strlen(myServiceType) + BMP180_STATE_LENGTH * 2,
      l1 = strlen(UPnPClass::envelopeHeader) + l2 + strlen(UPnPClass::envelopeTrailer) + 5;
  char *tmp2 = (char *)malloc(l2),
       *tmp1 = (char *)malloc(l1);
#ifdef DEBUG
  DEBUG.println("BMP180SensorService::GetPressureHandler");
  delay(1000);
#endif
  strcpy(tmp1, UPnPClass::envelopeHeader);
  sprintf(tmp2, gsh_template, myServiceType,
    BMP180SensorService::temperature, BMP180SensorService::pressure);
  strcat(tmp1, tmp2);
  free(tmp2);
  strcat(tmp1, UPnPClass::envelopeTrailer);
  HTTP.send(200, UPnPClass::mimeTypeXML, tmp1);
  free(tmp1);
}

void BMP180SensorService::FloatToString(float f, char *s) {
  // sprintf(s, "%1.2f", f);
  int a = (int)f,
      b = (int)(100 * (f-(float)a));
  sprintf(s, "%d.%02d", a, b);
}

void BMP180SensorService::UpdateTemperature() {
  FloatToString(newTemperature, temperature);
}

void BMP180SensorService::UpdatePressure() {
  FloatToString(newPressure, pressure);
}
<commit_msg>Add some comments, make the code less verbose in DEBUG.<commit_after>/*
 * This is a sample of a UPnP service that runs on a IoT device.
 *
 * UPnP commands/queries can be used from an application or a script.
 * This service represents the BMP180 barometric pressure and temperature sensor.
 *
 * Copyright (c) 2015 Danny Backx
 *
 * License (MIT license):
 *   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 "UPnP.h"
#include "UPnP/UPnPService.h"
#include "SFE_BMP180.h"
#include "UPnP/BMP180SensorService.h"
#include "UPnP/WebServer.h"

extern WebServer HTTP;
static void GetVersion();

#undef DEBUG
// #define DEBUG Serial

// Printf style template, parameters : serviceType, state
static const char *gsh_template = "<u:GetStateResponse xmlns=\"%s\">\r\n<Temperature>%s</Temperature>\r\n<Pressure>%s</Pressure>\r\n</u:GetStateResponse>\r\n";

static const char *getStateXML = "<action>"
  "<name>getState</name>"
  "<argumentList>"
  "<argument>"
  "<retval/>"
  "<name>Temperature</name>"
  "<relatedStateVariable>Temperature</relatedStateVariable>"
  "<direction>out</direction>"
  "</argument>"
  "<argument>"
  "<retval/>"
  "<name>Pressure</name>"
  "<relatedStateVariable>Pressure</relatedStateVariable>"
  "<direction>out</direction>"
  "</argument>"
  "</argumentList>"
  "</action>";

static const char *getVersionXML = "<action>"
  "<name>getVersion</name>"
  "<argumentList>"
  "<argument>"
  "<retval/>"
  "<name>Version</name>"
  "<direction>out</direction>"
  "</argument>"
  "</argumentList>"
  "</action>";

// These are static/global variables as a demo that you can query such variables.
static const char *versionTemplate = "%s %s %s\n";
static const char *versionFileInfo = __FILE__;
static const char *versionDateInfo = __DATE__;
static const char *versionTimeInfo = __TIME__;

static const char *myServiceName = "PressureSensor";
static const char *myServiceType = "urn:danny-backx-info:service:bmp180sensor:1";
static const char *myServiceId = "urn:danny-backx-info:serviceId:bmp180sensor1";
static const char *temperatureString = "Temperature";
static const char *pressureString = "Pressure";
static const char *percentageString = "Percentage";
static const char *getStateString = "getState";
static const char *getVersionString = "getVersion";
static const char *stringString = "string";
static const int defaultPercentage = 3;		// Percentage, see poll().

BMP180SensorService::BMP180SensorService() :
  UPnPService(myServiceName, myServiceType, myServiceId)
{
  addAction(getStateString, static_cast<MemberActionFunction>(&BMP180SensorService::GetPressureHandler), getStateXML);
  addAction(getVersionString, GetVersion, getVersionXML);
  addStateVariable(temperatureString, stringString, true);
  addStateVariable(pressureString, stringString, true);
  addStateVariable(percentageString, stringString, false);
}

BMP180SensorService::BMP180SensorService(const char *deviceURN) :
  UPnPService(myServiceName, myServiceType, myServiceId)
{
  addAction(getStateString, static_cast<MemberActionFunction>(&BMP180SensorService::GetPressureHandler), getStateXML);
  addAction(getVersionString, GetVersion, getVersionXML);
  addStateVariable(temperatureString, stringString, true);
  addStateVariable(pressureString, stringString, true);
  addStateVariable(percentageString, stringString, false);
}

BMP180SensorService::BMP180SensorService(const char *serviceType, const char *serviceId) :
  UPnPService(myServiceName, serviceType, serviceId)
{
  addAction(getStateString, static_cast<MemberActionFunction>(&BMP180SensorService::GetPressureHandler), getStateXML);
  addAction(getVersionString, GetVersion, getVersionXML);
  addStateVariable(temperatureString, stringString, true);
  addStateVariable(pressureString, stringString, true);
  addStateVariable(percentageString, stringString, false);
}

BMP180SensorService::~BMP180SensorService() {
#ifdef DEBUG
  DEBUG.println("BMP180SensorService DTOR");
#endif
}

void BMP180SensorService::begin() {
#ifdef DEBUG
  DEBUG.println("BMP180SensorService::begin");
#endif
  temperature[0] = 0;
  pressure[0] = 0;

  config = new Configuration("BMP180",
    new ConfigurationItem("name", ""),
    new ConfigurationItem(percentageString, defaultPercentage),
    NULL);
  UPnPService::begin(config);
  percentage = config->GetValue(percentageString);

  bmp = new SFE_BMP180();
  bmp->begin();
}

// Return true if a noticable difference
bool BMP180SensorService::Difference(float oldval, float newval) {
  float cmp1, cmp2;
  if (oldval == 0 && newval == 0)
    return false;
  if (oldval == 0)
    cmp1 = newval;
  else
    cmp1 = oldval;
  cmp2 = ((oldval - newval) / cmp1) * 100;
#ifdef DEBUGx
  DEBUG.print("Difference ");
  DEBUG.print(cmp2);
  DEBUG.print(" = ");
  DEBUG.print(oldval);
  DEBUG.print(" - ");
  DEBUG.print(newval);
  DEBUG.print(" / ");
  DEBUG.print(cmp1);
  DEBUG.print(" * 100");
#endif

  if (cmp2 < 0)
    cmp2 = -cmp2;
  if (percentage < cmp2) {
#ifdef DEBUGx
    DEBUG.println(" --> true");
#endif
    return true;
  }
#ifdef DEBUGx
  DEBUG.println(" --> false");
#endif
  return false;
}

/*
 * Query the sensor. This is an I2C device, but all that complexity is in a separate class.
 *
 * This method makes sure we report changes only if they exceed a settable percentage.
 * (Working with float readings requires something like this.)
 */
void BMP180SensorService::poll() {
  oldTemperature = newTemperature;
  oldPressure = newPressure;

  // This is a multi-part query to the I2C device, see the SFE_BMP180 source files.
  char d = bmp->startTemperature();
  delay(d);
  d = bmp->getTemperature(newTemperature);
  d = bmp->startPressure(0);
  delay(d);
  d = bmp->getPressure(newPressure, newTemperature);

  if (isnan(newTemperature) || isnan(newPressure)) {
    newTemperature = oldTemperature;
    newPressure = oldPressure;
    return;
  }

  if (Difference(oldTemperature, newTemperature)) {
    UpdateTemperature();
    SendNotify(temperatureString);
  }
  if (Difference(oldPressure, newPressure)) {
    UpdatePressure();
    SendNotify(pressureString);

#ifdef DEBUG
  DEBUG.print("BMP180 : Temp ");
  DEBUG.print(newTemperature);
  DEBUG.print(" Pressure ");
  DEBUG.println(newPressure);
#endif
  }
}

const char *BMP180SensorService::GetTemperature() {
  return temperature;
}

const char *BMP180SensorService::GetPressure() {
  return pressure;
}

// Example of a static function to handle UPnP requests : only access to global variables here.
static void GetVersion() {
  char msg[128];
  sprintf(msg, versionTemplate, versionFileInfo, versionDateInfo, versionTimeInfo);
  HTTP.send(200, UPnPClass::mimeTypeXML, msg);
}

// Example of a member function to handle UPnP requests : this can access stuff in the class
void BMP180SensorService::GetPressureHandler() {
  int l2 = strlen(gsh_template) + strlen(myServiceType) + BMP180_STATE_LENGTH * 2,
      l1 = strlen(UPnPClass::envelopeHeader) + l2 + strlen(UPnPClass::envelopeTrailer) + 5;
  char *tmp2 = (char *)malloc(l2),
       *tmp1 = (char *)malloc(l1);
#ifdef DEBUG
  DEBUG.println("BMP180SensorService::GetPressureHandler");
  delay(1000);
#endif
  strcpy(tmp1, UPnPClass::envelopeHeader);
  sprintf(tmp2, gsh_template, myServiceType,
    BMP180SensorService::temperature, BMP180SensorService::pressure);
  strcat(tmp1, tmp2);
  free(tmp2);
  strcat(tmp1, UPnPClass::envelopeTrailer);
  HTTP.send(200, UPnPClass::mimeTypeXML, tmp1);
  free(tmp1);
}

void BMP180SensorService::FloatToString(float f, char *s) {
  // sprintf(s, "%1.2f", f);
  int a = (int)f,
      b = (int)(100 * (f-(float)a));
  sprintf(s, "%d.%02d", a, b);
}

void BMP180SensorService::UpdateTemperature() {
  FloatToString(newTemperature, temperature);
}

void BMP180SensorService::UpdatePressure() {
  FloatToString(newPressure, pressure);
}
<|endoftext|>