Used for testing battery sense circuit, looking for max and min levels. Allow for finding true empty, half and full values for driving LEDs for example
Dependencies: mbed MPL3115A2 TSI WiGo_BattCharger
main.cpp@3:3b88d6ae24f2, 2013-05-23 (annotated)
- Committer:
- monpjc
- Date:
- Thu May 23 11:42:10 2013 +0000
- Revision:
- 3:3b88d6ae24f2
- Parent:
- 2:c08efa9effc8
- Child:
- 5:4438d5665b4f
Added Alt code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
monpjc | 0:17ad5a30ff25 | 1 | #include "mbed.h" |
monpjc | 1:b1921e153d21 | 2 | #include "WiGo_BattCharger.h" |
monpjc | 2:c08efa9effc8 | 3 | #include "WiGo_AmbLight.h" |
monpjc | 3:3b88d6ae24f2 | 4 | #include "TSISensor.h" |
monpjc | 3:3b88d6ae24f2 | 5 | #include "math.h" |
monpjc | 3:3b88d6ae24f2 | 6 | #include "MPL3115A2.h" |
monpjc | 3:3b88d6ae24f2 | 7 | |
monpjc | 3:3b88d6ae24f2 | 8 | #define MPL3115A2_I2C_ADDRESS (0x60<<1) |
monpjc | 3:3b88d6ae24f2 | 9 | MPL3115A2 wigo_sensor1(PTE0, PTE1, MPL3115A2_I2C_ADDRESS); |
monpjc | 3:3b88d6ae24f2 | 10 | |
monpjc | 3:3b88d6ae24f2 | 11 | #define MAG_ADDR 0x1D |
monpjc | 3:3b88d6ae24f2 | 12 | |
monpjc | 3:3b88d6ae24f2 | 13 | // define registers |
monpjc | 3:3b88d6ae24f2 | 14 | #define MAG_DR_STATUS 0x00 |
monpjc | 3:3b88d6ae24f2 | 15 | #define MAG_OUT_X_MSB 0x01 |
monpjc | 3:3b88d6ae24f2 | 16 | #define MAG_OUT_X_LSB 0x02 |
monpjc | 3:3b88d6ae24f2 | 17 | #define MAG_OUT_Y_MSB 0x03 |
monpjc | 3:3b88d6ae24f2 | 18 | #define MAG_OUT_Y_LSB 0x04 |
monpjc | 3:3b88d6ae24f2 | 19 | #define MAG_OUT_Z_MSB 0x05 |
monpjc | 3:3b88d6ae24f2 | 20 | #define MAG_OUT_Z_LSB 0x06 |
monpjc | 3:3b88d6ae24f2 | 21 | #define MAG_WHO_AM_I 0x07 |
monpjc | 3:3b88d6ae24f2 | 22 | #define MAG_SYSMOD 0x08 |
monpjc | 3:3b88d6ae24f2 | 23 | #define MAG_OFF_X_MSB 0x09 |
monpjc | 3:3b88d6ae24f2 | 24 | #define MAG_OFF_X_LSB 0x0A |
monpjc | 3:3b88d6ae24f2 | 25 | #define MAG_OFF_Y_MSB 0x0B |
monpjc | 3:3b88d6ae24f2 | 26 | #define MAG_OFF_Y_LSB 0x0C |
monpjc | 3:3b88d6ae24f2 | 27 | #define MAG_OFF_Z_MSB 0x0D |
monpjc | 3:3b88d6ae24f2 | 28 | #define MAG_OFF_Z_LSB 0x0E |
monpjc | 3:3b88d6ae24f2 | 29 | #define MAG_DIE_TEMP 0x0F |
monpjc | 3:3b88d6ae24f2 | 30 | #define MAG_CTRL_REG1 0x10 |
monpjc | 3:3b88d6ae24f2 | 31 | #define MAG_CTRL_REG2 0x11 |
monpjc | 3:3b88d6ae24f2 | 32 | |
monpjc | 3:3b88d6ae24f2 | 33 | // what should WHO_AM_I return? |
monpjc | 3:3b88d6ae24f2 | 34 | #define MAG_3110_WHO_AM_I_VALUE 0xC4 |
monpjc | 3:3b88d6ae24f2 | 35 | |
monpjc | 3:3b88d6ae24f2 | 36 | |
monpjc | 3:3b88d6ae24f2 | 37 | // Fields in registers |
monpjc | 3:3b88d6ae24f2 | 38 | // CTRL_REG1: dr2,dr1,dr0 os1,os0 fr tm ac |
monpjc | 3:3b88d6ae24f2 | 39 | |
monpjc | 3:3b88d6ae24f2 | 40 | // Sampling rate from 80Hz down to 0.625Hz |
monpjc | 3:3b88d6ae24f2 | 41 | #define MAG_3110_SAMPLE80 0 |
monpjc | 3:3b88d6ae24f2 | 42 | #define MAG_3110_SAMPLE40 0x20 |
monpjc | 3:3b88d6ae24f2 | 43 | #define MAG_3110_SAMPLE20 0x40 |
monpjc | 3:3b88d6ae24f2 | 44 | #define MAG_3110_SAMPLE10 0x60 |
monpjc | 3:3b88d6ae24f2 | 45 | #define MAG_3110_SAMPLE5 0x80 |
monpjc | 3:3b88d6ae24f2 | 46 | #define MAG_3110_SAMPLE2_5 0xA0 |
monpjc | 3:3b88d6ae24f2 | 47 | #define MAG_3110_SAMPLE1_25 0xC0 |
monpjc | 3:3b88d6ae24f2 | 48 | #define MAG_3110_SAMPLE0_625 0xE0 |
monpjc | 3:3b88d6ae24f2 | 49 | |
monpjc | 3:3b88d6ae24f2 | 50 | // How many samples to average (lowers data rate) |
monpjc | 3:3b88d6ae24f2 | 51 | #define MAG_3110_OVERSAMPLE1 0 |
monpjc | 3:3b88d6ae24f2 | 52 | #define MAG_3110_OVERSAMPLE2 0x08 |
monpjc | 3:3b88d6ae24f2 | 53 | #define MAG_3110_OVERSAMPLE3 0x10 |
monpjc | 3:3b88d6ae24f2 | 54 | #define MAG_3110_OVERSAMPLE4 0x18 |
monpjc | 3:3b88d6ae24f2 | 55 | |
monpjc | 3:3b88d6ae24f2 | 56 | // read only 1 byte per axis |
monpjc | 3:3b88d6ae24f2 | 57 | #define MAG_3110_FASTREAD 0x04 |
monpjc | 3:3b88d6ae24f2 | 58 | // do one measurement (even if in standby mode) |
monpjc | 3:3b88d6ae24f2 | 59 | #define MAG_3110_TRIGGER 0x02 |
monpjc | 3:3b88d6ae24f2 | 60 | // put in active mode |
monpjc | 3:3b88d6ae24f2 | 61 | #define MAG_3110_ACTIVE 0x01 |
monpjc | 3:3b88d6ae24f2 | 62 | |
monpjc | 3:3b88d6ae24f2 | 63 | // CTRL_REG2: AUTO_MRST_EN _ RAW MAG_RST _ _ _ _ _ |
monpjc | 3:3b88d6ae24f2 | 64 | // reset sensor after each reading |
monpjc | 3:3b88d6ae24f2 | 65 | #define MAG_3110_AUTO_MRST_EN 0x80 |
monpjc | 3:3b88d6ae24f2 | 66 | // don't subtract user offsets |
monpjc | 3:3b88d6ae24f2 | 67 | #define MAG_3110_RAW 0x20 |
monpjc | 3:3b88d6ae24f2 | 68 | // reset magnetic sensor after too-large field |
monpjc | 3:3b88d6ae24f2 | 69 | #define MAG_3110_MAG_RST 0x10 |
monpjc | 3:3b88d6ae24f2 | 70 | |
monpjc | 3:3b88d6ae24f2 | 71 | // DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR |
monpjc | 3:3b88d6ae24f2 | 72 | #define MAG_3110_ZYXDR 0x08 |
monpjc | 3:3b88d6ae24f2 | 73 | |
monpjc | 3:3b88d6ae24f2 | 74 | |
monpjc | 3:3b88d6ae24f2 | 75 | #define PI 3.14159265359 |
monpjc | 3:3b88d6ae24f2 | 76 | #define ON 0 |
monpjc | 3:3b88d6ae24f2 | 77 | #define OFF 1 |
monpjc | 0:17ad5a30ff25 | 78 | |
monpjc | 0:17ad5a30ff25 | 79 | #define RGB_LED_ON 0 |
monpjc | 0:17ad5a30ff25 | 80 | #define RGB_LED_OFF 1 |
monpjc | 0:17ad5a30ff25 | 81 | |
monpjc | 3:3b88d6ae24f2 | 82 | // Some LEDs for showing status |
monpjc | 3:3b88d6ae24f2 | 83 | DigitalOut redLed(LED_RED); |
monpjc | 3:3b88d6ae24f2 | 84 | DigitalOut greenLed(LED_GREEN); |
monpjc | 3:3b88d6ae24f2 | 85 | DigitalOut blueLed(LED_BLUE); |
monpjc | 3:3b88d6ae24f2 | 86 | |
monpjc | 3:3b88d6ae24f2 | 87 | // Slide sensor acts as a button |
monpjc | 3:3b88d6ae24f2 | 88 | TSISensor tsi; |
monpjc | 3:3b88d6ae24f2 | 89 | |
monpjc | 3:3b88d6ae24f2 | 90 | // I2C used to communicate with sensor |
monpjc | 3:3b88d6ae24f2 | 91 | I2C i2c(PTE0, PTE1); |
monpjc | 3:3b88d6ae24f2 | 92 | |
monpjc | 3:3b88d6ae24f2 | 93 | int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax; |
monpjc | 3:3b88d6ae24f2 | 94 | // Ideally these would be saved in eeprom/flash |
monpjc | 3:3b88d6ae24f2 | 95 | struct settings_t { |
monpjc | 3:3b88d6ae24f2 | 96 | long maxX, minX, maxY, minY; |
monpjc | 3:3b88d6ae24f2 | 97 | } |
monpjc | 3:3b88d6ae24f2 | 98 | settings; |
monpjc | 3:3b88d6ae24f2 | 99 | |
monpjc | 3:3b88d6ae24f2 | 100 | const int addr = MAG_ADDR; |
monpjc | 0:17ad5a30ff25 | 101 | |
monpjc | 0:17ad5a30ff25 | 102 | Serial pc(USBTX, USBRX); |
monpjc | 0:17ad5a30ff25 | 103 | |
monpjc | 1:b1921e153d21 | 104 | WiGo_BattCharger Batt( BATT_LOW, BATT_MED, BATT_FULL, CHRG_EN1, CHRG_EN2, CHRG_SNS_EN, CHRG_SNS, CHRG_POK, CHRG_CHG); |
monpjc | 2:c08efa9effc8 | 105 | WiGo_AmbLight Light( AMBLIGHT_EN, AMBLIGHT_LVL); |
monpjc | 0:17ad5a30ff25 | 106 | |
monpjc | 3:3b88d6ae24f2 | 107 | // Read a single byte form 8 bit register, return as int |
monpjc | 3:3b88d6ae24f2 | 108 | int readReg(char regAddr) |
monpjc | 3:3b88d6ae24f2 | 109 | { |
monpjc | 3:3b88d6ae24f2 | 110 | char cmd[1]; |
monpjc | 3:3b88d6ae24f2 | 111 | |
monpjc | 3:3b88d6ae24f2 | 112 | cmd[0] = regAddr; |
monpjc | 3:3b88d6ae24f2 | 113 | i2c.write(addr, cmd, 1); |
monpjc | 3:3b88d6ae24f2 | 114 | |
monpjc | 3:3b88d6ae24f2 | 115 | cmd[0] = 0x00; |
monpjc | 3:3b88d6ae24f2 | 116 | i2c.read(addr, cmd, 1); |
monpjc | 3:3b88d6ae24f2 | 117 | return (int)( cmd[0]); |
monpjc | 3:3b88d6ae24f2 | 118 | } |
monpjc | 3:3b88d6ae24f2 | 119 | |
monpjc | 3:3b88d6ae24f2 | 120 | |
monpjc | 3:3b88d6ae24f2 | 121 | // read a register per, pass first reg value, reading 2 bytes increments register |
monpjc | 3:3b88d6ae24f2 | 122 | // Reads MSB first then LSB |
monpjc | 3:3b88d6ae24f2 | 123 | int readVal(char regAddr) |
monpjc | 3:3b88d6ae24f2 | 124 | { |
monpjc | 3:3b88d6ae24f2 | 125 | char cmd[2]; |
monpjc | 3:3b88d6ae24f2 | 126 | |
monpjc | 3:3b88d6ae24f2 | 127 | cmd[0] = regAddr; |
monpjc | 3:3b88d6ae24f2 | 128 | i2c.write(addr, cmd, 1); |
monpjc | 3:3b88d6ae24f2 | 129 | |
monpjc | 3:3b88d6ae24f2 | 130 | cmd[0] = 0x00; |
monpjc | 3:3b88d6ae24f2 | 131 | cmd[1] = 0x00; |
monpjc | 3:3b88d6ae24f2 | 132 | i2c.read(addr, cmd, 2); |
monpjc | 3:3b88d6ae24f2 | 133 | return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB |
monpjc | 3:3b88d6ae24f2 | 134 | } |
monpjc | 3:3b88d6ae24f2 | 135 | |
monpjc | 3:3b88d6ae24f2 | 136 | |
monpjc | 3:3b88d6ae24f2 | 137 | void initMag() { |
monpjc | 3:3b88d6ae24f2 | 138 | char cmd[2]; |
monpjc | 3:3b88d6ae24f2 | 139 | |
monpjc | 3:3b88d6ae24f2 | 140 | cmd[0] = MAG_CTRL_REG2; |
monpjc | 3:3b88d6ae24f2 | 141 | cmd[1] = 0x80; |
monpjc | 3:3b88d6ae24f2 | 142 | i2c.write(addr, cmd, 2); |
monpjc | 3:3b88d6ae24f2 | 143 | |
monpjc | 3:3b88d6ae24f2 | 144 | cmd[0] = MAG_CTRL_REG1; |
monpjc | 3:3b88d6ae24f2 | 145 | cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91; |
monpjc | 3:3b88d6ae24f2 | 146 | i2c.write(addr, cmd, 2); |
monpjc | 3:3b88d6ae24f2 | 147 | } |
monpjc | 3:3b88d6ae24f2 | 148 | |
monpjc | 3:3b88d6ae24f2 | 149 | |
monpjc | 3:3b88d6ae24f2 | 150 | void calXY() //magnetometer calibration: finding max and min of X, Y axis |
monpjc | 3:3b88d6ae24f2 | 151 | { |
monpjc | 3:3b88d6ae24f2 | 152 | int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY; |
monpjc | 3:3b88d6ae24f2 | 153 | redLed = ON; |
monpjc | 3:3b88d6ae24f2 | 154 | |
monpjc | 3:3b88d6ae24f2 | 155 | printf("Waiting for initial press\n"); |
monpjc | 3:3b88d6ae24f2 | 156 | // Wait for slider to be pressed |
monpjc | 3:3b88d6ae24f2 | 157 | while( tsi.readDistance() == 0 ) { |
monpjc | 3:3b88d6ae24f2 | 158 | redLed = ON; |
monpjc | 3:3b88d6ae24f2 | 159 | wait(0.2); |
monpjc | 3:3b88d6ae24f2 | 160 | redLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 161 | wait(0.2); |
monpjc | 3:3b88d6ae24f2 | 162 | } |
monpjc | 3:3b88d6ae24f2 | 163 | |
monpjc | 3:3b88d6ae24f2 | 164 | printf("Waiting for release\n"); |
monpjc | 3:3b88d6ae24f2 | 165 | |
monpjc | 3:3b88d6ae24f2 | 166 | // Wait for release |
monpjc | 3:3b88d6ae24f2 | 167 | while( tsi.readDistance() != 0 ) { |
monpjc | 3:3b88d6ae24f2 | 168 | redLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 169 | wait(0.2); |
monpjc | 3:3b88d6ae24f2 | 170 | redLed = ON; |
monpjc | 3:3b88d6ae24f2 | 171 | wait(0.2); |
monpjc | 3:3b88d6ae24f2 | 172 | } |
monpjc | 3:3b88d6ae24f2 | 173 | redLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 174 | wait(0.5); |
monpjc | 3:3b88d6ae24f2 | 175 | |
monpjc | 3:3b88d6ae24f2 | 176 | printf("Rotate\n"); |
monpjc | 3:3b88d6ae24f2 | 177 | |
monpjc | 3:3b88d6ae24f2 | 178 | tempXmax = tempXmin = readVal(MAG_OUT_X_MSB); |
monpjc | 3:3b88d6ae24f2 | 179 | tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB); |
monpjc | 3:3b88d6ae24f2 | 180 | |
monpjc | 3:3b88d6ae24f2 | 181 | while(tsi.readDistance() == 0) { |
monpjc | 3:3b88d6ae24f2 | 182 | greenLed = ON; |
monpjc | 3:3b88d6ae24f2 | 183 | wait(0.1); |
monpjc | 3:3b88d6ae24f2 | 184 | greenLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 185 | wait(0.1); |
monpjc | 3:3b88d6ae24f2 | 186 | newX = readVal(MAG_OUT_X_MSB); |
monpjc | 3:3b88d6ae24f2 | 187 | newY = readVal(MAG_OUT_Y_MSB); |
monpjc | 3:3b88d6ae24f2 | 188 | if (newX > tempXmax) tempXmax = newX; |
monpjc | 3:3b88d6ae24f2 | 189 | if (newX < tempXmin) tempXmin = newX; |
monpjc | 3:3b88d6ae24f2 | 190 | if (newY > tempYmax) tempYmax = newY; |
monpjc | 3:3b88d6ae24f2 | 191 | if (newY < tempYmin) tempYmin = newY; |
monpjc | 3:3b88d6ae24f2 | 192 | } |
monpjc | 3:3b88d6ae24f2 | 193 | |
monpjc | 3:3b88d6ae24f2 | 194 | settings.maxX = tempXmax; |
monpjc | 3:3b88d6ae24f2 | 195 | settings.minX = tempXmin; |
monpjc | 3:3b88d6ae24f2 | 196 | settings.maxY = tempYmax; |
monpjc | 3:3b88d6ae24f2 | 197 | settings.minY = tempYmin; |
monpjc | 3:3b88d6ae24f2 | 198 | |
monpjc | 3:3b88d6ae24f2 | 199 | //store new X, Y values in EEPROM/Flash |
monpjc | 3:3b88d6ae24f2 | 200 | |
monpjc | 3:3b88d6ae24f2 | 201 | // Calculate average from min/max |
monpjc | 3:3b88d6ae24f2 | 202 | avgX=(settings.maxX+settings.minX)/2; |
monpjc | 3:3b88d6ae24f2 | 203 | avgY=(settings.maxY+settings.minY)/2; |
monpjc | 3:3b88d6ae24f2 | 204 | |
monpjc | 3:3b88d6ae24f2 | 205 | // Wait for release |
monpjc | 3:3b88d6ae24f2 | 206 | while( tsi.readDistance() != 0 ) { |
monpjc | 3:3b88d6ae24f2 | 207 | greenLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 208 | wait(0.2); |
monpjc | 3:3b88d6ae24f2 | 209 | greenLed = ON; |
monpjc | 3:3b88d6ae24f2 | 210 | wait(0.2); |
monpjc | 3:3b88d6ae24f2 | 211 | } |
monpjc | 3:3b88d6ae24f2 | 212 | greenLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 213 | wait(1.0); |
monpjc | 3:3b88d6ae24f2 | 214 | |
monpjc | 3:3b88d6ae24f2 | 215 | |
monpjc | 3:3b88d6ae24f2 | 216 | } |
monpjc | 3:3b88d6ae24f2 | 217 | |
monpjc | 0:17ad5a30ff25 | 218 | float max_batt; |
monpjc | 0:17ad5a30ff25 | 219 | float batt_lvl; |
monpjc | 3:3b88d6ae24f2 | 220 | float sensor_data[3]; |
monpjc | 0:17ad5a30ff25 | 221 | |
monpjc | 0:17ad5a30ff25 | 222 | int main() |
monpjc | 0:17ad5a30ff25 | 223 | { |
monpjc | 3:3b88d6ae24f2 | 224 | |
monpjc | 2:c08efa9effc8 | 225 | Batt.init(CHRG_500MA); |
monpjc | 1:b1921e153d21 | 226 | Batt.sense_en(1); |
monpjc | 2:c08efa9effc8 | 227 | Light.en(1); |
monpjc | 0:17ad5a30ff25 | 228 | wait(0.5); |
monpjc | 1:b1921e153d21 | 229 | max_batt = Batt.read(); |
monpjc | 3:3b88d6ae24f2 | 230 | wigo_sensor1.Oversample_Ratio( OVERSAMPLE_RATIO_32); |
monpjc | 1:b1921e153d21 | 231 | |
monpjc | 3:3b88d6ae24f2 | 232 | printf("MAG3110 Test\n"); |
monpjc | 3:3b88d6ae24f2 | 233 | |
monpjc | 3:3b88d6ae24f2 | 234 | redLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 235 | greenLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 236 | blueLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 237 | |
monpjc | 3:3b88d6ae24f2 | 238 | initMag(); |
monpjc | 3:3b88d6ae24f2 | 239 | |
monpjc | 3:3b88d6ae24f2 | 240 | // Get some values |
monpjc | 3:3b88d6ae24f2 | 241 | printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS )); |
monpjc | 3:3b88d6ae24f2 | 242 | printf("WHO_AM_I %X\n", readReg( MAG_WHO_AM_I )); |
monpjc | 3:3b88d6ae24f2 | 243 | printf("SYSMOD %X\n", readReg( MAG_SYSMOD )); |
monpjc | 3:3b88d6ae24f2 | 244 | printf("DIE_TEMP %d\n", readReg( MAG_DIE_TEMP )); |
monpjc | 3:3b88d6ae24f2 | 245 | |
monpjc | 3:3b88d6ae24f2 | 246 | printf("OFF_X %d\n", readVal( MAG_OFF_X_MSB )); |
monpjc | 3:3b88d6ae24f2 | 247 | printf("OFF_Y %d\n", readVal( MAG_OFF_Y_MSB )); |
monpjc | 3:3b88d6ae24f2 | 248 | printf("OFF_Z %d\n", readVal( MAG_OFF_Z_MSB )); |
monpjc | 3:3b88d6ae24f2 | 249 | |
monpjc | 3:3b88d6ae24f2 | 250 | printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 )); |
monpjc | 3:3b88d6ae24f2 | 251 | printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 )); |
monpjc | 3:3b88d6ae24f2 | 252 | |
monpjc | 3:3b88d6ae24f2 | 253 | printf("calibrate\n"); |
monpjc | 3:3b88d6ae24f2 | 254 | calXY(); |
monpjc | 3:3b88d6ae24f2 | 255 | printf("....Finished\n"); |
monpjc | 3:3b88d6ae24f2 | 256 | printf("avgX = %d, avgY = %d\n", avgX, avgY); |
monpjc | 3:3b88d6ae24f2 | 257 | |
monpjc | 3:3b88d6ae24f2 | 258 | redLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 259 | greenLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 260 | blueLed = OFF; |
monpjc | 0:17ad5a30ff25 | 261 | |
monpjc | 0:17ad5a30ff25 | 262 | while(1) { |
monpjc | 0:17ad5a30ff25 | 263 | |
monpjc | 3:3b88d6ae24f2 | 264 | wait(2); |
monpjc | 3:3b88d6ae24f2 | 265 | int xVal = readVal(MAG_OUT_X_MSB); |
monpjc | 3:3b88d6ae24f2 | 266 | int yVal = readVal(MAG_OUT_Y_MSB); |
monpjc | 3:3b88d6ae24f2 | 267 | float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI; |
monpjc | 3:3b88d6ae24f2 | 268 | |
monpjc | 3:3b88d6ae24f2 | 269 | // Do something with heading - display direction and turn on blue LED if heading approx north |
monpjc | 3:3b88d6ae24f2 | 270 | if (abs(heading) <= 22.5) { printf("N\n"); blueLed = ON; } else blueLed = OFF; |
monpjc | 3:3b88d6ae24f2 | 271 | if (abs(heading) >= 157.5) printf("S\n"); |
monpjc | 3:3b88d6ae24f2 | 272 | if (heading >= 67.5 && heading <= 112.5) printf("E \n"); |
monpjc | 3:3b88d6ae24f2 | 273 | if (heading <= -67.5 && heading >= -112.5) printf("W \n"); |
monpjc | 3:3b88d6ae24f2 | 274 | if (heading > 22.5 && heading < 67.5) printf("NE\n"); |
monpjc | 3:3b88d6ae24f2 | 275 | if (heading < -22.5 && heading > -67.5) printf("NW\n"); |
monpjc | 3:3b88d6ae24f2 | 276 | if (heading > 112.5 && heading < 157.5) printf("SE\n"); |
monpjc | 3:3b88d6ae24f2 | 277 | if (heading < -112.5 && heading > -157.5) printf("SW\n"); |
monpjc | 0:17ad5a30ff25 | 278 | |
monpjc | 3:3b88d6ae24f2 | 279 | if (heading < 0) heading += 360.0; |
monpjc | 3:3b88d6ae24f2 | 280 | printf("xVal - avgX = %d, yVal - avgY = %d ", xVal-avgX, yVal-avgY); |
monpjc | 3:3b88d6ae24f2 | 281 | printf("X = %d, Y = %d, Heading %f\n", xVal, yVal, heading); |
monpjc | 0:17ad5a30ff25 | 282 | |
monpjc | 2:c08efa9effc8 | 283 | Batt.LEDupdate(1); |
monpjc | 0:17ad5a30ff25 | 284 | |
monpjc | 1:b1921e153d21 | 285 | batt_lvl = Batt.read(); |
monpjc | 0:17ad5a30ff25 | 286 | if( batt_lvl > max_batt ) { |
monpjc | 0:17ad5a30ff25 | 287 | max_batt = batt_lvl; |
monpjc | 0:17ad5a30ff25 | 288 | } |
monpjc | 0:17ad5a30ff25 | 289 | |
monpjc | 3:3b88d6ae24f2 | 290 | pc.printf(">%f Max:%f %i %i\n", batt_lvl, max_batt, Batt.level(), Light.level()); |
monpjc | 3:3b88d6ae24f2 | 291 | |
monpjc | 3:3b88d6ae24f2 | 292 | if ( wigo_sensor1.isDataAvailable()) { |
monpjc | 3:3b88d6ae24f2 | 293 | wigo_sensor1.getAllData( &sensor_data[0]); |
monpjc | 3:3b88d6ae24f2 | 294 | pc.printf("\Altitude: %f\tTemperature: %f\r\n", sensor_data[0], sensor_data[1]); |
monpjc | 3:3b88d6ae24f2 | 295 | } |
monpjc | 0:17ad5a30ff25 | 296 | } |
monpjc | 0:17ad5a30ff25 | 297 | } |