1 | // %flair:license{
|
---|
2 | // This file is part of the Flair framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %flair:license}
|
---|
5 | // created: 2014/01/24
|
---|
6 | // filename: ParrotBatteryMonitor.cpp
|
---|
7 | //
|
---|
8 | // author: Guillaume Sanahuja
|
---|
9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
10 | //
|
---|
11 | // version: $Id: $
|
---|
12 | //
|
---|
13 | // purpose: Parrot battery monitor
|
---|
14 | //
|
---|
15 | //
|
---|
16 | /*********************************************************************/
|
---|
17 |
|
---|
18 | #include "ParrotBatteryMonitor.h"
|
---|
19 | #include <Layout.h>
|
---|
20 | #include <LayoutPosition.h>
|
---|
21 | #include <Unix_I2cPort.h>
|
---|
22 |
|
---|
23 | #define INTERFACE_GROUP_ADDRESS 0x49
|
---|
24 | #define INTBR_GPBR1 0x91
|
---|
25 |
|
---|
26 | //INTBR_GPBR1 bits
|
---|
27 | #define DEFAULT_MADC_CLK_EN (1<<4) /* MADC clock enable */
|
---|
28 | #define MADC_HFCLK_EN (1<<7) /* HFCLK clock divider enable */
|
---|
29 |
|
---|
30 | #define MADC_GROUP_ADDRESS 0x4a
|
---|
31 | #define MADC_CTRL1 0x00
|
---|
32 | #define MADC_SW1SELECT_LSB 0x06
|
---|
33 | #define MADC_SW1AVERAGE_LSB 0x08
|
---|
34 | #define MADC_CTRL_SW1 0x12
|
---|
35 | #define MADC_GPCH0_LSB 0x37
|
---|
36 | #define MADC_GPCH0_MSB 0x38
|
---|
37 |
|
---|
38 | //MADC_CTRL1 bits
|
---|
39 | #define MADC_MADCON (1<<0) /* MADC power on */
|
---|
40 |
|
---|
41 | //MADC_SW1SELECT_LSB bits
|
---|
42 | #define MADC_ADCIN0 (1<<0)
|
---|
43 |
|
---|
44 | //MADC_SW1AVERAGE_LSB bits
|
---|
45 | #define MADC_AV_CH0 (1<<0)
|
---|
46 |
|
---|
47 | //MADC_CTRL_SW1 bits
|
---|
48 | #define MADC_EOC_SW1 (1<<1) /* end of convrsion SW1 */
|
---|
49 | #define MADC_SW1 (1<<5) /* start an all channel convrsion */
|
---|
50 |
|
---|
51 | using std::string;
|
---|
52 | using namespace flair::core;
|
---|
53 |
|
---|
54 | namespace flair {
|
---|
55 | namespace sensor {
|
---|
56 |
|
---|
57 | ParrotBatteryMonitor::ParrotBatteryMonitor(const gui::LayoutPosition* position,string name) : Thread(position->getLayout(),name,5),BatteryMonitor(position,name) {
|
---|
58 | uint8_t tx[2];
|
---|
59 |
|
---|
60 | port=new Unix_I2cPort((BatteryMonitor*)this,"i2c1","/dev/i2c-1");
|
---|
61 |
|
---|
62 | //interface group address
|
---|
63 | port->SetSlave(INTERFACE_GROUP_ADDRESS);
|
---|
64 |
|
---|
65 | //enable clocks
|
---|
66 | tx[0]=INTBR_GPBR1;
|
---|
67 | tx[1]=MADC_HFCLK_EN|DEFAULT_MADC_CLK_EN;
|
---|
68 | port->Write(tx,2);
|
---|
69 |
|
---|
70 | //MADC group address
|
---|
71 | port->SetSlave(MADC_GROUP_ADDRESS);
|
---|
72 |
|
---|
73 | // turn on MADC
|
---|
74 | tx[0]=MADC_CTRL1;
|
---|
75 | tx[1]=MADC_MADCON;
|
---|
76 | port->Write(tx,2);
|
---|
77 |
|
---|
78 | // select channel 0
|
---|
79 | tx[0]=MADC_SW1SELECT_LSB;
|
---|
80 | tx[1]=MADC_ADCIN0;
|
---|
81 | port->Write(tx,2);
|
---|
82 |
|
---|
83 | // setup averaging on channel 0
|
---|
84 | tx[0]=MADC_SW1AVERAGE_LSB;
|
---|
85 | tx[1]=MADC_AV_CH0;
|
---|
86 | port->Write(tx,2);
|
---|
87 | }
|
---|
88 |
|
---|
89 | ParrotBatteryMonitor::~ParrotBatteryMonitor() {
|
---|
90 | SafeStop();
|
---|
91 | Join();
|
---|
92 | }
|
---|
93 |
|
---|
94 | void ParrotBatteryMonitor::Run(void) {
|
---|
95 | unsigned char lsb, msb;
|
---|
96 | int adc_value;
|
---|
97 | uint8_t tx[2];
|
---|
98 | uint8_t rx;
|
---|
99 |
|
---|
100 | SetPeriodMS(1000);
|
---|
101 |
|
---|
102 | while(!ToBeStopped()) {
|
---|
103 | // start an all channel conversion
|
---|
104 | tx[0]=MADC_CTRL_SW1;
|
---|
105 | tx[1]=MADC_SW1;
|
---|
106 | port->Write(tx,2);
|
---|
107 |
|
---|
108 | WaitPeriod();
|
---|
109 |
|
---|
110 | //check end of conversion in CTRL_SW1
|
---|
111 | //(conversion should be ended as period is 1s)
|
---|
112 | port->Write(tx,1);
|
---|
113 | port->Read(&rx,1);
|
---|
114 | if((rx&MADC_EOC_SW1)==MADC_EOC_SW1) {
|
---|
115 | //read value
|
---|
116 | tx[0]=MADC_GPCH0_LSB;
|
---|
117 | port->Write(tx,1);
|
---|
118 | port->Read(&lsb,1);
|
---|
119 |
|
---|
120 | tx[0]=MADC_GPCH0_MSB;
|
---|
121 | port->Write(tx,1);
|
---|
122 | port->Read(&msb,1);
|
---|
123 |
|
---|
124 | adc_value = (lsb >> 6) | (msb << 2);
|
---|
125 |
|
---|
126 | //convert to volts
|
---|
127 | //thanks to Paparazzi (https://wiki.paparazziuav.org) for the coefficient
|
---|
128 | SetBatteryValue(adc_value*0.013595166);
|
---|
129 | }
|
---|
130 | }
|
---|
131 | }
|
---|
132 |
|
---|
133 | } // end namespace sensor
|
---|
134 | } // end namespace framewor
|
---|