From 5d45d863f6bb4ba3e645cee07f89478482879b2b Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 26 Mar 2015 05:33:06 -1000 Subject: Wip Added uavcan functionality and led driver --- src/modules/uavcannode/indication_controller.cpp | 84 +++++++++++++ src/modules/uavcannode/indication_controller.hpp | 41 +++++++ src/modules/uavcannode/led.cpp | 76 ++++++++++++ src/modules/uavcannode/led.hpp | 38 ++++++ src/modules/uavcannode/sim_controller.cpp | 150 +++++++++++++++++++++++ src/modules/uavcannode/sim_controller.hpp | 41 +++++++ 6 files changed, 430 insertions(+) create mode 100644 src/modules/uavcannode/indication_controller.cpp create mode 100644 src/modules/uavcannode/indication_controller.hpp create mode 100644 src/modules/uavcannode/led.cpp create mode 100644 src/modules/uavcannode/led.hpp create mode 100644 src/modules/uavcannode/sim_controller.cpp create mode 100644 src/modules/uavcannode/sim_controller.hpp (limited to 'src') diff --git a/src/modules/uavcannode/indication_controller.cpp b/src/modules/uavcannode/indication_controller.cpp new file mode 100644 index 000000000..100f43472 --- /dev/null +++ b/src/modules/uavcannode/indication_controller.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "indication_controller.hpp" +#include +#include "led.hpp" + +namespace +{ +unsigned self_light_index = 0; + +void cb_light_command(const uavcan::ReceivedDataStructure& msg) +{ + for (auto& cmd : msg.commands) + { + if (cmd.light_id == self_light_index) + { + using uavcan::equipment::indication::RGB565; + + uavcan::uint32_t red = uavcan::uint32_t(float(cmd.color.red) * + (255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F); + + uavcan::uint32_t green = uavcan::uint32_t(float(cmd.color.green) * + (255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F); + + uavcan::uint32_t blue = uavcan::uint32_t(float(cmd.color.blue) * + (255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F); + + red = uavcan::min(red, 0xFFU); + green = uavcan::min(green, 0xFFU); + blue = uavcan::min(blue, 0xFFU); + + rgb_led(red, green, blue, 0); + break; + } + } +} +} +int init_indication_controller(uavcan::INode& node) +{ + static uavcan::Subscriber sub_light(node); + + self_light_index = 0; + + int res = 0; + + res = sub_light.start(cb_light_command); + if (res != 0) { + return res; + } + return 0; +} diff --git a/src/modules/uavcannode/indication_controller.hpp b/src/modules/uavcannode/indication_controller.hpp new file mode 100644 index 000000000..1b78e2872 --- /dev/null +++ b/src/modules/uavcannode/indication_controller.hpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + + +int init_indication_controller(uavcan::INode& node); + diff --git a/src/modules/uavcannode/led.cpp b/src/modules/uavcannode/led.cpp new file mode 100644 index 000000000..af57edd15 --- /dev/null +++ b/src/modules/uavcannode/led.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include + +#include + +#include +#include "chip/stm32_tim.h" + + +#include "led.hpp" + +void rgb_led(int r, int g ,int b, int freqs) +{ + + long fosc = 72000000; + long prescale = 2048; + long p1s = fosc/prescale; + long p0p5s = p1s/2; + stm32_tim_channel_t mode = (stm32_tim_channel_t)(STM32_TIM_CH_OUTPWM | STM32_TIM_CH_POLARITY_NEG); + static struct stm32_tim_dev_s *tim = 0; + + if (tim == 0) { + tim = stm32_tim_init(3); + STM32_TIM_SETMODE(tim, STM32_TIM_MODE_UP); + STM32_TIM_SETCLOCK(tim, p1s-8); + STM32_TIM_SETPERIOD(tim, p1s); + STM32_TIM_SETCOMPARE(tim, 1, 0); + STM32_TIM_SETCOMPARE(tim, 2, 0); + STM32_TIM_SETCOMPARE(tim, 3, 0); + STM32_TIM_SETCHANNEL(tim, 1, mode); + STM32_TIM_SETCHANNEL(tim, 2, mode); + STM32_TIM_SETCHANNEL(tim, 3, mode); + } + + long p = freqs==0 ? p1s : p1s/freqs; + STM32_TIM_SETPERIOD(tim, p); + + p = freqs==0 ? p1s+1 : p0p5s/freqs; + + STM32_TIM_SETCOMPARE(tim, 2, (r * p)/255 ); + STM32_TIM_SETCOMPARE(tim, 1, (b * p)/255); + STM32_TIM_SETCOMPARE(tim, 3, (g * p)/255); +} + diff --git a/src/modules/uavcannode/led.hpp b/src/modules/uavcannode/led.hpp new file mode 100644 index 000000000..53d8d653f --- /dev/null +++ b/src/modules/uavcannode/led.hpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include + +__BEGIN_DECLS +void rgb_led(int r, int g ,int b, int freqs); +__END_DECLS diff --git a/src/modules/uavcannode/sim_controller.cpp b/src/modules/uavcannode/sim_controller.cpp new file mode 100644 index 000000000..816ce2e8d --- /dev/null +++ b/src/modules/uavcannode/sim_controller.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include + +#include +#include + +#include "sim_controller.hpp" +#include +#include +#include +#include "led.hpp" + + +uavcan::Publisher* pub_status; +namespace { +unsigned self_index = 0; +int rpm = 0; +bool on = false; + + +static void cb_raw_command(const uavcan::ReceivedDataStructure& msg) +{ + if (msg.cmd.size() <= self_index) { + on = false; + rgb_led(0,0,0,0); + return; + } + + const float scaled = msg.cmd[self_index] / float(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max()); + + ::syslog(LOG_INFO,"scaled:%09.6f\n",(double)scaled); + if (scaled > 0) { + on = true; + rgb_led(255,0,0,0); + } else { + on = false; + rgb_led(0,0,0,0); + } +} + +static void cb_rpm_command(const uavcan::ReceivedDataStructure& msg) +{ + if (msg.rpm.size() <= self_index) { + rgb_led(0,0,0,0); + return; + } + + rpm = msg.rpm[self_index]; + ::syslog(LOG_INFO,"rpm:%d\n",rpm); + if (rpm > 0) { + on = true; + rgb_led(255,0,0,rpm); + } else { + on = false; + rgb_led(0,0,0,0); + } +} + +void cb_10Hz(const uavcan::TimerEvent& event) +{ + uavcan::equipment::esc::Status msg; + + msg.esc_index = self_index; + msg.rpm = rpm; + msg.voltage = 3.3F; + msg.current = 0.001F; + msg.temperature = 24.0F; + msg.power_rating_pct = static_cast(.5F * 100 + 0.5F); + msg.error_count = 0; + + if (!on) { + // Lower the publish rate to 1Hz if the motor is not running + static uavcan::MonotonicTime prev_pub_ts; + if ((event.scheduled_time - prev_pub_ts).toMSec() >= 990) { + prev_pub_ts = event.scheduled_time; + pub_status->broadcast(msg); + } + } else { + pub_status->broadcast(msg); + } +} + +} +int init_sim_controller(uavcan::INode& node) +{ + + typedef void (*cb)(const uavcan::TimerEvent&); + static uavcan::Subscriber sub_raw_command(node); + static uavcan::Subscriber sub_rpm_command(node); + static uavcan::TimerEventForwarder timer_10hz(node); + + self_index = 0; + + int res = 0; + + res = sub_raw_command.start(cb_raw_command); + if (res != 0) { + return res; + } + + res = sub_rpm_command.start(cb_rpm_command); + if (res != 0) { + return res; + } + + pub_status = new uavcan::Publisher(node); + res = pub_status->init(); + if (res != 0) { + return res; + } + + timer_10hz.setCallback(&cb_10Hz); + timer_10hz.startPeriodic(uavcan::MonotonicDuration::fromMSec(100)); + + return 0; +} + diff --git a/src/modules/uavcannode/sim_controller.hpp b/src/modules/uavcannode/sim_controller.hpp new file mode 100644 index 000000000..06f5312cd --- /dev/null +++ b/src/modules/uavcannode/sim_controller.hpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +int init_sim_controller(uavcan::INode& node); + -- cgit v1.2.3