diff options
author | David Sidrane <david_s5@nscdg.com> | 2015-03-26 05:33:06 -1000 |
---|---|---|
committer | David Sidrane <david_s5@nscdg.com> | 2015-03-26 05:33:06 -1000 |
commit | 5d45d863f6bb4ba3e645cee07f89478482879b2b (patch) | |
tree | 26b813c50c5b7582d700f2d6b3c821de081d8e5f /src | |
parent | 2b1e9c620246738fa59caff7f702a5ec3cbe05b6 (diff) | |
download | px4-firmware-5d45d863f6bb4ba3e645cee07f89478482879b2b.tar.gz px4-firmware-5d45d863f6bb4ba3e645cee07f89478482879b2b.tar.bz2 px4-firmware-5d45d863f6bb4ba3e645cee07f89478482879b2b.zip |
Wip Added uavcan functionality and led driver
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/uavcannode/indication_controller.cpp | 84 | ||||
-rw-r--r-- | src/modules/uavcannode/indication_controller.hpp | 41 | ||||
-rw-r--r-- | src/modules/uavcannode/led.cpp | 76 | ||||
-rw-r--r-- | src/modules/uavcannode/led.hpp | 38 | ||||
-rw-r--r-- | src/modules/uavcannode/sim_controller.cpp | 150 | ||||
-rw-r--r-- | src/modules/uavcannode/sim_controller.hpp | 41 |
6 files changed, 430 insertions, 0 deletions
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 <pavel.kirienko@gmail.com> + * + * 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 <systemlib/systemlib.h> +#include "indication_controller.hpp" +#include <uavcan/equipment/indication/LightsCommand.hpp> +#include "led.hpp" + +namespace +{ +unsigned self_light_index = 0; + +void cb_light_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand>& 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<uavcan::uint32_t>(red, 0xFFU); + green = uavcan::min<uavcan::uint32_t>(green, 0xFFU); + blue = uavcan::min<uavcan::uint32_t>(blue, 0xFFU); + + rgb_led(red, green, blue, 0); + break; + } + } +} +} +int init_indication_controller(uavcan::INode& node) +{ + static uavcan::Subscriber<uavcan::equipment::indication::LightsCommand> 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 <pavel.kirienko@gmail.com> + * + * 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 <uavcan_stm32/uavcan_stm32.hpp> + + +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<david_s5@nscdg.com> + * + * 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 <nuttx/config.h> + +#include <stdint.h> + +#include <arch/board/board.h> +#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<david_s5@nscdg.com> + * + * 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 <systemlib/visibility.h> + +__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 <pavel.kirienko@gmail.com> + * David Sidrane<david_s5@nscdg.com> + * + * 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 <nuttx/config.h> + +#include <syslog.h> +#include <systemlib/systemlib.h> + +#include "sim_controller.hpp" +#include <uavcan/equipment/esc/RawCommand.hpp> +#include <uavcan/equipment/esc/RPMCommand.hpp> +#include <uavcan/equipment/esc/Status.hpp> +#include "led.hpp" + + +uavcan::Publisher<uavcan::equipment::esc::Status>* pub_status; +namespace { +unsigned self_index = 0; +int rpm = 0; +bool on = false; + + +static void cb_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& 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<uavcan::equipment::esc::RPMCommand>& 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<unsigned>(.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<uavcan::equipment::esc::RawCommand> sub_raw_command(node); + static uavcan::Subscriber<uavcan::equipment::esc::RPMCommand> sub_rpm_command(node); + static uavcan::TimerEventForwarder<cb> 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<uavcan::equipment::esc::Status>(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 <pavel.kirienko@gmail.com> + * David Sidrane<david_s5@nscdg.com> + * + * 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 <uavcan_stm32/uavcan_stm32.hpp> + +int init_sim_controller(uavcan::INode& node); + |