From 1fed1ad34976812c3fc574743bdb115d9f75a51c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 26 Mar 2015 17:55:28 -1000 Subject: PX4 Cannode Demo DO NOT MERGE --- src/modules/uavcan/actuators/esc.cpp | 44 ++++++++++++++++++++++++++++------ src/modules/uavcan/actuators/esc.hpp | 11 ++++++++- src/modules/uavcan/uavcan_main.cpp | 46 +++++++++++++++++++++++++++++------- src/modules/uavcan/uavcan_main.hpp | 3 ++- 4 files changed, 87 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 51589e43e..c5cf902f1 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -45,10 +45,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), - _uavcan_pub_raw_cmd(node), + _uavcan_pub_raw_cmd(node), _uavcan_sub_status(node), - _orb_timer(node) + _orb_timer(node), + _uavcan_pub_rgb_cmd(node) { + _led_test = false; if (_perfcnt_invalid_input == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); } @@ -137,6 +139,23 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Note that for a quadrotor it takes one CAN frame */ (void)_uavcan_pub_raw_cmd.broadcast(msg); + + + if (_led_test) { + uavcan::equipment::indication::LightsCommand rgb; + uavcan::equipment::indication::SingleLightCommand sc; + sc.color.red = _red; + sc.color.blue = _blue; + sc.color.green = _green; + sc.light_id = 0; + rgb.commands.push_back(sc); + sc.light_id = 1; + sc.color.red = _Hz; + sc.color.green = _Hz; + sc.color.blue = _Hz; + rgb.commands.push_back(sc); + (void)_uavcan_pub_rgb_cmd.broadcast(rgb); + } } void UavcanEscController::arm_all_escs(bool arm) @@ -150,11 +169,22 @@ void UavcanEscController::arm_all_escs(bool arm) void UavcanEscController::arm_single_esc(int num, bool arm) { - if (arm) { - _armed_mask = MOTOR_BIT(num); - } else { - _armed_mask = 0; - } + if (arm) { + _armed_mask = MOTOR_BIT(num); + } else { + _armed_mask = 0; + } +} + +void UavcanEscController::leds(int r, int g, int b, int hz) +{ + _led_test = false; + _red = r; + _green = g; + _blue = b; + _Hz = hz; + _led_test = r || g || b || hz; + } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c035542..af742e00f 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -46,6 +46,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,7 @@ public: void arm_all_escs(bool arm); void arm_single_esc(int num, bool arm); + void leds(int r, int g, int b, int hz); private: /** @@ -90,14 +92,21 @@ private: esc_status_s _esc_status = {}; orb_advert_t _esc_status_pub = -1; + bool _led_test; + uint8_t _red; + uint8_t _green; + uint8_t _blue; + uint8_t _Hz; + /* * libuavcan related things */ uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting uavcan::INode &_node; - uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::Publisher _uavcan_pub_raw_cmd; uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; + uavcan::Publisher _uavcan_pub_rgb_cmd; /* * ESC states diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index b93a95f96..58d53ef2e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -514,9 +514,17 @@ UavcanNode::teardown() int UavcanNode::arm_actuators(bool arm) { - _is_armed = arm; - _esc_controller.arm_all_escs(arm); - return OK; + _is_armed = arm; + _esc_controller.arm_all_escs(arm); + return OK; +} + +int +UavcanNode::leds(int r, int g, int b, int hz) +{ + + _esc_controller.leds(r, g, b, hz); + return OK; } void @@ -684,7 +692,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop|arm|disarm}"); + "\tuavcan {start|status|stop|arm|disarm|led r g b hz}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -731,10 +739,32 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } - if (!std::strcmp(argv[1], "arm")) { - inst->arm_actuators(true); - ::exit(0); - } + if (!std::strcmp(argv[1], "arm")) { + inst->arm_actuators(true); + ::exit(0); + } + + if (!std::strcmp(argv[1], "led")) { + + int r = 255; + int g = 255; + int b = 255; + int hz = 2; + if (argc > 2) { + r = atol(argv[2]); + } + if (argc > 3) { + g = atol(argv[3]); + } + if (argc > 4) { + b = atol(argv[4]); + } + if (argc > 5) { + hz = atol(argv[5]); + } + inst->leds(r, g, b, hz); + ::exit(0); + } if (!std::strcmp(argv[1], "disarm")) { inst->arm_actuators(false); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 96b3ec1a6..ba7efb51c 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -90,7 +90,8 @@ public: void subscribe(); int teardown(); - int arm_actuators(bool arm); + int arm_actuators(bool arm); + int leds(int r, int g, int b, int hz = 2); void print_info(); -- cgit v1.2.3