addition of camera scripts

camera/raspberry/lights/src/LEDs.cpp

@@ -0,0 +1,16 @@
+#include "LEDs.hpp"
+
+LEDs::LEDs(){
+    wiringPiSetup();
+    pinMode(GPIO_LED_left, PWM_OUTPUT);
+    pinMode(GPIO_LED_right, PWM_OUTPUT);
+    pwmSetClock(4000);      // Clock prescaller for PWM generator
+}
+
+void LEDs::Left(float power){
+    pwmWrite(GPIO_LED_left, power * 1024);
+}
+
+void LEDs::Right(float power){
+    pwmWrite(GPIO_LED_right, power * 1024);
+}

camera/raspberry/lights/src/LEDs.hpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <wiringPi.h>
+
+using namespace std;
+
+/**
+ * @brief LEDs strips controlled by AL8861Y
+ *          Dimmable by PWM on channel PWM0 and PWM1
+ */
+class LEDs{
+    /**
+     * @brief WiringPi GPIO number of pin which controls left channel
+     */
+    int GPIO_LED_left = 23;
+
+    /**
+     * @brief WiringPi GPIO number of pin which controls left channel
+     */
+    int GPIO_LED_right = 26;
+
+public:
+    /**
+     * @brief   Construct a new LEDs object
+     *          Configure GPIo and sets up frequency of PWM
+     *          Closes possible frequency reachable by Rpi is around 7 kHz
+     */
+    LEDs();
+
+    /**
+     * @brief Set power for right LED strip
+     *        Right channel is connected to pin 33 on header, GPIO13 (wiring pin 26)
+     *
+     * @param power Power of LED strip in range from 0.0 to 1.0
+     */
+    void Right(float power = 0);
+
+    /**
+     * @brief Set power for left LED strip
+     *        Left channel is connected to pin 32 on header, GPIO12 (wiring pin 23)
+     *
+     * @param power Power of LED strip in range from 0.0 to 1.0
+     */
+    void Left(float power = 0);
+};

camera/raspberry/lights/src/adxl345.cpp

@@ -0,0 +1,104 @@
+#include "adxl345.hpp"
+
+ADXL345::ADXL345(bool channel):channel(channel){
+    // SPI Mode 3 - CPOL = 1, CPHA = 1
+    wiringPiSPISetupMode(channel, 500000, 3);
+    // Power-up
+    Write(registers::POWER_CTL, 0b00001000);
+    // Maximal data output rate
+    Write(registers::BW_RATE, 0b00001010);
+
+    // Check connection by reading constant value of chip ID
+    if(ID() != 0xe5){
+        cout << "Cannot establish connection to accelerometer" << endl;
+    }
+}
+
+uint8_t ADXL345::ID(){
+    return Read(registers::DEVID);
+}
+
+array<int, 3> ADXL345::Trim_offsets(){
+    // Zero offset
+    Write(registers::OFSX, 0);
+    Write(registers::OFSY, 0);
+    Write(registers::OFSZ, 0);
+
+    // Acquire current values
+    usleep(100*1000);
+    auto axis_values = Raw_values();
+
+    // Calculate trims, Only acceleration of 1g should be on Z axis
+    int X_trim = -(round(axis_values[0]/4.0));
+    int Y_trim = -(round(axis_values[1]/4.0));
+    int Z_trim = -(round((axis_values[2]-255)/4.0));
+
+    // Write new trim values to registers
+    Write(registers::OFSX, X_trim);
+    Write(registers::OFSY, Y_trim);
+    Write(registers::OFSZ, Z_trim);
+
+    // Wait for propagation
+    usleep(100*1000);
+
+    return {X_trim, Y_trim, Z_trim};
+}
+
+void ADXL345::Trim_offsets(array<int8_t, 3> axis_offsets){
+    Write(registers::OFSX, axis_offsets[0]);
+    Write(registers::OFSY, axis_offsets[1]);
+    Write(registers::OFSZ, axis_offsets[2]);
+
+    // Wait for propagation
+    usleep(100*1000);
+}
+
+uint8_t ADXL345::Read(registers address){
+    // 0x80 is read command
+    vector<uint8_t> transfer_data = {static_cast<uint8_t>(static_cast<uint8_t>(address) | 0x80), dummy_packet};
+    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
+    return transfer_data[1];
+}
+
+vector<uint8_t> ADXL345::Read_sequence(registers address, short length){
+    vector<uint8_t> transfer_data( length + 1, dummy_packet);
+    // Insert address to first position in data transfer, rest if filled with dummy bytes
+    transfer_data[0] = static_cast<uint8_t>(address) | 0xc0;
+    wiringPiSPIDataRW(channel, transfer_data.data(), length + 1);
+    // Remove first byte (address) from data
+    transfer_data.erase(transfer_data.begin());
+    return transfer_data;
+}
+
+void ADXL345::Write(registers address, uint8_t data){
+    vector<uint8_t> transfer_data = {static_cast<uint8_t>(address), data};
+    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
+}
+
+array<int16_t, 3> ADXL345::Raw_values(){
+    vector<uint8_t> register_values = Read_sequence(registers::DATAX0, 6);
+    array<int16_t, 3> axis_values;
+    // Lower register represents LSB of value
+    axis_values[0] = register_values[0] | (register_values[1] << 8);
+    axis_values[1] = register_values[2] | (register_values[3] << 8);
+    axis_values[2] = register_values[4] | (register_values[5] << 8);
+    return axis_values;
+}
+
+double ADXL345::Inclination(){
+    auto axis_values = Raw_values();
+    // Resulting value is angle of IC package to horizontal plane
+    double angle = atan(static_cast<double>(axis_values[1])/axis_values[2]) * (180 / numbers::pi);
+    // Calculate absolute tilt based on quadrants from sensor values
+    if((axis_values[1] < 0) and (axis_values[2] >= 0)){
+        angle *= -1;
+    } else if ((axis_values[1] < 0) and (axis_values[2] < 0)){
+        angle = 180 - angle;
+    } else if ((axis_values[1] >= 0) and (axis_values[2] < 0)){
+        angle = -angle + 180;
+    } else if ((axis_values[1] > 0) and (axis_values[2] >= 0)){
+        angle = 360 - angle;
+    }
+    // Calculate inverse angle to corespond with clockwise rotation
+    return - angle + 360;
+}

camera/raspberry/lights/src/adxl345.hpp

@@ -0,0 +1,124 @@
+#pragma once
+
+#include <iostream>
+#include <vector>
+#include <array>
+#include <cmath>
+#include <numbers>
+
+#include <unistd.h>
+
+#include <wiringPiSPI.h>
+
+using namespace std;
+
+/**
+ * @brief   Accelerometer AXDL345
+ *          Datasheet: https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL345.pdf
+ */
+class ADXL345{
+    /**
+     * @brief  Channel on which is accelerometer connected
+     */
+    bool channel;
+
+    /**
+     * @brief Some of ADXL345 regsisters
+     */
+    enum class registers: uint8_t{
+        DEVID       = 0x00,
+        OFSX        = 0x1e,
+        OFSY        = 0x1f,
+        OFSZ        = 0x20,
+        BW_RATE     = 0x2c,
+        POWER_CTL   = 0x2d,
+        DATA_FORMAT = 0x31,
+        DATAX0      = 0x32,
+        DATAX1      = 0x33,
+        DATAY0      = 0x34,
+        DATAY1      = 0x35,
+        DATAZ0      = 0x36,
+        DATAZ1      = 0x37
+    };
+
+    /**
+     * @brief   Dummy packet, is not interpreted as any command by ADXL
+                Is used as padding for SPI transfer
+     */
+    uint8_t dummy_packet = 0xaa;
+
+public:
+
+    /**
+     * @brief Construct a new ADXL345 object
+     *
+     * @param channel SPI channel of Rpi, has only two channels, 0 or 1
+     */
+    ADXL345(bool channel = 0);
+
+    /**
+     * @brief   Reads ID from accelerometer
+     *          Value is read only
+     *          Suitable for testing of connection
+     *
+     * @return uint8_t  Always should returns 0xe5 if communications is working
+     */
+    uint8_t ID();
+
+    /**
+     * @brief   Calculates roll angle of sensor based on value from X and Z values of acceleration
+     *
+     * @return double Roll angle of sensor (around X axis of IC), From 0 to 360°, clockwise rotation
+     */
+    double Inclination();
+
+    /**
+     * @brief   Trims offsets of accelerometer based on current values !!
+     *          During this calibration process accelerometer should be position in base position (0 degrees)
+
+     * @return array<int8_t, 3> Values which was used as trimming values for coresponding axis [X,Y,Z]
+     */
+    array<int, 3> Trim_offsets();
+
+    /**
+     * @brief   Trims offsets of accelerometer with defined values
+     *
+     * @param axis_offsets  Trim values for all accelerometer axis [X,Y,Z]
+     *                      Value is multiplied by 4 and added to resulting acceleration of respective axis
+     */
+    void Trim_offsets(array<int8_t, 3> axis_offsets);
+
+
+private:
+    /**
+     * @brief   Reads raw values of acceleration for all axis
+     *
+     * @return array<int16_t, 3>    Acceleration values from sensor for each axis [X,Y,Z]
+     */
+    array<int16_t, 3> Raw_values();
+
+    /**
+     * @brief   Read one byte of data from ADXL memory
+     *
+     * @param address   Address in ADXL memory from which are data read
+     * @return uint8_t  Data from memory
+     */
+    uint8_t Read(registers address);
+
+    /**
+     * @brief       Reads sequence of data from ADXL memory
+     *
+     * @param address   Address in ADXL memory from which reading starts
+     * @param length    Amount of bytes to read
+     * @return vector<uint8_t>  Readout data
+     */
+    vector<uint8_t> Read_sequence(registers address, short length);
+
+    /**
+     * @brief   Write one byte of data into ADXL memory
+     *
+     * @param address   Address to which will be writing performed
+     * @param data      Data to write
+     */
+    void Write(registers address, uint8_t data);
+};

camera/raspberry/lights/src/demo.cpp

@@ -0,0 +1,21 @@
+#include "demo.hpp"
+
+using namespace std;
+
+int main(){
+
+    // ----- Fan -----
+    auto fan = Fan();
+    fan.On();
+
+    // ----- LEDS -----
+    auto led = LEDs();
+    for(int i = 0; i < 100000; i++){
+        led.Left(1.0);
+        led.Right(1.0);
+        usleep(10000);
+        led.Left(0.0);
+        led.Right(0.0);
+    }
+    fan.Off();
+}

camera/raspberry/lights/src/demo.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <iostream>
+#include <iomanip>
+
+#include <unistd.h>
+
+#include "adxl345.hpp"
+#include "fan.hpp"
+#include "LEDs.hpp"
+#include "sick_side.hpp"
+#include "sick_front.hpp"

camera/raspberry/lights/src/fan.cpp

@@ -0,0 +1,14 @@
+#include "fan.hpp"
+
+Fan::Fan(){
+    wiringPiSetup();
+    pinMode(GPIO_fan, OUTPUT);
+}
+
+void Fan::On(){
+    digitalWrite(GPIO_fan, 1);
+}
+
+void Fan::Off(){
+    digitalWrite(GPIO_fan, 0);
+}

camera/raspberry/lights/src/fan.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <wiringPi.h>
+#include <softPwm.h>
+
+using namespace std;
+
+/**
+ * @brief PWM controlled fan without RPM detection
+ */
+class Fan{
+    /**
+     * @brief WiringPi GPIO number of pin which controls fan
+     */
+    int GPIO_fan = 25;
+
+public:
+    /**
+     * @brief   Construct a new Fan object
+     *          Create soft-PWM thread for fan control signal
+     */
+    Fan();
+
+    /**
+     * @brief Enable cooling fan
+     */
+    void On();
+
+    /**
+     * @brief Disable cooling fan
+     */
+    void Off();
+
+};

camera/raspberry/lights/src/sick_front.cpp

@@ -0,0 +1,61 @@
+#include "sick_front.hpp"
+
+Sick_front::Sick_front(bool channel):channel(channel){
+    wiringPiSPISetupMode(channel, 500000, 3);
+    wiringPiSetup();
+    pinMode(GPIO_conversion_start, OUTPUT);
+    pinMode(GPIO_laser_enable, OUTPUT);
+}
+
+optional<unsigned int> Sick_front::Distance_avg(unsigned int measurement_count){
+    unsigned int sum = 0;
+    unsigned int successful_measurements = 0;
+    for(unsigned int i = 0; i < measurement_count; i++){
+        auto distance = Distance();
+        if(distance){
+            sum += *distance;
+            successful_measurements++;
+        }
+    }
+    if(sum == 0){
+        return {};
+    }
+    return sum/successful_measurements;
+}
+
+optional<unsigned int> Sick_front::Distance(){
+    auto raw = Conversion_raw();
+    float differential_raw = static_cast<float>(static_cast<uint16_t>(raw));
+    // Calculation of true voltage from reference voltage and ADC value
+    double voltage = differential_raw / (1 << 16) * reference_voltage;
+    //cout << "Voltage: " << hex << raw << dec << " : " <<differential_raw << " : " << setprecision(4) << voltage << " V" << setprecision(0);
+    // Out of Range
+    if((voltage < voltage_close) | (voltage > voltage_far)){
+        return {};
+    }
+
+    double range_percentage = (voltage-voltage_min)/(voltage_max-voltage_min);
+    double distance = round((range_max-range_min)*range_percentage+range_min);
+    return distance;
+}
+
+uint16_t Sick_front::Conversion_raw(){
+    // Start conversion
+    digitalWrite(GPIO_conversion_start, 1);
+    array<uint8_t, 2> received_data = {0x00, 0x00};
+    wiringPiSPIDataRW(channel, received_data.data(), 2);
+    // Start conversion
+    digitalWrite(GPIO_conversion_start, 0);
+    return ((received_data[0]<<8) | received_data[1]);
+}
+
+void Sick_front::Laser(bool state){
+    if(state){
+        // ON
+        pinMode(GPIO_laser_enable, INPUT);
+    } else {
+        // OFF
+        pinMode(GPIO_laser_enable, OUTPUT);
+        digitalWrite(GPIO_laser_enable, 0);
+    }
+}

camera/raspberry/lights/src/sick_front.hpp

@@ -0,0 +1,134 @@
+#pragma once
+
+#include <iostream>
+#include <array>
+#include <string>
+#include <optional>
+#include <cmath>
+
+#include <iomanip>
+
+#include <wiringPi.h>
+#include <wiringPiSPI.h>
+
+using namespace std;
+
+/**
+Type: DT50-N2113
+**/
+class Sick_front{
+    /**
+     * @brief  Channel on which is ADC for front distance sensor connected
+     */
+    bool channel;
+
+    /**
+     * @brief WiringPi GPIO number of pin which start conversion by change 0->1
+     */
+    const int GPIO_conversion_start = 5;
+
+    /**
+     * @brief WiringPi GPIO number of pin which turn ON/OFF laser of sensor
+     */
+    const int GPIO_laser_enable = 4;
+
+        /**
+     * @brief   Minimal configured distance of sensor
+     */
+    const int range_min = 200;
+
+    /**
+     * @brief   Maximal configured distance of sensor
+     */
+    const int range_max = 10000;
+
+    /**
+     * @brief   Value of sense resistor in ohms used for current loop
+     */
+    const int sense_resistor = 150;
+
+    /**
+     * @brief   Current coresponding to minimal distance, configured in "range_min"
+     */
+    const float current_loop_min = 0.004;
+
+    /**
+     * @brief   Current coresponding to maximal distance, configured in "range_max"
+     */
+    const float current_loop_max = 0.020;
+
+    /**
+     * @brief   Current coresponding to state when minimal distance is exceeded
+     */
+    const float current_loop_close = 0.0035;
+
+    /**
+     * @brief   Current coresponding to state when maximal distance is exceeded
+     */
+    const float current_loop_far = 0.0205;
+
+    /**
+     * @brief   Voltage coresponding to maximal distance
+     */
+    const float voltage_max = current_loop_max*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to minimal distance
+     */
+    const float voltage_min = current_loop_min*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to state when maximal distance is exceeded
+     */
+    const float voltage_close = current_loop_close*sense_resistor;
+
+    /**
+     * @brief   Voltage coresponding to state when minimal distance is exceeded
+     */
+    const float voltage_far = current_loop_far*sense_resistor;
+
+    /**
+     * @brief  Reference voltage of ADC
+     */
+    const float reference_voltage = 3.308f;
+
+public:
+    /**
+     * @brief   Construct a new Sick_front object
+     *          Configure GPIO and SPI interface
+     *
+     * @param channel   Channel on which is sensor connected
+     */
+    Sick_front(bool channel = 1);
+
+    /**
+     * @brief Calculate distance measurted by distance sensor
+     *
+     * @return optional<unsigned int>   True and value in mm if in range, False otherwise
+     */
+    optional<unsigned int> Distance();
+
+    /**
+     * @brief Average multiple measurements of distance sensor output
+     *
+     * @param measurement_count         Amount of measurents to average
+     * @return optional<unsigned int>   Same as Distance()
+     */
+    optional<unsigned int> Distance_avg(unsigned int measurement_count = 8);
+
+    /**
+     * @brief           Turn ON or OFF laser of sensor
+     *
+     * @param state     New state of laser
+     */
+    void Laser(bool state);
+
+private:
+    /**
+     * @brief           Read raw ADC data from ADC
+     *
+     * @return uint16_t raw ADc data in 2's complement format
+     */
+    uint16_t Conversion_raw();
+
+};

camera/raspberry/lights/src/sick_side.cpp

@@ -0,0 +1,103 @@
+#include "sick_side.hpp"
+
+Sick_side::Sick_side(){
+    serial_descriptor = serialOpen("/dev/ttyS0", 9600);
+    if(serial_descriptor == -1){
+        cout << "Cannot open serial line for communication with side distance sensor" << endl;
+    }
+}
+
+uint8_t Sick_side::ID(){
+    Send_data({static_cast<uint8_t>(commands::Read), 0x01, 0x00});
+    //while(Data_available() < 12);
+    // Readout data which are in buffer from transmittion
+    //Receive();
+    // Read real received data from sensor
+    if(auto responce = Receive()){
+        return (*responce)[2];
+    } else {
+        return 0x00;
+    }
+}
+
+void Sick_side::Laser(bool state){
+    if(state){
+        // ON
+        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x03});
+    } else {
+        // OFF
+        Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x02});
+    }
+
+    //Read responce to clear buffer
+    Receive();
+}
+
+optional<double> Sick_side::Distance(){
+    auto distance_data = Distance_data();
+    if(distance_data == 0x7fff){
+        // Out of measuring range
+        return {};
+    } else {
+        // Convert to 2'complement and then to mm
+        return static_cast<int16_t>(distance_data)/100.0;
+    }
+}
+
+uint16_t Sick_side::Distance_data(){
+    Send_data({static_cast<uint8_t>(commands::Command), 0xb0, 0x01});
+    if(auto responce = Receive()){
+        return (((*responce)[1]<<8) | (*responce)[2]);
+    } else {
+        return 0x7fff;
+    }
+}
+
+void Sick_side::Send_data(array<uint8_t, 3> data){
+    array<uint8_t, 6> packet = {static_cast<uint8_t>(signs::STX), data[0], data[1], data[2], static_cast<uint8_t>(signs::ETX), 0x00};
+    packet[5] = Calculate_BCC(packet);
+    Transmit(packet);
+}
+
+void Sick_side::Transmit(array<uint8_t, 6> data){
+    for(int i = 0; i < 6; i++){
+        serialPutchar(serial_descriptor, data[i]);
+    }
+}
+
+uint8_t Sick_side::Calculate_BCC(array<uint8_t, 6> data){
+    uint8_t BCC = 0;
+    // BCC is calculated only from bytes on positions 1,2,3
+    for(int byte_index = 1; byte_index < 4; byte_index++){
+        BCC ^= data[byte_index];
+    }
+    return BCC;
+}
+
+optional<array<uint8_t, 3>> Sick_side::Receive(){
+    while(Data_available() < 12);
+    // Readout data which are in buffer from transmittion
+    for(int i = 0; i < 6; i++){
+        serialGetchar(serial_descriptor);
+    }
+    // Receive true data
+    array<uint8_t, 6> received_data;
+    for(int i = 0; i < 6; i++){
+        received_data[i] = serialGetchar(serial_descriptor);
+    }
+    // Check for communication errors
+    if(static_cast<signs>(received_data[1]) == signs::NAK){
+        cout << error_codes[received_data[2]] << endl;
+        return {};
+    }
+    if(received_data[5] != Calculate_BCC(received_data)){
+        cout << "Received BCC is invalid" << endl;
+        return {};
+    }
+
+    return array<uint8_t, 3>({received_data[1], received_data[2], received_data[3]});
+}
+
+unsigned int Sick_side::Data_available(){
+    return serialDataAvail(serial_descriptor);
+}

camera/raspberry/lights/src/sick_side.hpp

@@ -0,0 +1,119 @@
+#pragma once
+
+#include <iostream>
+#include <array>
+#include <optional>
+#include <map>
+#include <string>
+
+#include <wiringSerial.h>
+
+using namespace std;
+
+/**
+Type: OD1-B100C50A15
+**/
+class Sick_side{
+
+    int serial_descriptor = 0;
+
+    enum class signs: uint8_t{
+        STX       = 0x02,
+        ETX       = 0x03,
+        ACK       = 0x06,
+        NAK       = 0x15,
+    };
+
+    map<uint8_t, string> error_codes{
+        {0x02, "Address is invalid"},
+        {0x04, "BCC values is invalid"},
+        {0x05, "Invalid command"},
+        {0x06, "Invalid setting value (out of spec)"},
+        {0x07, "Invalid setting value (out of range)"}
+    };
+
+    enum class commands: uint8_t{
+        Command     = 0x43, // Also used for reading out measured values
+        Write       = 0x57,
+        Read        = 0x52,
+    };
+
+public:
+    /**
+     * @brief   Construct a new Sick_side object
+     *          Opens serial line and save file descriptor
+     */
+    Sick_side();
+
+    /**
+     * @brief           Read sensor ID from memory of sensor
+     *                  Used for connection check
+     *
+     * @return uint8_t  Value 0x64, 0x00 if communication fails
+     */
+    uint8_t ID();
+
+    /**
+     * @brief           Turn ON or OFF laser of sensor
+     *
+     * @param state     New state of laser
+     */
+    void Laser(bool state);
+
+    /**
+     * @brief   Read and calculate distance from distance sensor
+     *          Distance is relative to sensors zero point
+     *
+     * @return optional<double> And and relative distance from zero point, false otherwise (Out of Range)
+     */
+    optional<double> Distance();
+
+private:
+
+    /**
+     * @brief Reads distance data from sensor register
+     *
+     * @return uint16_t Distance in 2's complement, range -5000 to 5000, coresponds to -50 mm to +50 mm
+     */
+    uint16_t Distance_data();
+
+    /**
+     * @brief       Transmit given data over serial to RS485 transmitter
+     *
+     * @param data  Data to send via serial line, header and footer are added
+     */
+    void Send_data(array<uint8_t, 3> data);
+
+    /**
+     * @brief           Calculates BCC for given packet of data
+     *                  BCC is XOR of bytes on index 1,2,3, remaning bytes are ignored
+     *
+     * @param data      Packet used for calculation
+     * @return uint8_t  Values of BCC
+     */
+    uint8_t Calculate_BCC(array<uint8_t, 6> data);
+
+    /**
+     * @brief       Transmits data over UART to RS485 transmitter
+     *              Data are also copied to input buffer due to half-duplex connection
+     *
+     * @param data  Data to transmit
+     */
+    void Transmit(array<uint8_t, 6> data);
+
+    /**
+     * @brief       Data received from sensor
+     *              Only usefull data are returned, without STX, ACK/NACK, BCC
+     *
+     * @return optional<array<uint8_t, 3>>  True and value if communication is valid, False otherwise
+     */
+    optional<array<uint8_t, 3>> Receive();
+
+    /**
+     * @brief Reads amopunt of data in input buffer
+     *
+     * @return unsigned int Number of bytes in input buffer
+     */
+    unsigned int Data_available();
+
+};