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

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

sensoractuator

File size: 3.2 KB
Line 
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.