To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 8a1314d2 authored by remartin's avatar remartin
Browse files

Added copyright headers, cosmetic changes. Appended Arduino file.

parent 46222d21
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Handles the connection to the Arduino via a serial port.
// Based on previously existing SerialUnix file.
//
// ----------------------------------------------------------------------------------
#pragma once
......@@ -18,8 +52,6 @@
//SERIAL COMMUNICATION PARAMETERS
#define MAX_SERIAL_BUFFER_SIZE 180
//#define SERIAL_BAUD_RATE B921600
#define SERIAL_LIGHT_BAUD_RATE B9600
#define SERIAL_BYTE_SIZE 8
......@@ -40,19 +72,22 @@
class SerialLight
{
class SerialLight{
public:
SerialLight();
// Opens the connection. Needs to detect the
// correct port first.
void init();
void closeConnection();
//bool isConnected(const char* port = "");
bool isConnected();
// send something through the port.
bool writeSerialLight(const char* buffer);
int manageSerial();
// redundant
std::string readSerialLight();
private:
......
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// ROS node that listens to UWB position data
// and handles the spotlight calibration procedure.
//
// ----------------------------------------------------------------------------------
#ifndef UWBSPOTLIGHT_MAIN_H
......@@ -13,16 +42,42 @@
#include "std_msgs/UInt32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/UWBAnchor.h"
#include "d_fall_pps/UWBAnchorArray.h"
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include <stdio.h>
#include <string>
#include <vector>
#include <ros/package.h>
#include <rosbag/bag.h>
#include <ros/ros.h>
#include "spotlightManager.h"
namespace d_fall_pps{
spotlightManager slM;
ros::ServiceClient uwbManager;
int spotlightAnchor = 5; // Anchor placed next to the spotlight
bool placedSpotlight = false;
//bool foundTarget = false;
bool finishedCalibration = false;
int frequencyDivision = 150; // 67 Hz by UWB.
int clockState = 1;
// Received data from the target/crazyflie
void updateTargetPositionCallback(const CrazyflieData &data);
void anchorArrangementChangedCallback(const std_msgs::Int32& msg);
// reading one necessary anchor value
void placeSpotlight();
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
}
#endif
......
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Deals with Coordinate transformations and translation into DMX values.
// Provides various functions used for the calibration procedure
// and testing purposes.
//
// ----------------------------------------------------------------------------------
#pragma once
#include<stdio.h>
#include <stdio.h>
#include "SerialLight.h"
#include <list>
......@@ -10,39 +46,48 @@
#include <cmath>
#include <sstream>
#include <iostream>
#include <termios.h> //For serial communication
class spotlightManager{
public:
spotlightManager();
void someDefaultSettings();
void setPTmanually(bool blockOverhead);
// toggles some pan values.
void trivialDemo();
// changes the dmx values directly.
void lineOnWall(bool ptMode, int msInterval);
// uses actual coordinates
void testPTupdater();
// Turns on the lamp.
void someDefaultSettings();
void setTargetPosition(double x, double y, double z);
void setSpotlightPosition(double x, double y, double z);
// calculates pan and tilt.
void updatePTvalues();
// private afterwards:
// WASD key control
void setPTmanually(bool blockOverlap);
void updateSpherical();
void updatePTvalues();
void setPositions(double spotlightPosition[3], double targetPosition[3]);
// Procedure to manually point the head at the target.
void calibrateOrientation();
// Reacts to yaw changes of the target.
void fancyColourFunction(double yaw);
// toggles some pan values.
void trivialDemo();
// changes the dmx values directly.
void lineOnWall(bool ptMode, int msInterval);
// changes actual coordinates.
void testPTupdater();
private:
int posPT;
int posPT; //Where the four PT values start.
SerialLight sL;
const static char portLocation[];
unsigned char defaultChannel; // value between 0 and 255...
......@@ -57,6 +102,8 @@ private:
double spherical[3]; // saving that is bad practice?
//void calibrateOrientation();
void updateSpherical();
// construct strings that can be decoded by the arduino
std::string buildMessage(); // 'bad practice to import namespace std in header'
std::string buildPT();
......
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Arduino program required to control the spotlight.
// Listens to serial port and translates int the DMX protocol.
//
// ----------------------------------------------------------------------------------
#include <DmxSimple.h>
const byte channelMode = 25; // The spotlight requires all values.
byte currentDmxMessage[channelMode];
byte channelOffset = 0; // corresponding to the device addressed.
byte posPT = 3; // offset of the pan/tilt DMX values.
unsigned long time = 0;
// open the serial connection.
void setup(){
Serial.begin(9600); // 9600 baud
Serial.println("Arduino_DMX has started.");
Serial.println();
Serial.println("Syntax:");
Serial.print(" c*c : DMX channel index is *. ");
Serial.println("Must be below 128-(14/25) due to Arduino limitations.");
Serial.println(" m199,255,40,10,0,33,m : some DMX command. Comma at the end! ");
Serial.println(" p200,200,30,0,p : setting pan, pan fine, tilt, tilt fine only. ");
Serial.println();
Serial.println();
sendFullDmxMessage(); // required to send it out once.
}
void loop() {
// t2 = milis()
//printCurrentDmxMessage();
//sendDmxMessage();
}
void sendFullDmxMessage(){
//Serial.print("Sent out (ch/Setting):");
for(int gg = channelOffset; gg < channelOffset + channelMode; gg++){
//if(gg>128) Serial.println(std7); covered when setting
DmxSimple.write(gg, currentDmxMessage[gg-channelOffset]);
// Serial.print(gg);
// Serial.print("/");
// Serial.print(currentDmxMessage[gg-channelOffset]);
// Serial.print(", ");
}
// Serial.println();
}
void printCurrentDmxMessage(){
Serial.print("Msg atm: ");
for(int a = channelOffset; a < channelOffset + 14; a++){
Serial.print(currentDmxMessage[a-channelOffset]);
Serial.print(", ");
}
Serial.println();
}
int parserTemp = 0;
int i = 0; // Offset in the currentDmxMessage-Array
bool readingMessage = false;
bool readingChannelOffset = false;
bool readingPT = false;
// Reads messages from the serial port: Either new channel numbers (= channelOffset) or DmxMessages.
void serialEvent(){
//Serial.println("Received some stuff here");
//Serial.print("Received nr. of bytes: ");
//Serial.println(Serial.available());
while(Serial.available()){
char ch = Serial.read();
//Serial.write(ch); // Necessary as ACK?
// Detection structure: dm
// Problem: Arduino sends some info by default -> needs to match precise format.
if(!readingMessage && ch == 'm'){
readingMessage = true;
i = 0;
time = micros();
}else if(!readingChannelOffset && ch == 'c'){
readingChannelOffset = true;
}else if(!readingPT && ch == 'p'){
readingPT = true;
i = 3;
time = micros();
}else if(readingMessage){
if(ch == 'm'){//End reached
parserTemp = 0;
i = 0; // reset array offset
readingMessage = false;
time = micros() - time;
Serial.print("Parsing took uS: ");
Serial.println(time, DEC);
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
Serial.println("Warning: DMX-Range"); //(std3);
}else{
currentDmxMessage[i] = parserTemp;
DmxSimple.write(channelOffset+i, parserTemp);
}
parserTemp = 0;
i++;
}else if(isDigit(ch)){
parserTemp = 10*parserTemp + (ch-'0');
}else{// abort that attempt and reset everything.
parserTemp = 0;
i = 0;
readingMessage = false;
//Serial.println("Error while parsing a message.");
}
}else if(readingChannelOffset){
if(ch == 'c'){//End of channel setting
if(parserTemp>103){
Serial.println("Warning: ChNr");//(std4);
}else{
channelOffset = parserTemp;
Serial.print("New offset: ");
Serial.print(channelOffset);
Serial.println();
}
parserTemp = 0;
readingChannelOffset = false;
}else if(isDigit(ch)){
parserTemp = 10*parserTemp + (ch-'0');
}else{
parserTemp = 0;
readingChannelOffset = false;
//Serial.println("Error while parsing a channel number.");
}
}else if(readingPT){
if(ch == 'p'){//End reached
parserTemp = 0;
i = 0; // reset array offset
readingPT = false;
time = micros() - time;
Serial.print("Parsing a PT took uS: ");
Serial.println(time, DEC);
//printCurrentDmxMessage();
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
Serial.println("Warning: DMX-Range"); //(std3);
}else{
currentDmxMessage[i] = parserTemp;
DmxSimple.write(channelOffset+i, parserTemp);
}
parserTemp = 0;
i++;
}else if(isDigit(ch)){
parserTemp = 10*parserTemp + (ch-'0');
}else{// abort that attempt and reset everything.
parserTemp = 0;
i = 0;
readingPT = false;
//Serial.println("Error while parsing a PT.");
}
}else{// all other stuff
// Arduino sends some info stuff at startup -> need to disable those error messages.
// Serial.println("Received Gibberish. That one:");
// Serial.write(ch);
}
}
}
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Handles the connection to the Arduino via a serial port.
// Based on previously existing SerialUnix file.
//
// ----------------------------------------------------------------------------------
#include "SerialLight.h"
......@@ -10,11 +44,11 @@ SerialLight::SerialLight(){
void SerialLight::init(){
// The Device could be located at ttyACM0 or ttyACM1.
// Need to determine which. Or just hard-code the path.
// The device could be located at ttyACM0 or ttyACM1.
// Need to determine which.
printf("Searching for the /dev file linked to the Arduino.\n");
// the command: get device infos, suppress error stream, filter for arduino, count lines.
// The command: get device infos, suppress error stream, filter for arduino, count lines.
char command[] = "udevadm info /dev/ttyACM0 2> /dev/null | grep arduino | wc -l > rubbishBin.txt";
char path[] = "/dev/ttyACM0";
......@@ -40,6 +74,7 @@ void SerialLight::init(){
}
}
if(!foundDevice){printf("Can't find the Arduino. Is it plugged in? \n");return;}
startConnection(path);
......@@ -52,7 +87,8 @@ int SerialLight::startConnection(const char* port){
_port = "";
_isConnected = false;
tty_fd = open(port, O_RDWR | O_NONBLOCK | O_NOCTTY | O_DSYNC); // both read and write, open nonblockingly
tty_fd = open(port, O_RDWR | O_NONBLOCK | O_NOCTTY | O_DSYNC);
// both read and write, open nonblockingly
// some sync on file. Change O_SYNC to O_DSYNC?
if (tty_fd < 0) {
......@@ -86,8 +122,7 @@ int SerialLight::startConnection(const char* port){
cfsetospeed(&tio, SERIAL_LIGHT_BAUD_RATE);
cfsetispeed(&tio, SERIAL_LIGHT_BAUD_RATE);
if (tcsetattr(tty_fd, TCSANOW, &tio) != 0) // "TCSANOW: change occurs immediately"
{
if (tcsetattr(tty_fd, TCSANOW, &tio) != 0){ // "TCSANOW: change occurs immediately"
printf("%sError from tcsetattr: %s\n", K_ERROR, strerror(errno));
return -1;
}
......@@ -108,14 +143,6 @@ void SerialLight::closeConnection(){
close(tty_fd); // Port release
}
/*
bool SerialLight::isConnected(const char* port){
if (port != ""){
return (_isConnected && _port == port);
}
return _isConnected;
}
*/
bool SerialLight::isConnected(){
return _isConnected;
}
......@@ -141,75 +168,15 @@ bool SerialLight::writeSerialLight(const char* buffer){
// echo 'A new log: ' > eavesDroppingLog.txt
// cat -v < /dev/ttyACM0 |& tee -a eavesDroppingLog.txt
tcflush(tty_fd, TCIFLUSH);
//int tcflush(int fd, int queue_selector);
//tcflush(ttyDevice, TCIOFLUSH); //TCIFLUSH, TCOFLUSH -> launch eavesdropper otherwise.
printf("Sending packet to Serial (%s)\n", buffer);
// zero day in code? (Just sending first 14 bytes -> pointer stuff? )
// zero day SerialUnix code? (Just sending first 14 bytes -> pointer stuff? )
//for (unsigned int i = 0; i < strlen(buffer); i++){
// write(tty_fd, &buffer[i], 1);
//}
//printf("Length of that thing: %i \n", strlen(buffer));
write(tty_fd, buffer, strlen(buffer));
//usleep(1000);
//ioctl(tty_fd, TCFLSH, 1); // flush transmit
// bool timeout = true;
// _ready = false;
// timer = std::clock();
// while (!_ready && timeout) {
// timeout = (std::clock() - timer) / ((double)CLOCKS_PER_SEC / 1000) < SERIAL_FLAG_TIMEOUT;
// manageSerial();
// }
//
// if (!timeout)
// {
// printf("%s---- BASE STATION ACK TIMEOUT ----\n", K_ERROR); //We got a timeout
// return 0;
// }
return 1;
}
// just some ack stuff-> unnecessary?
int SerialLight::manageSerial()
{
if(!_isConnected)
return 0;
int bytes = 0;
ioctl(tty_fd, FIONREAD, &bytes);
//go through all bytes (read them separately)
if (bytes) { //There are bytes on the buffer
char value;
for (unsigned int i = 0; i < bytes; i++)
{