1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
/**
* @file CameraSettingsV6Manager.h
*
*/

#ifndef CAMERASETTINGSV6MANAGER_H
#define CAMERASETTINGSV6MANAGER_H

#include "V4LCameraSettingsManager.h"
#include <Representations/Infrastructure/CameraSettings.h>
#include <bitset>

class CameraSettingsV6Manager : public V4LCameraSettingsManager 
{  
public:

  CameraSettingsV6Manager(int cameraFd, const std::string& cameraName);

  virtual void query(naoth::CameraSettings& settings);
  virtual void apply(const naoth::CameraSettings& settings, bool force = false);

private:
  //naoth::CameraSettings current;
  naoth::V6CameraSettings current;
  bool initialized;

  int cameraFd; 
  std::string cameraName;

  /* 
  NOTE: this structure is used to write and read registers in setRegister and getRegister.
  In the process is is converted to (unsigned char*) and read bytewise.  
  For this the exact array lenghts has to be 
     size(Register) == 5
  which only holds if the alignment 
  is set to 1 byte. This can be achieved with #pragma pack(1) or with GCC speciffic __attribute__((packed)).
  #pragma pack(1) is also avaliable in VS and seems more general.
  */
  #pragma pack(1)
  struct Register {
    uint8_t id;
    uint16_t addr; 
    uint16_t value;
  };//__attribute__((packed)); // very gcc specific
  #pragma pack()
  
  // make sure the alignment is correct
  static_assert(sizeof(Register) == 5, "Register has to have alignment 1, sich that size(Register) == 5.");
  
  
  void print_bytes(const uint8_t* data, size_t size) {
    for(size_t i = 0; i < size; ++i) {
      std::cout << std::bitset<8>(data[i]);
    }
    std::cout << std::endl;
  }
  
  inline uint16_t set( size_t bitNum, size_t n ) {
      return static_cast<uint16_t>(bitNum | (1 << n));
  }

  inline uint16_t reset( size_t bitNum, size_t n ) {
      return static_cast<uint16_t>(bitNum & (~(1 << n )) );
  }
  
  // convert between big-endian and little-endian
  uint16_t swapBytes(uint16_t v) {
    return static_cast<uint16_t>(((v & 0xff) << 8) | ((v & 0xff00) >> 8));
  }
  
  uint16_t getRegister(int cameraFd, uint16_t addr);
  uint32_t getRegister32(int cameraFd, uint16_t addr);
  bool setRegister32(int cameraFd, uint16_t addr, uint32_t value);
  bool setRegister(int cameraFd, uint16_t addr, uint16_t val);<--- Function 'setRegister' argument 3 names different: declaration 'val' definition 'value'.
};

#endif // CAMERASETTINGSV6MANAGER_H