source: flair-src/trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.cpp@ 4

Last change on this file since 4 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

File size: 3.2 KB
RevLine 
[3]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
51using std::string;
52using namespace flair::core;
53
54namespace flair {
55namespace sensor {
56
57ParrotBatteryMonitor::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
89ParrotBatteryMonitor::~ParrotBatteryMonitor() {
90 SafeStop();
91 Join();
92}
93
94void 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
Note: See TracBrowser for help on using the repository browser.